Coverage for src/beamme/core/rotation.py: 96%

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.""" 

23 

24import numpy as _np 

25import quaternion as _quaternion 

26from numpy.typing import NDArray as _NDArray 

27 

28from beamme.core.conf import bme as _bme 

29 

30 

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 

41 

42 

43class Rotation: 

44 """A class that represents a rotation of a coordinate system. 

45 

46 Internally the rotations are stored as quaternions. 

47 """ 

48 

49 def __init__(self, *args): 

50 """Initialize the rotation object. 

51 

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 """ 

60 

61 self.q = _np.zeros(4) 

62 

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!") 

77 

78 @classmethod 

79 def from_quaternion(cls, q, *, normalized=False): 

80 """Create the object from a quaternion float array (4x1) 

81 

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) 

93 

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 

102 

103 @classmethod 

104 def from_rotation_matrix(cls, R): 

105 """Create the object from a rotation matrix. 

106 

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 """ 

111 

112 q = _np.zeros(4) 

113 trace = _np.trace(R) 

114 values = [R[i, i] for i in range(3)] 

115 values.append(trace) 

116 arg_max = _np.argmax(values) 

117 if arg_max == 3: 

118 q[0] = _np.sqrt(trace + 1) * 0.5 

119 q[1] = (R[2, 1] - R[1, 2]) / (4 * q[0]) 

120 q[2] = (R[0, 2] - R[2, 0]) / (4 * q[0]) 

121 q[3] = (R[1, 0] - R[0, 1]) / (4 * q[0]) 

122 else: 

123 i_index = arg_max 

124 j_index = (i_index + 1) % 3 

125 k_index = (i_index + 2) % 3 

126 q_i = _np.sqrt(R[i_index, i_index] * 0.5 + (1 - trace) * 0.25) 

127 q[0] = (R[k_index, j_index] - R[j_index, k_index]) / (4 * q_i) 

128 q[i_index + 1] = q_i 

129 q[j_index + 1] = (R[j_index, i_index] + R[i_index, j_index]) / (4 * q_i) 

130 q[k_index + 1] = (R[k_index, i_index] + R[i_index, k_index]) / (4 * q_i) 

131 

132 return cls.from_quaternion(q) 

133 

134 @classmethod 

135 def from_basis(cls, t1, t2): 

136 """Create the object from two basis vectors t1, t2. 

137 

138 t2 will be orthogonalized on t1, and t3 will be calculated with 

139 the cross product. 

140 """ 

141 

142 t1_normal = t1 / _np.linalg.norm(t1) 

143 t2_ortho = t2 - t1_normal * _np.dot(t1_normal, t2) 

144 t2_normal = t2_ortho / _np.linalg.norm(t2_ortho) 

145 t3_normal = _np.cross(t1_normal, t2_normal) 

146 

147 R = _np.transpose([t1_normal, t2_normal, t3_normal]) 

148 return cls.from_rotation_matrix(R) 

149 

150 @classmethod 

151 def from_rotation_vector(cls, rotation_vector): 

152 """Create the object from a rotation vector.""" 

153 

154 q = _np.zeros(4) 

155 rotation_vector = _np.asarray(rotation_vector) 

156 phi = _np.linalg.norm(rotation_vector) 

157 q[0] = _np.cos(0.5 * phi) 

158 if phi < _bme.eps_quaternion: 

159 # This is the Taylor series expansion of sin(phi/2)/phi around phi=0 

160 q[1:] = 0.5 * rotation_vector 

161 else: 

162 q[1:] = _np.sin(0.5 * phi) / phi * rotation_vector 

163 return cls.from_quaternion(q) 

164 

165 def check(self): 

166 """Perform all checks for the rotation.""" 

167 self.check_uniqueness() 

168 self.check_quaternion_constraint() 

169 

170 def check_uniqueness(self): 

171 """We always want q0 to be positive -> the range for the rotational 

172 angle is 0 <= phi <= pi.""" 

173 

174 if self.q[0] < 0: 

175 self.q *= -1 

176 

177 def check_quaternion_constraint(self): 

178 """We want to check that q.q = 1.""" 

179 

180 if _np.abs(1 - _np.linalg.norm(self.q)) > _bme.eps_quaternion: 

