Coverage for src/meshpy/core/rotation.py: 95%
191 statements
« prev ^ index » next coverage.py v7.8.0, created at 2025-04-28 04:21 +0000
1# The MIT License (MIT)
2#
3# Copyright (c) 2018-2025 MeshPy Authors
4#
5# Permission is hereby granted, free of charge, to any person obtaining a copy
6# of this software and associated documentation files (the "Software"), to deal
7# in the Software without restriction, including without limitation the rights
8# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9# copies of the Software, and to permit persons to whom the Software is
10# furnished to do so, subject to the following conditions:
11#
12# The above copyright notice and this permission notice shall be included in
13# all copies or substantial portions of the Software.
14#
15# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
21# THE SOFTWARE.
22"""This module defines a class that represents a rotation in 3D."""
24import copy as _copy
26import numpy as _np
28from meshpy.core.conf import mpy as _mpy
31def skew_matrix(vector):
32 """Return the skew matrix for the vector."""
33 skew = _np.zeros([3, 3])
34 skew[0, 1] = -vector[2]
35 skew[0, 2] = vector[1]
36 skew[1, 0] = vector[2]
37 skew[1, 2] = -vector[0]
38 skew[2, 0] = -vector[1]
39 skew[2, 1] = vector[0]
40 return skew
43class Rotation:
44 """A class that represents a rotation of a coordinate system.
46 Internally the rotations are stored as quaternions.
47 """
49 def __init__(self, *args):
50 """Initialize the rotation object.
52 Args
53 ----
54 *args:
55 - Rotation()
56 Create a identity rotation.
57 - Rotation(axis, phi)
58 Create a rotation around the vector axis with the angle phi.
59 """
61 self.q = _np.zeros(4)
63 if len(args) == 0:
64 # Identity element.
65 self.q[0] = 1
66 elif len(args) == 2:
67 # Set from rotation axis and rotation angle.
68 axis = _np.asarray(args[0])
69 phi = args[1]
70 norm = _np.linalg.norm(axis)
71 if norm < _mpy.eps_quaternion:
72 raise ValueError("The rotation axis can not be a zero vector!")
73 self.q[0] = _np.cos(0.5 * phi)
74 self.q[1:] = _np.sin(0.5 * phi) * axis / norm
75 else:
76 raise ValueError(f"The given arguments {args} are invalid!")
78 @classmethod
79 def from_quaternion(cls, q, *, normalized=False):
80 """Create the object from a quaternion float array (4x1)
82 Args
83 ----
84 q: Quaternion, q0, qx,qy,qz
85 normalized: Flag if the input quaternion is normalized. If so, no
86 normalization is performed which can potentially improve performance.
87 Skipping the normalization should only be done in very special cases
88 where we can be sure that the input quaternion is normalized to avoid
89 error accumulation.
90 """
91 rotation = object.__new__(cls)
92 if normalized:
93 rotation.q = _np.array(q)
94 else:
95 rotation.q = _np.asarray(q) / _np.linalg.norm(q)
96 if (not rotation.q.ndim == 1) or (not len(rotation.q) == 4):
97 raise ValueError("Got quaternion array with unexpected dimensions")
98 return rotation
100 @classmethod
101 def from_rotation_matrix(cls, R):
102 """Create the object from a rotation matrix.
104 The code is based on Spurriers algorithm:
105 R. A. Spurrier (1978): “Comment on “Singularity-free extraction of a quaternion from a
106 direction-cosine matrix”
107 """
109 q = _np.zeros(4)
110 trace = _np.trace(R)
111 values = [R[i, i] for i in range(3)]
112 values.append(trace)
113 arg_max = _np.argmax(values)
114 if arg_max == 3:
115 q[0] = _np.sqrt(trace + 1) * 0.5
116 q[1] = (R[2, 1] - R[1, 2]) / (4 * q[0])
117 q[2] = (R[0, 2] - R[2, 0]) / (4 * q[0])
118 q[3] = (R[1, 0] - R[0, 1]) / (4 * q[0])
119 else:
120 i_index = arg_max
121 j_index = (i_index + 1) % 3
122 k_index = (i_index + 2) % 3
123 q_i = _np.sqrt(R[i_index, i_index] * 0.5 + (1 - trace) * 0.25)
124 q[0] = (R[k_index, j_index] - R[j_index, k_index]) / (4 * q_i)
125 q[i_index + 1] = q_i
126 q[j_index + 1] = (R[j_index, i_index] + R[i_index, j_index]) / (4 * q_i)
127 q[k_index + 1] = (R[k_index, i_index] + R[i_index, k_index]) / (4 * q_i)
129 return cls.from_quaternion(q)
131 @classmethod
132 def from_basis(cls, t1, t2):
133 """Create the object from two basis vectors t1, t2.
135 t2 will be orthogonalized on t1, and t3 will be calculated with
136 the cross product.
137 """
139 t1_normal = t1 / _np.linalg.norm(t1)
140 t2_ortho = t2 - t1_normal * _np.dot(t1_normal, t2)
141 t2_normal = t2_ortho / _np.linalg.norm(t2_ortho)
142 t3_normal = _np.cross(t1_normal, t2_normal)
144 R = _np.transpose([t1_normal, t2_normal, t3_normal])
145 return cls.from_rotation_matrix(R)
147 @classmethod
148 def from_rotation_vector(cls, rotation_vector):
149 """Create the object from a rotation vector."""
151 q = _np.zeros(4)
152 rotation_vector = _np.asarray(rotation_vector)
153 phi = _np.linalg.norm(rotation_vector)
154 q[0] = _np.cos(0.5 * phi)
155 if phi < _mpy.eps_quaternion:
156 # This is the Taylor series expansion of sin(phi/2)/phi around phi=0
157 q[1:] = 0.5 * rotation_vector
158 else:
159 q[1:] = _np.sin(0.5 * phi) / phi * rotation_vector
160 return cls.from_quaternion(q)
162 def check(self):
163 """Perform all checks for the rotation."""
164 self.check_uniqueness()
165 self.check_quaternion_constraint()
167 def check_uniqueness(self):
168 """We always want q0 to be positive -> the range for the rotational
169 angle is 0 <= phi <= pi."""
171 if self.q[0] < 0:
172 self.q *= -1
174 def check_quaternion_constraint(self):
175 """We want to check that q.q = 1."""
177 if _np.abs(1 - _np.linalg.norm(self.q)) > _mpy.eps_quaternion:
178 raise ValueError(
179 f"The rotation object is corrupted. q.q does not equal 1! q={self.q}"
180 )
182 def get_rotation_matrix(self):
183 """Return the rotation matrix for this rotation.
185 (Krenk (3.50))
186 """
187 q_skew = skew_matrix(self.q[1:])
188 R = (
189 (self.q[0] ** 2 - _np.dot(self.q[1:], self.q[1:])) * _np.eye(3)
190 + 2 * self.q[0] * q_skew
191 + 2 * ([self.q[1:]] * _np.transpose([self.q[1:]]))
192 )
194 return R
196 def get_quaternion(self):
197 """Return the quaternion for this rotation, as numpy array (copy)."""
199 return _np.array(self.q)
201 def get_rotation_vector(self):
202 """Return the rotation vector for this object."""
204 self.check()
206 norm = _np.linalg.norm(self.q[1:])
207 phi = 2 * _np.arctan2(norm, self.q[0])
209 if phi < _mpy.eps_quaternion:
210 # For small angles return the Taylor series expansion of phi/sin(phi/2)
211 scale_factor = 2
212 else:
213 scale_factor = phi / _np.sin(phi / 2)
214 if _np.abs(_np.abs(phi) - _np.pi) < _mpy.eps_quaternion:
215 # For rotations of exactly +-pi, numerical issues might occur, resulting in
216 # a rotation vector that is non-deterministic. The result is correct, but
217 # the sign can switch due to different implementation of basic underlying
218 # math functions. This is especially triggered when using this code with
219 # different OS. To avoid this, we scale the rotation axis in such a way,
220 # for a rotation angle of +-pi, the first component of the rotation axis
221 # that is not 0 is positive.
222 for i_dir in range(3):
223 if _np.abs(self.q[1 + i_dir]) > _mpy.eps_quaternion:
224 if self.q[1 + i_dir] < 0:
225 scale_factor *= -1
226 break
227 return self.q[1:] * scale_factor
229 def get_transformation_matrix(self):
230 """Return the transformation matrix for this rotation.
232 The transformation matrix maps the (infinitesimal)
233 multiplicative rotational increments onto the additive ones.
234 """
236 omega = self.get_rotation_vector()
237 omega_norm = _np.linalg.norm(omega)
239 # We have to take the inverse of the the rotation angle here, therefore,
240 # we have a branch for small angles where the singularity is not present.
241 if omega_norm**2 > _mpy.eps_quaternion:
242 # Taken from Jelenic and Crisfield (1999) Equation (2.5)
243 omega_dir = omega / omega_norm
244 omega_skew = skew_matrix(omega)
245 transformation_matrix = (
246 _np.outer(omega_dir, omega_dir)
247 - 0.5 * omega_skew
248 + 0.5
249 * omega_norm
250 / _np.tan(0.5 * omega_norm)
251 * (_np.identity(3) - _np.outer(omega_dir, omega_dir))
252 )
253 else:
254 # This is the constant part of the Taylor series expansion. If this
255 # function is used with automatic differentiation, higher order
256 # terms have to be added!
257 transformation_matrix = _np.identity(3)
258 return transformation_matrix
260 def get_transformation_matrix_inv(self):
261 """Return the inverse of the transformation matrix for this rotation.
263 The inverse of the transformation matrix maps the
264 (infinitesimal) additive rotational increments onto the
265 multiplicative ones.
266 """
268 omega = self.get_rotation_vector()
269 omega_norm = _np.linalg.norm(omega)
271 # We have to take the inverse of the the rotation angle here, therefore,
272 # we have a branch for small angles where the singularity is not present.
273 if omega_norm**2 > _mpy.eps_quaternion:
274 # Taken from Jelenic and Crisfield (1999) Equation (2.5)
275 omega_dir = omega / omega_norm
276 omega_skew = skew_matrix(omega)
277 transformation_matrix_inverse = (
278 (1.0 - _np.sin(omega_norm) / omega_norm)
279 * _np.outer(omega_dir, omega_dir)
280 + _np.sin(omega_norm) / omega_norm * _np.identity(3)
281 + (1.0 - _np.cos(omega_norm)) / omega_norm**2 * omega_skew
282 )
283 else:
284 # This is the constant part of the Taylor series expansion. If this
285 # function is used with automatic differentiation, higher order
286 # terms have to be added!
287 transformation_matrix_inverse = _np.identity(3)
288 return transformation_matrix_inverse
290 def inv(self):
291 """Return the inverse of this rotation."""
293 tmp_quaternion = self.q.copy()
294 tmp_quaternion[0] *= -1.0
295 return Rotation.from_quaternion(tmp_quaternion)
297 def __mul__(self, other):
298 """Add this rotation to another, or apply it on a vector."""
300 # Check if the other object is also a rotation.
301 if isinstance(other, Rotation):
302 # Get quaternions of the two objects.
303 p = self.q
304 q = other.q
305 # Add the rotations.
306 added_rotation = _np.zeros_like(self.q)
307 added_rotation[0] = p[0] * q[0] - _np.dot(p[1:], q[1:])
308 added_rotation[1:] = p[0] * q[1:] + q[0] * p[1:] + _np.cross(p[1:], q[1:])
309 return Rotation.from_quaternion(added_rotation)
310 elif isinstance(other, (list, _np.ndarray)) and len(other) == 3:
311 # Apply rotation to vector.
312 return _np.dot(self.get_rotation_matrix(), _np.asarray(other))
313 raise NotImplementedError("Error, not implemented, does not make sense anyway!")
315 def __eq__(self, other):
316 """Check if the other rotation is equal to this one."""
318 if isinstance(other, Rotation):
319 return bool(
320 (_np.linalg.norm(self.q - other.q) < _mpy.eps_quaternion)
321 or (_np.linalg.norm(self.q + other.q) < _mpy.eps_quaternion)
322 )
323 else:
324 return object.__eq__(self, other)
326 def copy(self):
327 """Return a deep copy of this object."""
328 return _copy.deepcopy(self)
330 def __str__(self):
331 """String representation of object."""
333 self.check()
334 return f"Rotation:\n q0: {self.q[0]}\n q: {self.q[1:]}"
337def add_rotations(rotation_21, rotation_10):
338 """Multiply a rotation onto another.
340 Args
341 ----
342 rotation_10: _np.ndarray
343 Array with the dimensions n x 4 or 4 x 1.
344 The first rotation that is applied.
345 rotation_21: _np.ndarray
346 Array with the dimensions n x 4 or 4 x 1.
347 The second rotation that is applied.
349 Return
350 ----
351 rot_new: _np.ndarray
352 Array with the dimensions n x 4.
353 This array contains the new quaternions.
354 """
356 # Transpose the arrays, to work with the following code.
357 if isinstance(rotation_10, Rotation):
358 rot1 = rotation_10.get_quaternion().transpose()
359 else:
360 rot1 = _np.transpose(rotation_10)
361 if isinstance(rotation_21, Rotation):
362 rot2 = rotation_21.get_quaternion().transpose()
363 else:
364 rot2 = _np.transpose(rotation_21)
366 if rot1.size > rot2.size:
367 rotnew = _np.zeros_like(rot1)
368 else:
369 rotnew = _np.zeros_like(rot2)
371 # Multiply the two rotations (code is taken from /utility/rotation.nb).
372 rotnew[0] = (
373 rot1[0] * rot2[0] - rot1[1] * rot2[1] - rot1[2] * rot2[2] - rot1[3] * rot2[3]
374 )
375 rotnew[1] = (
376 rot1[1] * rot2[0] + rot1[0] * rot2[1] + rot1[3] * rot2[2] - rot1[2] * rot2[3]
377 )
378 rotnew[2] = (
379 rot1[2] * rot2[0] - rot1[3] * rot2[1] + rot1[0] * rot2[2] + rot1[1] * rot2[3]
380 )
381 rotnew[3] = (
382 rot1[3] * rot2[0] + rot1[2] * rot2[1] - rot1[1] * rot2[2] + rot1[0] * rot2[3]
383 )
385 return rotnew.transpose()
388def rotate_coordinates(coordinates, rotation, *, origin=None):
389 """Rotate all given coordinates.
391 Args
392 ----
393 coordinates: _np.array
394 Array of 3D coordinates to be rotated
395 rotation: Rotation, list(quaternions) (nx4)
396 The rotation that will be applied to the coordinates. Can also be an
397 array with a quaternion for each coordinate.
398 origin: 3D vector
399 If this is given, the mesh is rotated about this point. Default is
400 (0,0,0)
401 """
403 if isinstance(rotation, Rotation):
404 rotation = rotation.get_quaternion().transpose()
406 # Check if origin has to be added
407 if origin is None:
408 origin = [0.0, 0.0, 0.0]
410 # New position array
411 coordinates_new = _np.zeros_like(coordinates)
413 # Evaluate the new positions using the numpy data structure
414 # (code is taken from /utility/rotation.nb)
415 rotation = rotation.transpose()
417 q0_q0 = _np.square(rotation[0])
418 q0_q1_2 = 2.0 * rotation[0] * rotation[1]
419 q0_q2_2 = 2.0 * rotation[0] * rotation[2]
420 q0_q3_2 = 2.0 * rotation[0] * rotation[3]
422 q1_q1 = _np.square(rotation[1])
423 q1_q2_2 = 2.0 * rotation[1] * rotation[2]
424 q1_q3_2 = 2.0 * rotation[1] * rotation[3]
426 q2_q2 = _np.square(rotation[2])
427 q2_q3_2 = 2.0 * rotation[2] * rotation[3]
429 q3_q3 = _np.square(rotation[3])
431 coordinates_new[:, 0] = (
432 (q0_q0 + q1_q1 - q2_q2 - q3_q3) * (coordinates[:, 0] - origin[0])
433 + (q1_q2_2 - q0_q3_2) * (coordinates[:, 1] - origin[1])
434 + (q0_q2_2 + q1_q3_2) * (coordinates[:, 2] - origin[2])
435 )
436 coordinates_new[:, 1] = (
437 (q1_q2_2 + q0_q3_2) * (coordinates[:, 0] - origin[0])
438 + (q0_q0 - q1_q1 + q2_q2 - q3_q3) * (coordinates[:, 1] - origin[1])
439 + (-q0_q1_2 + q2_q3_2) * (coordinates[:, 2] - origin[2])
440 )
441 coordinates_new[:, 2] = (
442 (-q0_q2_2 + q1_q3_2) * (coordinates[:, 0] - origin[0])
443 + (q0_q1_2 + q2_q3_2) * (coordinates[:, 1] - origin[1])
444 + (q0_q0 - q1_q1 - q2_q2 + q3_q3) * (coordinates[:, 2] - origin[2])
445 )
447 if origin is not None:
448 coordinates_new += origin
450 return coordinates_new
453def smallest_rotation(q: Rotation, t):
454 """Get the triad that results from the smallest rotation (rotation without
455 twist) from the triad q such that the rotated first basis vector aligns
456 with t. For more details see Christoph Meier's dissertation chapter 2.1.2.
458 Args
459 ----
460 q: Rotation
461 Starting triad.
462 t: Vector in R3
463 Direction of the first basis of the rotated triad.
464 Return
465 ----
466 q_sr: Rotation
467 The triad that results from a smallest rotation.
468 """
470 R_old = q.get_rotation_matrix()
471 g1_old = R_old[:, 0]
472 g1 = _np.asarray(t) / _np.linalg.norm(t)
474 # Quaternion components of relative rotation
475 q_rel = _np.zeros(4)
477 # The scalar quaternion part is cos(alpha/2) this is equal to
478 q_rel[0] = _np.linalg.norm(0.5 * (g1_old + g1))
480 # Vector part of the quaternion is sin(alpha/2)*axis
481 q_rel[1:] = _np.cross(g1_old, g1) / (2.0 * q_rel[0])
483 return Rotation.from_quaternion(q_rel) * q