Coverage for src/beamme/core/rotation.py: 96%
195 statements
« prev ^ index » next coverage.py v7.9.1, created at 2025-06-30 18:48 +0000
1# The MIT License (MIT)
2#
3# Copyright (c) 2018-2025 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
27from beamme.core.conf import mpy as _mpy
30def skew_matrix(vector):
31 """Return the skew matrix for the vector."""
32 skew = _np.zeros([3, 3])
33 skew[0, 1] = -vector[2]
34 skew[0, 2] = vector[1]
35 skew[1, 0] = vector[2]
36 skew[1, 2] = -vector[0]
37 skew[2, 0] = -vector[1]
38 skew[2, 1] = vector[0]
39 return skew
42class Rotation:
43 """A class that represents a rotation of a coordinate system.
45 Internally the rotations are stored as quaternions.
46 """
48 def __init__(self, *args):
49 """Initialize the rotation object.
51 Args
52 ----
53 *args:
54 - Rotation()
55 Create a identity rotation.
56 - Rotation(axis, phi)
57 Create a rotation around the vector axis with the angle phi.
58 """
60 self.q = _np.zeros(4)
62 if len(args) == 0:
63 # Identity element.
64 self.q[0] = 1
65 elif len(args) == 2:
66 # Set from rotation axis and rotation angle.
67 axis = _np.asarray(args[0])
68 phi = args[1]
69 norm = _np.linalg.norm(axis)
70 if norm < _mpy.eps_quaternion:
71 raise ValueError("The rotation axis can not be a zero vector!")
72 self.q[0] = _np.cos(0.5 * phi)
73 self.q[1:] = _np.sin(0.5 * phi) * axis / norm
74 else:
75 raise ValueError(f"The given arguments {args} are invalid!")
77 @classmethod
78 def from_quaternion(cls, q, *, normalized=False):
79 """Create the object from a quaternion float array (4x1)
81 Args
82 ----
83 q: Quaternion, q0, qx,qy,qz
84 normalized: Flag if the input quaternion is normalized. If so, no
85 normalization is performed which can potentially improve performance.
86 Skipping the normalization should only be done in very special cases
87 where we can be sure that the input quaternion is normalized to avoid
88 error accumulation.
89 """
90 if isinstance(q, _quaternion.quaternion):
91 q = _quaternion.as_float_array(q)
93 rotation = object.__new__(cls)
94 if normalized:
95 rotation.q = _np.array(q)
96 else:
97 rotation.q = _np.asarray(q) / _np.linalg.norm(q)
98 if (not rotation.q.ndim == 1) or (not len(rotation.q) == 4):
99 raise ValueError("Got quaternion array with unexpected dimensions")
100 return rotation
102 @classmethod
103 def from_rotation_matrix(cls, R):
104 """Create the object from a rotation matrix.
106 The code is based on Spurriers algorithm:
107 R. A. Spurrier (1978): “Comment on “Singularity-free extraction of a quaternion from a
108 direction-cosine matrix”
109 """
111 q = _np.zeros(4)
112 trace = _np.trace(R)
113 values = [R[i, i] for i in range(3)]
114 values.append(trace)
115 arg_max = _np.argmax(values)
116 if arg_max == 3:
117 q[0] = _np.sqrt(trace + 1) * 0.5
118 q[1] = (R[2, 1] - R[1, 2]) / (4 * q[0])
119 q[2] = (R[0, 2] - R[2, 0]) / (4 * q[0])
120 q[3] = (R[1, 0] - R[0, 1]) / (4 * q[0])
121 else:
122 i_index = arg_max
123 j_index = (i_index + 1) % 3
124 k_index = (i_index + 2) % 3
125 q_i = _np.sqrt(R[i_index, i_index] * 0.5 + (1 - trace) * 0.25)
126 q[0] = (R[k_index, j_index] - R[j_index, k_index]) / (4 * q_i)
127 q[i_index + 1] = q_i
128 q[j_index + 1] = (R[j_index, i_index] + R[i_index, j_index]) / (4 * q_i)
129 q[k_index + 1] = (R[k_index, i_index] + R[i_index, k_index]) / (4 * q_i)
131 return cls.from_quaternion(q)
133 @classmethod
134 def from_basis(cls, t1, t2):
135 """Create the object from two basis vectors t1, t2.
137 t2 will be orthogonalized on t1, and t3 will be calculated with
138 the cross product.
139 """
141 t1_normal = t1 / _np.linalg.norm(t1)
142 t2_ortho = t2 - t1_normal * _np.dot(t1_normal, t2)
143 t2_normal = t2_ortho / _np.linalg.norm(t2_ortho)
144 t3_normal = _np.cross(t1_normal, t2_normal)
146 R = _np.transpose([t1_normal, t2_normal, t3_normal])
147 return cls.from_rotation_matrix(R)
149 @classmethod
150 def from_rotation_vector(cls, rotation_vector):
151 """Create the object from a rotation vector."""
153 q = _np.zeros(4)
154 rotation_vector = _np.asarray(rotation_vector)
155 phi = _np.linalg.norm(rotation_vector)
156 q[0] = _np.cos(0.5 * phi)
157 if phi < _mpy.eps_quaternion:
158 # This is the Taylor series expansion of sin(phi/2)/phi around phi=0
159 q[1:] = 0.5 * rotation_vector
160 else:
161 q[1:] = _np.sin(0.5 * phi) / phi * rotation_vector
162 return cls.from_quaternion(q)
164 def check(self):
165 """Perform all checks for the rotation."""
166 self.check_uniqueness()
167 self.check_quaternion_constraint()
169 def check_uniqueness(self):
170 """We always want q0 to be positive -> the range for the rotational
171 angle is 0 <= phi <= pi."""
173 if self.q[0] < 0:
174 self.q *= -1
176 def check_quaternion_constraint(self):
177 """We want to check that q.q = 1."""
179 if _np.abs(1 - _np.linalg.norm(self.q)) > _mpy.eps_quaternion:
180 raise ValueError(
181 f"The rotation object is corrupted. q.q does not equal 1! q={self.q}"
182 )
184 def get_rotation_matrix(self):
185 """Return the rotation matrix for this rotation.
187 (Krenk (3.50))
188 """
189 q_skew = skew_matrix(self.q[1:])
190 R = (
191 (self.q[0] ** 2 - _np.dot(self.q[1:], self.q[1:])) * _np.eye(3)
192 + 2 * self.q[0] * q_skew
193 + 2 * ([self.q[1:]] * _np.transpose([self.q[1:]]))
194 )
196 return R
198 def get_quaternion(self):
199 """Return the quaternion for this rotation, as numpy array (copy)."""
200 return _np.array(self.q)
202 def get_numpy_quaternion(self):
203 """Return a numpy quaternion object representing this rotation
204 (copy)."""
205 return _quaternion.from_float_array(_np.array(self.q))
207 def get_rotation_vector(self):
208 """Return the rotation vector for this object."""
210 self.check()
212 norm = _np.linalg.norm(self.q[1:])
213 phi = 2 * _np.arctan2(norm, self.q[0])
215 if phi < _mpy.eps_quaternion:
216 # For small angles return the Taylor series expansion of phi/sin(phi/2)
217 scale_factor = 2
218 else:
219 scale_factor = phi / _np.sin(phi / 2)
220 if _np.abs(_np.abs(phi) - _np.pi) < _mpy.eps_quaternion:
221 # For rotations of exactly +-pi, numerical issues might occur, resulting in
222 # a rotation vector that is non-deterministic. The result is correct, but
223 # the sign can switch due to different implementation of basic underlying
224 # math functions. This is especially triggered when using this code with
225 # different OS. To avoid this, we scale the rotation axis in such a way,
226 # for a rotation angle of +-pi, the first component of the rotation axis
227 # that is not 0 is positive.
228 for i_dir in range(3):
229 if _np.abs(self.q[1 + i_dir]) > _mpy.eps_quaternion:
230 if self.q[1 + i_dir] < 0:
231 scale_factor *= -1
232 break
233 return self.q[1:] * scale_factor
235 def get_transformation_matrix(self):
236 """Return the transformation matrix for this rotation.
238 The transformation matrix maps the (infinitesimal)
239 multiplicative rotational increments onto the additive ones.
240 """
242 omega = self.get_rotation_vector()
243 omega_norm = _np.linalg.norm(omega)
245 # We have to take the inverse of the the rotation angle here, therefore,
246 # we have a branch for small angles where the singularity is not present.
247 if omega_norm**2 > _mpy.eps_quaternion:
248 # Taken from Jelenic and Crisfield (1999) Equation (2.5)
249 omega_dir = omega / omega_norm
250 omega_skew = skew_matrix(omega)
251 transformation_matrix = (
252 _np.outer(omega_dir, omega_dir)
253 - 0.5 * omega_skew
254 + 0.5
255 * omega_norm
256 / _np.tan(0.5 * omega_norm)
257 * (_np.identity(3) - _np.outer(omega_dir, omega_dir))
258 )
259 else:
260 # This is the constant part of the Taylor series expansion. If this
261 # function is used with automatic differentiation, higher order
262 # terms have to be added!
263 transformation_matrix = _np.identity(3)
264 return transformation_matrix
266 def get_transformation_matrix_inv(self):
267 """Return the inverse of the transformation matrix for this rotation.
269 The inverse of the transformation matrix maps the
270 (infinitesimal) additive rotational increments onto the
271 multiplicative ones.
272 """
274 omega = self.get_rotation_vector()
275 omega_norm = _np.linalg.norm(omega)
277 # We have to take the inverse of the the rotation angle here, therefore,
278 # we have a branch for small angles where the singularity is not present.
279 if omega_norm**2 > _mpy.eps_quaternion:
280 # Taken from Jelenic and Crisfield (1999) Equation (2.5)
281 omega_dir = omega / omega_norm
282 omega_skew = skew_matrix(omega)
283 transformation_matrix_inverse = (
284 (1.0 - _np.sin(omega_norm) / omega_norm)
285 * _np.outer(omega_dir, omega_dir)
286 + _np.sin(omega_norm) / omega_norm * _np.identity(3)
287 + (1.0 - _np.cos(omega_norm)) / omega_norm**2 * omega_skew
288 )
289 else:
290 # This is the constant part of the Taylor series expansion. If this
291 # function is used with automatic differentiation, higher order
292 # terms have to be added!
293 transformation_matrix_inverse = _np.identity(3)
294 return transformation_matrix_inverse
296 def inv(self):
297 """Return the inverse of this rotation."""
299 tmp_quaternion = self.q.copy()
300 tmp_quaternion[0] *= -1.0
301 return Rotation.from_quaternion(tmp_quaternion)
303 def __mul__(self, other):
304 """Add this rotation to another, or apply it on a vector."""
306 # Check if the other object is also a rotation.
307 if isinstance(other, Rotation):
308 # Get quaternions of the two objects.
309 p = self.q
310 q = other.q
311 # Add the rotations.
312 added_rotation = _np.zeros_like(self.q)
313 added_rotation[0] = p[0] * q[0] - _np.dot(p[1:], q[1:])
314 added_rotation[1:] = p[0] * q[1:] + q[0] * p[1:] + _np.cross(p[1:], q[1:])
315 return Rotation.from_quaternion(added_rotation)
316 elif isinstance(other, (list, _np.ndarray)) and len(other) == 3:
317 # Apply rotation to vector.
318 return _np.dot(self.get_rotation_matrix(), _np.asarray(other))
319 raise NotImplementedError("Error, not implemented, does not make sense anyway!")
321 def __eq__(self, other):
322 """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) < _mpy.eps_quaternion)
327 or (_np.linalg.norm(self.q + other.q) < _mpy.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."""
339 self.check()
340 return f"Rotation:\n q0: {self.q[0]}\n q: {self.q[1:]}"
343def add_rotations(rotation_21, rotation_10):
344 """Multiply a rotation onto another.
346 Args
347 ----
348 rotation_10: _np.ndarray
349 Array with the dimensions n x 4 or 4 x 1.
350 The first rotation that is applied.
351 rotation_21: _np.ndarray
352 Array with the dimensions n x 4 or 4 x 1.
353 The second rotation that is applied.
355 Return
356 ----
357 rot_new: _np.ndarray
358 Array with the dimensions n x 4.
359 This array contains the new quaternions.
360 """
362 # Transpose the arrays, to work with the following code.
363 if isinstance(rotation_10, Rotation):
364 rot1 = rotation_10.get_quaternion().transpose()
365 else:
366 rot1 = _np.transpose(rotation_10)
367 if isinstance(rotation_21, Rotation):
368 rot2 = rotation_21.get_quaternion().transpose()
369 else:
370 rot2 = _np.transpose(rotation_21)
372 if rot1.size > rot2.size:
373 rotnew = _np.zeros_like(rot1)
374 else:
375 rotnew = _np.zeros_like(rot2)
377 # Multiply the two rotations (code is taken from /utility/rotation.nb).
378 rotnew[0] = (
379 rot1[0] * rot2[0] - rot1[1] * rot2[1] - rot1[2] * rot2[2] - rot1[3] * rot2[3]
380 )
381 rotnew[1] = (
382 rot1[1] * rot2[0] + rot1[0] * rot2[1] + rot1[3] * rot2[2] - rot1[2] * rot2[3]
383 )
384 rotnew[2] = (
385 rot1[2] * rot2[0] - rot1[3] * rot2[1] + rot1[0] * rot2[2] + rot1[1] * rot2[3]
386 )
387 rotnew[3] = (
388 rot1[3] * rot2[0] + rot1[2] * rot2[1] - rot1[1] * rot2[2] + rot1[0] * rot2[3]
389 )
391 return rotnew.transpose()
394def rotate_coordinates(coordinates, rotation, *, origin=None):
395 """Rotate all given coordinates.
397 Args
398 ----
399 coordinates: _np.array
400 Array of 3D coordinates to be rotated
401 rotation: Rotation, list(quaternions) (nx4)
402 The rotation that will be applied to the coordinates. Can also be an
403 array with a quaternion for each coordinate.
404 origin: 3D vector
405 If this is given, the mesh is rotated about this point. Default is
406 (0,0,0)
407 """
409 if isinstance(rotation, Rotation):
410 rotation = rotation.get_quaternion().transpose()
412 # Check if origin has to be added
413 if origin is None:
414 origin = [0.0, 0.0, 0.0]
416 # New position array
417 coordinates_new = _np.zeros_like(coordinates)
419 # Evaluate the new positions using the numpy data structure
420 # (code is taken from /utility/rotation.nb)
421 rotation = rotation.transpose()
423 q0_q0 = _np.square(rotation[0])
424 q0_q1_2 = 2.0 * rotation[0] * rotation[1]
425 q0_q2_2 = 2.0 * rotation[0] * rotation[2]
426 q0_q3_2 = 2.0 * rotation[0] * rotation[3]
428 q1_q1 = _np.square(rotation[1])
429 q1_q2_2 = 2.0 * rotation[1] * rotation[2]
430 q1_q3_2 = 2.0 * rotation[1] * rotation[3]
432 q2_q2 = _np.square(rotation[2])
433 q2_q3_2 = 2.0 * rotation[2] * rotation[3]
435 q3_q3 = _np.square(rotation[3])
437 coordinates_new[:, 0] = (
438 (q0_q0 + q1_q1 - q2_q2 - q3_q3) * (coordinates[:, 0] - origin[0])
439 + (q1_q2_2 - q0_q3_2) * (coordinates[:, 1] - origin[1])
440 + (q0_q2_2 + q1_q3_2) * (coordinates[:, 2] - origin[2])
441 )
442 coordinates_new[:, 1] = (
443 (q1_q2_2 + q0_q3_2) * (coordinates[:, 0] - origin[0])
444 + (q0_q0 - q1_q1 + q2_q2 - q3_q3) * (coordinates[:, 1] - origin[1])
445 + (-q0_q1_2 + q2_q3_2) * (coordinates[:, 2] - origin[2])
446 )
447 coordinates_new[:, 2] = (
448 (-q0_q2_2 + q1_q3_2) * (coordinates[:, 0] - origin[0])
449 + (q0_q1_2 + q2_q3_2) * (coordinates[:, 1] - origin[1])
450 + (q0_q0 - q1_q1 - q2_q2 + q3_q3) * (coordinates[:, 2] - origin[2])
451 )
453 if origin is not None:
454 coordinates_new += origin
456 return coordinates_new
459def smallest_rotation(q: Rotation, t):
460 """Get the triad that results from the smallest rotation (rotation without
461 twist) from the triad q such that the rotated first basis vector aligns
462 with t. For more details see Christoph Meier's dissertation chapter 2.1.2.
464 Args
465 ----
466 q: Rotation
467 Starting triad.
468 t: Vector in R3
469 Direction of the first basis of the rotated triad.
470 Return
471 ----
472 q_sr: Rotation
473 The triad that results from a smallest rotation.
474 """
476 R_old = q.get_rotation_matrix()
477 g1_old = R_old[:, 0]
478 g1 = _np.asarray(t) / _np.linalg.norm(t)
480 # Quaternion components of relative rotation
481 q_rel = _np.zeros(4)
483 # The scalar quaternion part is cos(alpha/2) this is equal to
484 q_rel[0] = _np.linalg.norm(0.5 * (g1_old + g1))
486 # Vector part of the quaternion is sin(alpha/2)*axis
487 q_rel[1:] = _np.cross(g1_old, g1) / (2.0 * q_rel[0])
489 return Rotation.from_quaternion(q_rel) * q