181 raise ValueError( 

182 f"The rotation object is corrupted. q.q does not equal 1! q={self.q}" 

183 ) 

184 

185 def get_rotation_matrix(self): 

186 """Return the rotation matrix for this rotation. 

187 

188 (Krenk (3.50)) 

189 """ 

190 q_skew = skew_matrix(self.q[1:]) 

191 R = ( 

192 (self.q[0] ** 2 - _np.dot(self.q[1:], self.q[1:])) * _np.eye(3) 

193 + 2 * self.q[0] * q_skew 

194 + 2 * ([self.q[1:]] * _np.transpose([self.q[1:]])) 

195 ) 

196 

197 return R 

198 

199 def get_quaternion(self): 

200 """Return the quaternion for this rotation, as numpy array (copy).""" 

201 return _np.array(self.q) 

202 

203 def get_numpy_quaternion(self): 

204 """Return a numpy quaternion object representing this rotation 

205 (copy).""" 

206 return _quaternion.from_float_array(_np.array(self.q)) 

207 

208 def get_rotation_vector(self): 

209 """Return the rotation vector for this object.""" 

210 

211 self.check() 

212 

213 norm = _np.linalg.norm(self.q[1:]) 

214 phi = 2 * _np.arctan2(norm, self.q[0]) 

215 

216 if phi < _bme.eps_quaternion: 

217 # For small angles return the Taylor series expansion of phi/sin(phi/2) 

218 scale_factor = 2 

219 else: 

220 scale_factor = phi / _np.sin(phi / 2) 

221 if _np.abs(_np.abs(phi) - _np.pi) < _bme.eps_quaternion: 

222 # For rotations of exactly +-pi, numerical issues might occur, resulting in 

223 # a rotation vector that is non-deterministic. The result is correct, but 

224 # the sign can switch due to different implementation of basic underlying 

225 # math functions. This is especially triggered when using this code with 

226 # different OS. To avoid this, we scale the rotation axis in such a way, 

227 # for a rotation angle of +-pi, the first component of the rotation axis 

228 # that is not 0 is positive. 

229 for i_dir in range(3): 

230 if _np.abs(self.q[1 + i_dir]) > _bme.eps_quaternion: 

231 if self.q[1 + i_dir] < 0: 

232 scale_factor *= -1 

233 break 

234 return self.q[1:] * scale_factor 

235 

236 def get_transformation_matrix(self): 

237 """Return the transformation matrix for this rotation. 

238 

239 The transformation matrix maps the (infinitesimal) 

240 multiplicative rotational increments onto the additive ones. 

241 """ 

242 

243 omega = self.get_rotation_vector() 

244 omega_norm = _np.linalg.norm(omega) 

245 

246 # We have to take the inverse of the the rotation angle here, therefore, 

247 # we have a branch for small angles where the singularity is not present. 

248 if omega_norm**2 > _bme.eps_quaternion: 

249 # Taken from Jelenic and Crisfield (1999) Equation (2.5) 

250 omega_dir = omega / omega_norm 

251 omega_skew = skew_matrix(omega) 

252 transformation_matrix = ( 

253 _np.outer(omega_dir, omega_dir) 

254 - 0.5 * omega_skew 

255 + 0.5 

256 * omega_norm 

257 / _np.tan(0.5 * omega_norm) 

258 * (_np.identity(3) - _np.outer(omega_dir, omega_dir)) 

259 ) 

260 else: 

261 # This is the constant part of the Taylor series expansion. If this 

262 # function is used with automatic differentiation, higher order 

263 # terms have to be added! 

264 transformation_matrix = _np.identity(3) 

265 return transformation_matrix 

266 

267 def get_transformation_matrix_inv(self): 

268 """Return the inverse of the transformation matrix for this rotation. 

269 

270 The inverse of the transformation matrix maps the 

271 (infinitesimal) additive rotational increments onto the 

272 multiplicative ones. 

273 """ 

274 

275 omega = self.get_rotation_vector() 

276 omega_norm = _np.linalg.norm(omega) 

277 

278 # We have to take the inverse of the the rotation angle here, therefore, 

279 # we have a branch for small angles where the singularity is not present. 

280 if omega_norm**2 > _bme.eps_quaternion: 

