Coverage for src/beamme/core/rotation.py: 96%
227 statements
« prev ^ index » next coverage.py v7.15.2, created at 2026-07-28 15:20 +0000
1# The MIT License (MIT)
2#
3# Copyright (c) 2018-2026 BeamMe 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."""
24from typing import Self as _Self
26import numpy as _np
27import quaternion as _quaternion
28from numpy.typing import NDArray as _NDArray
30from beamme.core.conf import bme as _bme
33def skew_matrix(vector):
34 """Return the skew matrix for the vector."""
35 skew = _np.zeros([3, 3])
36 skew[0, 1] = -vector[2]
37 skew[0, 2] = vector[1]
38 skew[1, 0] = vector[2]
39 skew[1, 2] = -vector[0]
40 skew[2, 0] = -vector[1]
41 skew[2, 1] = vector[0]
42 return skew
45class Rotation:
46 """A class that represents a rotation of a coordinate system.
48 Internally the rotations are stored as quaternions.
49 """
51 def __init__(self, *args):
52 """Initialize the rotation object.
54 Args
55 ----
56 *args:
57 - Rotation()
58 Create a identity rotation.
59 - Rotation(axis, phi)
60 Create a rotation around the vector axis with the angle phi.
61 """
62 self.q = _np.zeros(4)
64 if len(args) == 0:
65 # Identity element.
66 self.q[0] = 1
67 elif len(args) == 2:
68 # Set from rotation axis and rotation angle.
69 axis = _np.asarray(args[0])
70 phi = args[1]
71 norm = _np.linalg.norm(axis)
72 if norm < _bme.eps_quaternion:
73 raise ValueError("The rotation axis can not be a zero vector!")
74 self.q[0] = _np.cos(0.5 * phi)
75 self.q[1:] = _np.sin(0.5 * phi) * axis / norm
76 else:
77 raise ValueError(f"The given arguments {args} are invalid!")
79 @classmethod
80 def from_quaternion(cls, q, *, normalized=False) -> _Self:
81 """Create the object from a quaternion float array (4x1)
83 Args
84 ----
85 q: Quaternion, q0, qx,qy,qz
86 normalized: Flag if the input quaternion is normalized. If so, no
87 normalization is performed which can potentially improve performance.
88 Skipping the normalization should only be done in very special cases
89 where we can be sure that the input quaternion is normalized to avoid
90 error accumulation.
91 """
92 if isinstance(q, _quaternion.quaternion):
93 q = _quaternion.as_float_array(q)
95 rotation = object.__new__(cls)
96 if normalized:
97 rotation.q = _np.array(q)
98 else:
99 rotation.q = _np.asarray(q) / _np.linalg.norm(q)
100 if (not rotation.q.ndim == 1) or (not len(rotation.q) == 4):
101 raise ValueError("Got quaternion array with unexpected dimensions")
102 return rotation
104 @classmethod
105 def from_rotation_matrix(cls, R) -> _Self:
106 """Create the object from a rotation matrix.
108 The code is based on Spurriers algorithm:
109 R. A. Spurrier (1978): “Comment on “Singularity-free extraction of a quaternion from a
110 direction-cosine matrix”
111 """
112 R = _np.asarray(R)
113 q = _np.zeros(4)
114 trace = _np.trace(R)
115 values = [R[i, i] for i in range(3)]
116 values.append(trace)
117 arg_max = _np.argmax(values)
118 if arg_max == 3:
119 q[0] = _np.sqrt(trace + 1) * 0.5
120 q[1] = (R[2, 1] - R[1, 2]) / (4 * q[0])
121 q[2] = (R[0, 2] - R[2, 0]) / (4 * q[0])
122 q[3] = (R[1, 0] - R[0, 1]) / (4 * q[0])
123 else:
124 i_index = arg_max
125 j_index = (i_index + 1) % 3
126 k_index = (i_index + 2) % 3
127 q_i = _np.sqrt(R[i_index, i_index] * 0.5 + (1 - trace) * 0.25)
128 q[0] = (R[k_index, j_index] - R[j_index, k_index]) / (4 * q_i)
129 q[i_index + 1] = q_i
130 q[j_index + 1] = (R[j_index, i_index] + R[i_index, j_index]) / (4 * q_i)
131 q[k_index + 1] = (R[k_index, i_index] + R[i_index, k_index]) / (4 * q_i)
133 return cls.from_quaternion(q)
135 @classmethod
136 def from_basis(cls, t1, t2) -> _Self:
137 """Create the object from two basis vectors t1, t2.
139 t2 will be orthogonalized on t1, and t3 will be calculated with the cross
140 product.
141 """
142 t1_norm = _np.linalg.norm(t1)
143 if t1_norm < _bme.eps_quaternion:
144 raise ValueError(f"The given vector t1 can not be a zero vector, got {t1}.")
145 t1_normal = t1 / t1_norm
147 t2_ortho = t2 - t1_normal * _np.dot(t1_normal, t2)
148 t2_ortho_norm = _np.linalg.norm(t2_ortho)
149 if t2_ortho_norm < _bme.eps_quaternion:
150 raise ValueError(
151 f"Got two vectors {t1} and {t2} that are not linear independent."
152 )
153 t2_normal = t2_ortho / t2_ortho_norm
154 t3_normal = _np.cross(t1_normal, t2_normal)
156 R = _np.transpose([t1_normal, t2_normal, t3_normal])
157 return cls.from_rotation_matrix(R)
159 @classmethod
160 def from_rotation_vector(cls, rotation_vector) -> _Self:
161 """Create the object from a rotation vector."""
162 q = _np.zeros(4)
163 rotation_vector = _np.asarray(rotation_vector)
164 phi = _np.linalg.norm(rotation_vector)
165 q[0] = _np.cos(0.5 * phi)
166 if phi < _bme.eps_quaternion:
167 # This is the Taylor series expansion of sin(phi/2)/phi around phi=0
168 q[1:] = 0.5 * rotation_vector
169 else:
170 q[1:] = _np.sin(0.5 * phi) / phi * rotation_vector
171 return cls.from_quaternion(q)
173 def check(self):
174 """Perform all checks for the rotation."""
175 self.check_uniqueness()
176 self.check_quaternion_constraint()
178 def check_uniqueness(self):
179 """We always want q0 to be positive -> the range for the rotational angle is 0
180 <= phi <= pi."""
181 if self.q[0] < 0:
182 self.q *= -1
184 def check_quaternion_constraint(self):
185 """We want to check that q.q = 1."""
187 if _np.abs(1 - _np.linalg.norm(self.q)) > _bme.eps_quaternion:
188 raise ValueError(
189 f"The rotation object is corrupted. q.q does not equal 1! q={self.q}"
190 )
192 def get_rotation_matrix(self):
193 """Return the rotation matrix for this rotation.
195 (Krenk (3.50))
196 """
197 q_skew = skew_matrix(self.q[1:])
198 R = (
199 (self.q[0] ** 2 - _np.dot(self.q[1:], self.q[1:])) * _np.eye(3)
200 + 2 * self.q[0] * q_skew
201 + 2 * ([self.q[1:]] * _np.transpose([self.q[1:]]))
202 )
204 return R
206 def get_quaternion(self):
207 """Return the quaternion for this rotation, as numpy array (copy)."""
208 return _np.array(self.q)
210 def get_numpy_quaternion(self):
211 """Return a numpy quaternion object representing this rotation (copy)."""
212 return _quaternion.from_float_array(self.q)
214 def get_rotation_vector(self):
215 """Return the rotation vector for this object."""
216 self.check()
218 norm = _np.linalg.norm(self.q[1:])
219 phi = 2 * _np.arctan2(norm, self.q[0])
221 if phi < _bme.eps_quaternion:
222 # For small angles return the Taylor series expansion of phi/sin(phi/2)
223 scale_factor = 2
224 else:
225 scale_factor = phi / _np.sin(phi / 2)
226 if _np.abs(_np.abs(phi) - _np.pi) < _bme.eps_quaternion:
227 # For rotations of exactly +-pi, numerical issues might occur, resulting in
228 # a rotation vector that is non-deterministic. The result is correct, but
229 # the sign can switch due to different implementation of basic underlying
230 # math functions. This is especially triggered when using this code with
231 # different OS. To avoid this, we scale the rotation axis in such a way,
232 # for a rotation angle of +-pi, the first component of the rotation axis
233 # that is not 0 is positive.
234 for i_dir in range(3):
235 if _np.abs(self.q[1 + i_dir]) > _bme.eps_quaternion:
236 if self.q[1 + i_dir] < 0:
237 scale_factor *= -1
238 break
239 return self.q[1:] * scale_factor
241 def get_transformation_matrix(self):
242 """Return the transformation matrix for this rotation.
244 The transformation matrix maps the (infinitesimal) multiplicative rotational
245 increments onto the additive ones.
246 """
247 omega = self.get_rotation_vector()
248 omega_norm = _np.linalg.norm(omega)
250 # We have to take the inverse of the the rotation angle here, therefore,
251 # we have a branch for small angles where the singularity is not present.
252 if omega_norm**2 > _bme.eps_quaternion:
253 # Taken from Jelenic and Crisfield (1999) Equation (2.5)
254 omega_dir = omega / omega_norm
255 omega_skew = skew_matrix(omega)
256 transformation_matrix = (
257 _np.outer(omega_dir, omega_dir)
258 - 0.5 * omega_skew
259 + 0.5
260 * omega_norm
261 / _np.tan(0.5 * omega_norm)
262 * (_np.identity(3) - _np.outer(omega_dir, omega_dir))
263 )
264 else:
265 # This is the constant part of the Taylor series expansion. If this
266 # function is used with automatic differentiation, higher order
267 # terms have to be added!
268 transformation_matrix = _np.identity(3)
269 return transformation_matrix
271 def get_transformation_matrix_inv(self):
272 """Return the inverse of the transformation matrix for this rotation.
274 The inverse of the transformation matrix maps the (infinitesimal) additive
275 rotational increments onto the multiplicative ones.
276 """
277 omega = self.get_rotation_vector()
278 omega_norm = _np.linalg.norm(omega)
280 # We have to take the inverse of the the rotation angle here, therefore,
281 # we have a branch for small angles where the singularity is not present.
282 if omega_norm**2 > _bme.eps_quaternion:
283 # Taken from Jelenic and Crisfield (1999) Equation (2.5)
284 omega_dir = omega / omega_norm
285 omega_skew = skew_matrix(omega)
286 transformation_matrix_inverse = (
287 (1.0 - _np.sin(omega_norm) / omega_norm)
288 * _np.outer(omega_dir, omega_dir)
289 + _np.sin(omega_norm) / omega_norm * _np.identity(3)
290 + (1.0 - _np.cos(omega_norm)) / omega_norm**2 * omega_skew
291 )
292 else:
293 # This is the constant part of the Taylor series expansion. If this
294 # function is used with automatic differentiation, higher order
295 # terms have to be added!
296 transformation_matrix_inverse = _np.identity(3)
297 return transformation_matrix_inverse
299 def inv(self):
300 """Return the inverse of this rotation."""
301 tmp_quaternion = self.q.copy()
302 tmp_quaternion[0] *= -1.0
303 return Rotation.from_quaternion(tmp_quaternion)
305 def __mul__(self, other):
306 """Add this rotation to another, or apply it on a vector."""
307 # Check if the other object is also a rotation.
308 if isinstance(other, Rotation):
309 # Get quaternions of the two objects.
310 p = self.q
311 q = other.q
312 # Add the rotations.
313 added_rotation = _np.zeros_like(self.q)
314 added_rotation[0] = p[0] * q[0] - _np.dot(p[1:], q[1:])
315 added_rotation[1:] = p[0] * q[1:] + q[0] * p[1:] + _np.cross(p[1:], q[1:])
316 return Rotation.from_quaternion(added_rotation)
317 elif isinstance(other, (list, _np.ndarray)) and len(other) == 3:
318 # Apply rotation to vector.
319 return _np.dot(self.get_rotation_matrix(), _np.asarray(other))
320 raise NotImplementedError("Error, not implemented, does not make sense anyway!")
322 def __eq__(self, other):
323 """Check if the other rotation is equal to this one."""
324 if isinstance(other, Rotation):
325 return bool(
326 (_np.linalg.norm(self.q - other.q) < _bme.eps_quaternion)
327 or (_np.linalg.norm(self.q + other.q) < _bme.eps_quaternion)
328 )
329 else:
330 return object.__eq__(self, other)
332 def copy(self):
333 """Return a copy of this object."""
334 return Rotation.from_quaternion(self.q, normalized=True)
336 def __str__(self):
337 """String representation of object."""
338 self.check()
339 return f"Rotation:\n q0: {self.q[0]}\n q: {self.q[1:]}"
342def add_rotations(
343 rotation_21: Rotation | _NDArray[_quaternion.quaternion],
344 rotation_10: Rotation | _NDArray[_quaternion.quaternion],
345) -> _NDArray[_quaternion.quaternion]:
346 """Multiply rotations onto another.
348 Args:
349 rotation_10: The first rotation(s) that are applied.
350 rotation_21: The second rotation(s) that are applied.
352 Returns:
353 An array with the compound quaternions.
354 """
355 # Transpose the arrays, to work with the following code.
356 if isinstance(rotation_10, Rotation):
357 rot1 = rotation_10.get_quaternion().transpose()
358 else:
359 rot1 = _np.transpose(rotation_10)
360 if isinstance(rotation_21, Rotation):
361 rot2 = rotation_21.get_quaternion().transpose()
362 else:
363 rot2 = _np.transpose(rotation_21)
365 if rot1.size > rot2.size:
366 rotnew = _np.zeros_like(rot1)
367 else:
368 rotnew = _np.zeros_like(rot2)
370 # Multiply the two rotations (code is taken from /utility/rotation.nb).
371 rotnew[0] = (
372 rot1[0] * rot2[0] - rot1[1] * rot2[1] - rot1[2] * rot2[2] - rot1[3] * rot2[3]
373 )
374 rotnew[1] = (
375 rot1[1] * rot2[0] + rot1[0] * rot2[1] + rot1[3] * rot2[2] - rot1[2] * rot2[3]
376 )
377 rotnew[2] = (
378 rot1[2] * rot2[0] - rot1[3] * rot2[1] + rot1[0] * rot2[2] + rot1[1] * rot2[3]
379 )
380 rotnew[3] = (
381 rot1[3] * rot2[0] + rot1[2] * rot2[1] - rot1[1] * rot2[2] + rot1[0] * rot2[3]
382 )
384 return rotnew.transpose()
387def rotate_coordinates(
388 coordinates: _NDArray,
389 rotation: Rotation | _NDArray[_quaternion.quaternion],
390 *,
391 origin=None,
392):
393 """Rotate all given coordinates.
395 Args:
396 coordinates: Array of 3D coordinates to be rotated
397 rotation: The rotation(s) that will be applied to the coordinates. If
398 this is an array it has to hold a quaternion for each coordinate.
399 origin (3D vector): If this is given, the mesh is rotated about this
400 point. Defaults to (0, 0, 0).
401 """
402 if isinstance(rotation, Rotation):
403 rotation = rotation.get_quaternion().transpose()
405 # Check if origin has to be added
406 if origin is None:
407 origin = [0.0, 0.0, 0.0]
409 # New position array
410 coordinates_new = _np.zeros_like(coordinates)
412 # Evaluate the new positions using the numpy data structure
413 # (code is taken from /utility/rotation.nb)
414 rotation = rotation.transpose()
416 q0_q0 = _np.square(rotation[0])
417 q0_q1_2 = 2.0 * rotation[0] * rotation[1]
418 q0_q2_2 = 2.0 * rotation[0] * rotation[2]
419 q0_q3_2 = 2.0 * rotation[0] * rotation[3]
421 q1_q1 = _np.square(rotation[1])
422 q1_q2_2 = 2.0 * rotation[1] * rotation[2]
423 q1_q3_2 = 2.0 * rotation[1] * rotation[3]
425 q2_q2 = _np.square(rotation[2])
426 q2_q3_2 = 2.0 * rotation[2] * rotation[3]
428 q3_q3 = _np.square(rotation[3])
430 coordinates_new[:, 0] = (
431 (q0_q0 + q1_q1 - q2_q2 - q3_q3) * (coordinates[:, 0] - origin[0])
432 + (q1_q2_2 - q0_q3_2) * (coordinates[:, 1] - origin[1])
433 + (q0_q2_2 + q1_q3_2) * (coordinates[:, 2] - origin[2])
434 )
435 coordinates_new[:, 1] = (
436 (q1_q2_2 + q0_q3_2) * (coordinates[:, 0] - origin[0])
437 + (q0_q0 - q1_q1 + q2_q2 - q3_q3) * (coordinates[:, 1] - origin[1])
438 + (-q0_q1_2 + q2_q3_2) * (coordinates[:, 2] - origin[2])
439 )
440 coordinates_new[:, 2] = (
441 (-q0_q2_2 + q1_q3_2) * (coordinates[:, 0] - origin[0])
442 + (q0_q1_2 + q2_q3_2) * (coordinates[:, 1] - origin[1])
443 + (q0_q0 - q1_q1 - q2_q2 + q3_q3) * (coordinates[:, 2] - origin[2])
444 )
446 if origin is not None:
447 coordinates_new += origin
449 return coordinates_new
452def smallest_rotation(q: Rotation, t):
453 """Get the triad that results from the smallest rotation (rotation without twist)
454 from the triad q such that the rotated first basis vector aligns with t. For more
455 details see Christoph Meier's dissertation chapter 2.1.2.
457 Args
458 ----
459 q: Rotation
460 Starting triad.
461 t: Vector in R3
462 Direction of the first basis of the rotated triad.
463 Return
464 ----
465 q_sr: Rotation
466 The triad that results from a smallest rotation.
467 """
468 R_old = q.get_rotation_matrix()
469 g1_old = R_old[:, 0]
470 g1 = _np.asarray(t) / _np.linalg.norm(t)
472 # Quaternion components of relative rotation
473 q_rel = _np.zeros(4)
475 # The scalar quaternion part is cos(alpha/2) this is equal to
476 q_rel[0] = _np.linalg.norm(0.5 * (g1_old + g1))
478 # Vector part of the quaternion is sin(alpha/2)*axis
479 q_rel[1:] = _np.cross(g1_old, g1) / (2.0 * q_rel[0])
481 return Rotation.from_quaternion(q_rel) * q
484def get_rotation_vector_series(
485 rotation_vectors: list | _NDArray | list[Rotation],
486) -> _NDArray:
487 """Return an array containing the rotation vectors representing the given rotation
488 vectors.
490 The main feature of this function is, that the returned rotation
491 vectors don't have jumps when the rotation angle exceeds 2*pi. We
492 return a "continuous" series of rotation vectors that can be used to
493 interpolate between them.
495 Note: Interpolating between the returned rotation vectors will in general
496 result in a non-objective interpolation.
498 Args:
499 rotation_vectors: A list or array containing the rotation vectors, has
500 to be in order.
502 Returns:
503 An array containing the "continuous" rotation vectors.
504 """
505 if isinstance(rotation_vectors, list):
506 rotation_vector_array = _np.zeros((len(rotation_vectors), 3))
507 for i_rotation, rotation_entry in enumerate(rotation_vectors):
508 if isinstance(rotation_entry, Rotation):
509 rotation_vector_array[i_rotation] = rotation_entry.get_rotation_vector()
510 else:
511 rotation_vector_array[i_rotation] = rotation_entry
513 def closest_multiple_of_two_pi(x: float) -> float:
514 """Given the value x, return the multiple of 2*pi that is closest to it."""
515 return 2.0 * _np.pi * round(x / (2 * _np.pi))
517 rotation_vectors_continuous = _np.zeros((len(rotation_vector_array), 3))
518 rotation_vectors_continuous[0, :] = rotation_vector_array[0]
520 for i, current_rotation_vector in enumerate(rotation_vector_array[1:]):
521 rotation_vector_last = rotation_vector_array[i]
522 theta = _np.linalg.norm(current_rotation_vector)
524 if theta < _bme.eps_quaternion:
525 # The current rotation is an identity rotation.
526 theta_last = _np.linalg.norm(rotation_vector_last)
527 if theta_last < _bme.eps_quaternion:
528 # The last rotation was also an identity rotation, so simply add
529 # an empty rotation vector
530 rotation_vectors_continuous[i + 1] = [0.0, 0.0, 0.0]
531 else:
532 # The last rotation was not a zero rotation. In this case we simply
533 # take the direction of the last rotation vector and set it to to the
534 # closest length which is a multiple of 2*pi.
535 rotation_vectors_continuous[i + 1] = (
536 rotation_vector_last
537 / theta_last
538 * closest_multiple_of_two_pi(theta_last)
539 )
540 else:
541 axis = current_rotation_vector / theta
542 # This is the offset from the current rotation vector to the previous one
543 # in the sense of a multiple of 2*pi.
544 multiple_of_two_pi = closest_multiple_of_two_pi(
545 (_np.dot(axis, rotation_vector_last) - theta)
546 )
547 rotation_vectors_continuous[i + 1] = (multiple_of_two_pi + theta) * axis
548 return rotation_vectors_continuous