Coverage for src / beamme / core / rotation.py: 96%
226 statements
« prev ^ index » next coverage.py v7.13.4, created at 2026-02-23 08:08 +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."""
24import numpy as _np
25import quaternion as _quaternion
26from numpy.typing import NDArray as _NDArray
28from beamme.core.conf import bme as _bme
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 < _bme.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 if isinstance(q, _quaternion.quaternion):
92 q = _quaternion.as_float_array(q)
94 rotation = object.__new__(cls)
95 if normalized:
96 rotation.q = _np.array(q)
97 else:
98 rotation.q = _np.asarray(q) / _np.linalg.norm(q)
99 if (not rotation.q.ndim == 1) or (not len(rotation.q) == 4):
100 raise ValueError("Got quaternion array with unexpected dimensions")
101 return rotation
103 @classmethod
104 def from_rotation_matrix(cls, R):
105 """Create the object from a rotation matrix.
107 The code is based on Spurriers algorithm:
108 R. A. Spurrier (1978): “Comment on “Singularity-free extraction of a quaternion from a
109 direction-cosine matrix”
110 """
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):
137 """Create the object from two basis vectors t1, t2.
139 t2 will be orthogonalized on t1, and t3 will be calculated with
140 the cross product.
141 """
143 t1_norm = _np.linalg.norm(t1)
144 if t1_norm < _bme.eps_quaternion:
145 raise ValueError(f"The given vector t1 can not be a zero vector, got {t1}.")
146 t1_normal = t1 / t1_norm
148 t2_ortho = t2 - t1_normal * _np.dot(t1_normal, t2)
149 t2_ortho_norm = _np.linalg.norm(t2_ortho)
150 if t2_ortho_norm < _bme.eps_quaternion:
151 raise ValueError(
152 f"Got two vectors {t1} and {t2} that are not linear independent."
153 )
154 t2_normal = t2_ortho / t2_ortho_norm
155 t3_normal = _np.cross(t1_normal, t2_normal)
157 R = _np.transpose([t1_normal, t2_normal, t3_normal])
158 return cls.from_rotation_matrix(R)
160 @classmethod
161 def from_rotation_vector(cls, rotation_vector):
162 """Create the object from a rotation vector."""
164 q = _np.zeros(4)
165 rotation_vector = _np.asarray(rotation_vector)
166 phi = _np.linalg.norm(rotation_vector)
167 q[0] = _np.cos(0.5 * phi)
168 if phi < _bme.eps_quaternion:
169 # This is the Taylor series expansion of sin(phi/2)/phi around phi=0
170 q[1:] = 0.5 * rotation_vector
171 else:
172 q[1:] = _np.sin(0.5 * phi) / phi * rotation_vector
173 return cls.from_quaternion(q)
175 def check(self):
176 """Perform all checks for the rotation."""
177 self.check_uniqueness()
178 self.check_quaternion_constraint()
180 def check_uniqueness(self):
181 """We always want q0 to be positive -> the range for the rotational
182 angle is 0 <= phi <= pi."""
184 if self.q[0] < 0:
185 self.q *= -1
187 def check_quaternion_constraint(self):
188 """We want to check that q.q = 1."""
190 if _np.abs(1 - _np.linalg.norm(self.q)) > _bme.eps_quaternion:
191 raise ValueError(
192 f"The rotation object is corrupted. q.q does not equal 1! q={self.q}"
193 )
195 def get_rotation_matrix(self):
196 """Return the rotation matrix for this rotation.
198 (Krenk (3.50))
199 """
200 q_skew = skew_matrix(self.q[1:])
201 R = (
202 (self.q[0] ** 2 - _np.dot(self.q[1:], self.q[1:])) * _np.eye(3)
203 + 2 * self.q[0] * q_skew
204 + 2 * ([self.q[1:]] * _np.transpose([self.q[1:]]))
205 )
207 return R
209 def get_quaternion(self):
210 """Return the quaternion for this rotation, as numpy array (copy)."""
211 return _np.array(self.q)
213 def get_numpy_quaternion(self):
214 """Return a numpy quaternion object representing this rotation
215 (copy)."""
216 return _quaternion.from_float_array(_np.array(self.q))
218 def get_rotation_vector(self):
219 """Return the rotation vector for this object."""
221 self.check()
223 norm = _np.linalg.norm(self.q[1:])
224 phi = 2 * _np.arctan2(norm, self.q[0])
226 if phi < _bme.eps_quaternion:
227 # For small angles return the Taylor series expansion of phi/sin(phi/2)
228 scale_factor = 2
229 else:
230 scale_factor = phi / _np.sin(phi / 2)
231 if _np.abs(_np.abs(phi) - _np.pi) < _bme.eps_quaternion:
232 # For rotations of exactly +-pi, numerical issues might occur, resulting in
233 # a rotation vector that is non-deterministic. The result is correct, but
234 # the sign can switch due to different implementation of basic underlying
235 # math functions. This is especially triggered when using this code with
236 # different OS. To avoid this, we scale the rotation axis in such a way,
237 # for a rotation angle of +-pi, the first component of the rotation axis
238 # that is not 0 is positive.
239 for i_dir in range(3):
240 if _np.abs(self.q[1 + i_dir]) > _bme.eps_quaternion:
241 if self.q[1 + i_dir] < 0:
242 scale_factor *= -1
243 break
244 return self.q[1:] * scale_factor
246 def get_transformation_matrix(self):
247 """Return the transformation matrix for this rotation.
249 The transformation matrix maps the (infinitesimal)
250 multiplicative rotational increments onto the additive ones.
251 """
253 omega = self.get_rotation_vector()
254 omega_norm = _np.linalg.norm(omega)
256 # We have to take the inverse of the the rotation angle here, therefore,
257 # we have a branch for small angles where the singularity is not present.
258 if omega_norm**2 > _bme.eps_quaternion:
259 # Taken from Jelenic and Crisfield (1999) Equation (2.5)
260 omega_dir = omega / omega_norm
261 omega_skew = skew_matrix(omega)
262 transformation_matrix = (
263 _np.outer(omega_dir, omega_dir)
264 - 0.5 * omega_skew
265 + 0.5
266 * omega_norm
267 / _np.tan(0.5 * omega_norm)
268 * (_np.identity(3) - _np.outer(omega_dir, omega_dir))
269 )
270 else:
271 # This is the constant part of the Taylor series expansion. If this
272 # function is used with automatic differentiation, higher order
273 # terms have to be added!
274 transformation_matrix = _np.identity(3)
275 return transformation_matrix
277 def get_transformation_matrix_inv(self):
278 """Return the inverse of the transformation matrix for this rotation.
280 The inverse of the transformation matrix maps the
281 (infinitesimal) additive rotational increments onto the
282 multiplicative ones.
283 """
285 omega = self.get_rotation_vector()
286 omega_norm = _np.linalg.norm(omega)
288 # We have to take the inverse of the the rotation angle here, therefore,
289 # we have a branch for small angles where the singularity is not present.
290 if omega_norm**2 > _bme.eps_quaternion:
291 # Taken from Jelenic and Crisfield (1999) Equation (2.5)
292 omega_dir = omega / omega_norm
293 omega_skew = skew_matrix(omega)
294 transformation_matrix_inverse = (
295 (1.0 - _np.sin(omega_norm) / omega_norm)
296 * _np.outer(omega_dir, omega_dir)
297 + _np.sin(omega_norm) / omega_norm * _np.identity(3)
298 + (1.0 - _np.cos(omega_norm)) / omega_norm**2 * omega_skew
299 )
300 else:
301 # This is the constant part of the Taylor series expansion. If this
302 # function is used with automatic differentiation, higher order
303 # terms have to be added!
304 transformation_matrix_inverse = _np.identity(3)
305 return transformation_matrix_inverse
307 def inv(self):
308 """Return the inverse of this rotation."""
310 tmp_quaternion = self.q.copy()
311 tmp_quaternion[0] *= -1.0
312 return Rotation.from_quaternion(tmp_quaternion)
314 def __mul__(self, other):
315 """Add this rotation to another, or apply it on a vector."""
317 # Check if the other object is also a rotation.
318 if isinstance(other, Rotation):
319 # Get quaternions of the two objects.
320 p = self.q
321 q = other.q
322 # Add the rotations.
323 added_rotation = _np.zeros_like(self.q)
324 added_rotation[0] = p[0] * q[0] - _np.dot(p[1:], q[1:])
325 added_rotation[1:] = p[0] * q[1:] + q[0] * p[1:] + _np.cross(p[1:], q[1:])
326 return Rotation.from_quaternion(added_rotation)
327 elif isinstance(other, (list, _np.ndarray)) and len(other) == 3:
328 # Apply rotation to vector.
329 return _np.dot(self.get_rotation_matrix(), _np.asarray(other))
330 raise NotImplementedError("Error, not implemented, does not make sense anyway!")
332 def __eq__(self, other):
333 """Check if the other rotation is equal to this one."""
335 if isinstance(other, Rotation):
336 return bool(
337 (_np.linalg.norm(self.q - other.q) < _bme.eps_quaternion)
338 or (_np.linalg.norm(self.q + other.q) < _bme.eps_quaternion)
339 )
340 else:
341 return object.__eq__(self, other)
343 def copy(self):
344 """Return a copy of this object."""
345 return Rotation.from_quaternion(self.q, normalized=True)
347 def __str__(self):
348 """String representation of object."""
350 self.check()
351 return f"Rotation:\n q0: {self.q[0]}\n q: {self.q[1:]}"
354def add_rotations(
355 rotation_21: Rotation | _NDArray[_quaternion.quaternion],
356 rotation_10: Rotation | _NDArray[_quaternion.quaternion],
357) -> _NDArray[_quaternion.quaternion]:
358 """Multiply rotations onto another.
360 Args:
361 rotation_10: The first rotation(s) that are applied.
362 rotation_21: The second rotation(s) that are applied.
364 Returns:
365 An array with the compound quaternions.
366 """
368 # Transpose the arrays, to work with the following code.
369 if isinstance(rotation_10, Rotation):
370 rot1 = rotation_10.get_quaternion().transpose()
371 else:
372 rot1 = _np.transpose(rotation_10)
373 if isinstance(rotation_21, Rotation):
374 rot2 = rotation_21.get_quaternion().transpose()
375 else:
376 rot2 = _np.transpose(rotation_21)
378 if rot1.size > rot2.size:
379 rotnew = _np.zeros_like(rot1)
380 else:
381 rotnew = _np.zeros_like(rot2)
383 # Multiply the two rotations (code is taken from /utility/rotation.nb).
384 rotnew[0] = (
385 rot1[0] * rot2[0] - rot1[1] * rot2[1] - rot1[2] * rot2[2] - rot1[3] * rot2[3]
386 )
387 rotnew[1] = (
388 rot1[1] * rot2[0] + rot1[0] * rot2[1] + rot1[3] * rot2[2] - rot1[2] * rot2[3]
389 )
390 rotnew[2] = (
391 rot1[2] * rot2[0] - rot1[3] * rot2[1] + rot1[0] * rot2[2] + rot1[1] * rot2[3]
392 )
393 rotnew[3] = (
394 rot1[3] * rot2[0] + rot1[2] * rot2[1] - rot1[1] * rot2[2] + rot1[0] * rot2[3]
395 )
397 return rotnew.transpose()
400def rotate_coordinates(
401 coordinates: _NDArray,
402 rotation: Rotation | _NDArray[_quaternion.quaternion],
403 *,
404 origin=None,
405):
406 """Rotate all given coordinates.
408 Args:
409 coordinates: Array of 3D coordinates to be rotated
410 rotation: The rotation(s) that will be applied to the coordinates. If
411 this is an array it has to hold a quaternion for each coordinate.
412 origin (3D vector): If this is given, the mesh is rotated about this
413 point. Defaults to (0, 0, 0).
414 """
416 if isinstance(rotation, Rotation):
417 rotation = rotation.get_quaternion().transpose()
419 # Check if origin has to be added
420 if origin is None:
421 origin = [0.0, 0.0, 0.0]
423 # New position array
424 coordinates_new = _np.zeros_like(coordinates)
426 # Evaluate the new positions using the numpy data structure
427 # (code is taken from /utility/rotation.nb)
428 rotation = rotation.transpose()
430 q0_q0 = _np.square(rotation[0])
431 q0_q1_2 = 2.0 * rotation[0] * rotation[1]
432 q0_q2_2 = 2.0 * rotation[0] * rotation[2]
433 q0_q3_2 = 2.0 * rotation[0] * rotation[3]
435 q1_q1 = _np.square(rotation[1])
436 q1_q2_2 = 2.0 * rotation[1] * rotation[2]
437 q1_q3_2 = 2.0 * rotation[1] * rotation[3]
439 q2_q2 = _np.square(rotation[2])
440 q2_q3_2 = 2.0 * rotation[2] * rotation[3]
442 q3_q3 = _np.square(rotation[3])
444 coordinates_new[:, 0] = (
445 (q0_q0 + q1_q1 - q2_q2 - q3_q3) * (coordinates[:, 0] - origin[0])
446 + (q1_q2_2 - q0_q3_2) * (coordinates[:, 1] - origin[1])
447 + (q0_q2_2 + q1_q3_2) * (coordinates[:, 2] - origin[2])
448 )
449 coordinates_new[:, 1] = (
450 (q1_q2_2 + q0_q3_2) * (coordinates[:, 0] - origin[0])
451 + (q0_q0 - q1_q1 + q2_q2 - q3_q3) * (coordinates[:, 1] - origin[1])
452 + (-q0_q1_2 + q2_q3_2) * (coordinates[:, 2] - origin[2])
453 )
454 coordinates_new[:, 2] = (
455 (-q0_q2_2 + q1_q3_2) * (coordinates[:, 0] - origin[0])
456 + (q0_q1_2 + q2_q3_2) * (coordinates[:, 1] - origin[1])
457 + (q0_q0 - q1_q1 - q2_q2 + q3_q3) * (coordinates[:, 2] - origin[2])
458 )
460 if origin is not None:
461 coordinates_new += origin
463 return coordinates_new
466def smallest_rotation(q: Rotation, t):
467 """Get the triad that results from the smallest rotation (rotation without
468 twist) from the triad q such that the rotated first basis vector aligns
469 with t. For more details see Christoph Meier's dissertation chapter 2.1.2.
471 Args
472 ----
473 q: Rotation
474 Starting triad.
475 t: Vector in R3
476 Direction of the first basis of the rotated triad.
477 Return
478 ----
479 q_sr: Rotation
480 The triad that results from a smallest rotation.
481 """
483 R_old = q.get_rotation_matrix()
484 g1_old = R_old[:, 0]
485 g1 = _np.asarray(t) / _np.linalg.norm(t)
487 # Quaternion components of relative rotation
488 q_rel = _np.zeros(4)
490 # The scalar quaternion part is cos(alpha/2) this is equal to
491 q_rel[0] = _np.linalg.norm(0.5 * (g1_old + g1))
493 # Vector part of the quaternion is sin(alpha/2)*axis
494 q_rel[1:] = _np.cross(g1_old, g1) / (2.0 * q_rel[0])
496 return Rotation.from_quaternion(q_rel) * q
499def get_rotation_vector_series(
500 rotation_vectors: list | _NDArray | list[Rotation],
501) -> _NDArray:
502 """Return an array containing the rotation vectors representing the given
503 rotation vectors.
505 The main feature of this function is, that the returned rotation
506 vectors don't have jumps when the rotation angle exceeds 2*pi. We
507 return a "continuous" series of rotation vectors that can be used to
508 interpolate between them.
510 Note: Interpolating between the returned rotation vectors will in general
511 result in a non-objective interpolation.
513 Args:
514 rotation_vectors: A list or array containing the rotation vectors, has
515 to be in order.
517 Returns:
518 An array containing the "continuous" rotation vectors.
519 """
521 if isinstance(rotation_vectors, list):
522 rotation_vector_array = _np.zeros((len(rotation_vectors), 3))
523 for i_rotation, rotation_entry in enumerate(rotation_vectors):
524 if isinstance(rotation_entry, Rotation):
525 rotation_vector_array[i_rotation] = rotation_entry.get_rotation_vector()
526 else:
527 rotation_vector_array[i_rotation] = rotation_entry
529 def closest_multiple_of_two_pi(x: float) -> float:
530 """Given the value x, return the multiple of 2*pi that is closest to
531 it."""
532 return 2.0 * _np.pi * round(x / (2 * _np.pi))
534 rotation_vectors_continuous = _np.zeros((len(rotation_vector_array), 3))
535 rotation_vectors_continuous[0, :] = rotation_vector_array[0]
537 for i, current_rotation_vector in enumerate(rotation_vector_array[1:]):
538 rotation_vector_last = rotation_vector_array[i]
539 theta = _np.linalg.norm(current_rotation_vector)
541 if theta < _bme.eps_quaternion:
542 # The current rotation is an identity rotation.
543 theta_last = _np.linalg.norm(rotation_vector_last)
544 if theta_last < _bme.eps_quaternion:
545 # The last rotation was also an identity rotation, so simply add
546 # an empty rotation vector
547 rotation_vectors_continuous[i + 1] = [0.0, 0.0, 0.0]
548 else:
549 # The last rotation was not a zero rotation. In this case we simply
550 # take the direction of the last rotation vector and set it to to the
551 # closest length which is a multiple of 2*pi.
552 rotation_vectors_continuous[i + 1] = (
553 rotation_vector_last
554 / theta_last
555 * closest_multiple_of_two_pi(theta_last)
556 )
557 else:
558 axis = current_rotation_vector / theta
559 # This is the offset from the current rotation vector to the previous one
560 # in the sense of a multiple of 2*pi.
561 multiple_of_two_pi = closest_multiple_of_two_pi(
562 (_np.dot(axis, rotation_vector_last) - theta)
563 )
564 rotation_vectors_continuous[i + 1] = (multiple_of_two_pi + theta) * axis
565 return rotation_vectors_continuous