281 # Taken from Jelenic and Crisfield (1999) Equation (2.5) 

282 omega_dir = omega / omega_norm 

283 omega_skew = skew_matrix(omega) 

284 transformation_matrix_inverse = ( 

285 (1.0 - _np.sin(omega_norm) / omega_norm) 

286 * _np.outer(omega_dir, omega_dir) 

287 + _np.sin(omega_norm) / omega_norm * _np.identity(3) 

288 + (1.0 - _np.cos(omega_norm)) / omega_norm**2 * omega_skew 

289 ) 

290 else: 

291 # This is the constant part of the Taylor series expansion. If this 

292 # function is used with automatic differentiation, higher order 

293 # terms have to be added! 

294 transformation_matrix_inverse = _np.identity(3) 

295 return transformation_matrix_inverse 

296 

297 def inv(self): 

298 """Return the inverse of this rotation.""" 

299 

300 tmp_quaternion = self.q.copy() 

301 tmp_quaternion[0] *= -1.0 

302 return Rotation.from_quaternion(tmp_quaternion) 

303 

304 def __mul__(self, other): 

305 """Add this rotation to another, or apply it on a vector.""" 

306 

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!") 

321 

322 def __eq__(self, other): 

323 """Check if the other rotation is equal to this one.""" 

324 

325 if isinstance(other, Rotation): 

326 return bool( 

327 (_np.linalg.norm(self.q - other.q) < _bme.eps_quaternion) 

328 or (_np.linalg.norm(self.q + other.q) < _bme.eps_quaternion) 

329 ) 

330 else: 

331 return object.__eq__(self, other) 

332 

333 def copy(self): 

334 """Return a copy of this object.""" 

335 return Rotation.from_quaternion(self.q, normalized=True) 

336 

337 def __str__(self): 

338 """String representation of object.""" 

339 

340 self.check() 

341 return f"Rotation:\n q0: {self.q[0]}\n q: {self.q[1:]}" 

342 

343 

344def add_rotations( 

345 rotation_21: Rotation | _NDArray[_quaternion.quaternion], 

346 rotation_10: Rotation | _NDArray[_quaternion.quaternion], 

347) -> _NDArray[_quaternion.quaternion]: 

348 """Multiply rotations onto another. 

349 

350 Args: 

351 rotation_10: The first rotation(s) that are applied. 

352 rotation_21: The second rotation(s) that are applied. 

353 

354 Returns: 

355 An array with the compound quaternions. 

356 """ 

357 

358 # Transpose the arrays, to work with the following code. 

359 if isinstance(rotation_10, Rotation): 

360 rot1 = rotation_10.get_quaternion().transpose() 

361 else: 

362 rot1 = _np.transpose(rotation_10) 

363 if isinstance(rotation_21, Rotation): 

364 rot2 = rotation_21.get_quaternion().transpose() 

365 else: 

366 rot2 = _np.transpose(rotation_21) 

367 

368 if rot1.size > rot2.size: 

369 rotnew = _np.zeros_like(rot1) 

370 else: 

371 rotnew = _np.zeros_like(rot2) 

372 

373 # Multiply the two rotations (code is taken from /utility/rotation.nb). 

374 rotnew[0] = ( 

375 rot1[0] * rot2[0] - rot1[1] * rot2[1] - rot1[2] * rot2[2] - rot1[3] * rot2[3] 

376 ) 

377 rotnew[1] = ( 

378 rot1[1] * rot2[0] + rot1[0] * rot2[1] + rot1[3] * rot2[2] - rot1[2] * rot2[3] 

379 ) 

380 rotnew[2] = ( 

381 rot1[2] * rot2[0] - rot1[3] * rot2[1] + rot1[0] * rot2[2] + rot1[1] * rot2[3] 

382 ) 

383 rotnew[3] = ( 

384 rot1[3] * rot2[0] + rot1[2] * rot2[1] - rot1[1] * rot2[2] + rot1[0] * rot2[3] 

385 ) 

386 

387 return rotnew.transpose() 

388 

389 

390def rotate_coordinates( 

391 coordinates: _NDArray, 

392 rotation: Rotation | _NDArray[_quaternion.quaternion], 

393 *, 

394 origin=None, 

395): 

396 """Rotate all given coordinates. 

397 

398 Args: 

399 coordinates: Array of 3D coordinates to be rotated 

400 rotation: The rotation(s) that will be applied to the coordinates. If 

401 this is an array it has to hold a quaternion for each coordinate. 

402 origin (3D vector): If this is given, the mesh is rotated about this 

403 point. Defaults to (0, 0, 0). 

404 """ 

405 

406 if isinstance(rotation, Rotation): 

407 rotation = rotation.get_quaternion().transpose() 

408 

409 # Check if origin has to be added 

410 if origin is None: 

411 origin = [0.0, 0.0, 0.0] 

412 

413 # New position array 

414 coordinates_new = _np.zeros_like(coordinates) 

415 

416 # Evaluate the new positions using the numpy data structure 

417 # (code is taken from /utility/rotation.nb) 

418 rotation = rotation.transpose() 

419 

420 q0_q0 = _np.square(rotation[0]) 

421 q0_q1_2 = 2.0 * rotation[0] * rotation[1] 

422 q0_q2_2 = 2.0 * rotation[0] * rotation[2] 

423 q0_q3_2 = 2.0 * rotation[0] * rotation[3] 

424 

425 q1_q1 = _np.square(rotation[1]) 

426 q1_q2_2 = 2.0 * rotation[1] * rotation[2] 

427 q1_q3_2 = 2.0 * rotation[1] * rotation[3] 

428 

429 q2_q2 = _np.square(rotation[2]) 

430 q2_q3_2 = 2.0 * rotation[2] * rotation[3] 

431 

432 q3_q3 = _np.square(rotation[3]) 

433 

434 coordinates_new[:, 0] = ( 

435 (q0_q0 + q1_q1 - q2_q2 - q3_q3) * (coordinates[:, 0] - origin[0]) 

436 + (q1_q2_2 - q0_q3_2) * (coordinates[:, 1] - origin[1]) 

437 + (q0_q2_2 + q1_q3_2) * (coordinates[:, 2] - origin[2]) 

438 ) 

439 coordinates_new[:, 1] = ( 

440 (q1_q2_2 + q0_q3_2) * (coordinates[:, 0] - origin[0]) 

441 + (q0_q0 - q1_q1 + q2_q2 - q3_q3) * (coordinates[:, 1] - origin[1]) 

442 + (-q0_q1_2 + q2_q3_2) * (coordinates[:, 2] - origin[2]) 

443 ) 

444 coordinates_new[:, 2] = ( 

445 (-q0_q2_2 + q1_q3_2) * (coordinates[:, 0] - origin[0]) 

446 + (q0_q1_2 + q2_q3_2) * (coordinates[:, 1] - origin[1]) 

447 + (q0_q0 - q1_q1 - q2_q2 + q3_q3) * (coordinates[:, 2] - origin[2]) 

448 ) 

449 

450 if origin is not None: 

451 coordinates_new += origin 

452 

453 return coordinates_new 

454 

455 

456def smallest_rotation(q: Rotation, t): 

457 """Get the triad that results from the smallest rotation (rotation without 

458 twist) from the triad q such that the rotated first basis vector aligns 

459 with t. For more details see Christoph Meier's dissertation chapter 2.1.2. 

460 

461 Args 

462 ---- 

463 q: Rotation 

464 Starting triad. 

465 t: Vector in R3 

466 Direction of the first basis of the rotated triad. 

467 Return 

468 ---- 

469 q_sr: Rotation 

470 The triad that results from a smallest rotation. 

471 """ 

472 

473 R_old = q.get_rotation_matrix() 

474 g1_old = R_old[:, 0] 

475 g1 = _np.asarray(t) / _np.linalg.norm(t) 

476 

477 # Quaternion components of relative rotation 

478 q_rel = _np.zeros(4) 

479 

480 # The scalar quaternion part is cos(alpha/2) this is equal to 

481 q_rel[0] = _np.linalg.norm(0.5 * (g1_old + g1)) 

482 

483 # Vector part of the quaternion is sin(alpha/2)*axis 

484 q_rel[1:] = _np.cross(g1_old, g1) / (2.0 * q_rel[0]) 

485 

486 return Rotation.from_quaternion(q_rel) * q