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

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

23 

24import copy as _copy 

25 

26import numpy as _np 

27import quaternion as _quaternion 

28 

29from meshpy.core.conf import mpy as _mpy 

30 

31 

32def skew_matrix(vector): 

33 """Return the skew matrix for the vector.""" 

34 skew = _np.zeros([3, 3]) 

35 skew[0, 1] = -vector[2] 

36 skew[0, 2] = vector[1] 

37 skew[1, 0] = vector[2] 

38 skew[1, 2] = -vector[0] 

39 skew[2, 0] = -vector[1] 

40 skew[2, 1] = vector[0] 

41 return skew 

42 

43 

44class Rotation: 

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

46 

47 Internally the rotations are stored as quaternions. 

48 """ 

49 

50 def __init__(self, *args): 

51 """Initialize the rotation object. 

52 

53 Args 

54 ---- 

55 *args: 

56 - Rotation() 

57 Create a identity rotation. 

58 - Rotation(axis, phi) 

59 Create a rotation around the vector axis with the angle phi. 

60 """ 

61 

62 self.q = _np.zeros(4) 

63 

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

78 

79 @classmethod 

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

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

82 

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) 

94 

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 

103 

104 @classmethod 

105 def from_rotation_matrix(cls, R): 

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

107 

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 

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) 

132 

133 return cls.from_quaternion(q) 

134 

135 @classmethod 

136 def from_basis(cls, t1, t2): 

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

138 

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

140 the cross product. 

141 """ 

142 

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

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

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

146 t3_normal = _np.cross(t1_normal, t2_normal) 

147 

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

149 return cls.from_rotation_matrix(R) 

150 

151 @classmethod 

152 def from_rotation_vector(cls, rotation_vector): 

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

154 

155 q = _np.zeros(4) 

156 rotation_vector = _np.asarray(rotation_vector) 

157 phi = _np.linalg.norm(rotation_vector) 

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

159 if phi < _mpy.eps_quaternion: 

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

161 q[1:] = 0.5 * rotation_vector 

162 else: 

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

164 return cls.from_quaternion(q) 

165 

166 def check(self): 

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

168 self.check_uniqueness() 

169 self.check_quaternion_constraint() 

170 

171 def check_uniqueness(self): 

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

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

174 

175 if self.q[0] < 0: 

176 self.q *= -1 

177 

178 def check_quaternion_constraint(self): 

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

180 

181 if _np.abs(1 - _np.linalg.norm(self.q)) > _mpy.eps_quaternion: 

182 raise ValueError( 

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

184 ) 

185 

186 def get_rotation_matrix(self): 

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

188 

189 (Krenk (3.50)) 

190 """ 

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

192 R = ( 

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

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

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

196 ) 

197 

198 return R 

199 

200 def get_quaternion(self): 

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

202 return _np.array(self.q) 

203 

204 def get_numpy_quaternion(self): 

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

206 (copy).""" 

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

208 

209 def get_rotation_vector(self): 

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

211 

212 self.check() 

213 

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

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

216 

217 if phi < _mpy.eps_quaternion: 

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

219 scale_factor = 2 

220 else: 

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

222 if _np.abs(_np.abs(phi) - _np.pi) < _mpy.eps_quaternion: 

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

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

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

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

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

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

229 # that is not 0 is positive. 

230 for i_dir in range(3): 

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

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

233 scale_factor *= -1 

234 break 

235 return self.q[1:] * scale_factor 

236 

237 def get_transformation_matrix(self): 

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

239 

240 The transformation matrix maps the (infinitesimal) 

241 multiplicative rotational increments onto the additive ones. 

242 """ 

243 

244 omega = self.get_rotation_vector() 

245 omega_norm = _np.linalg.norm(omega) 

246 

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

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

249 if omega_norm**2 > _mpy.eps_quaternion: 

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

251 omega_dir = omega / omega_norm 

252 omega_skew = skew_matrix(omega) 

253 transformation_matrix = ( 

254 _np.outer(omega_dir, omega_dir) 

255 - 0.5 * omega_skew 

256 + 0.5 

257 * omega_norm 

258 / _np.tan(0.5 * omega_norm) 

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

260 ) 

261 else: 

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

263 # function is used with automatic differentiation, higher order 

264 # terms have to be added! 

265 transformation_matrix = _np.identity(3) 

266 return transformation_matrix 

267 

268 def get_transformation_matrix_inv(self): 

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

270 

271 The inverse of the transformation matrix maps the 

272 (infinitesimal) additive rotational increments onto the 

273 multiplicative ones. 

274 """ 

275 

276 omega = self.get_rotation_vector() 

277 omega_norm = _np.linalg.norm(omega) 

278 

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

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

281 if omega_norm**2 > _mpy.eps_quaternion: 

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

283 omega_dir = omega / omega_norm 

284 omega_skew = skew_matrix(omega) 

285 transformation_matrix_inverse = ( 

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

287 * _np.outer(omega_dir, omega_dir) 

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

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

290 ) 

291 else: 

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

293 # function is used with automatic differentiation, higher order 

294 # terms have to be added! 

295 transformation_matrix_inverse = _np.identity(3) 

296 return transformation_matrix_inverse 

297 

298 def inv(self): 

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

300 

301 tmp_quaternion = self.q.copy() 

302 tmp_quaternion[0] *= -1.0 

303 return Rotation.from_quaternion(tmp_quaternion) 

304 

305 def __mul__(self, other): 

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

307 

308 # Check if the other object is also a rotation. 

309 if isinstance(other, Rotation): 

310 # Get quaternions of the two objects. 

311 p = self.q 

312 q = other.q 

313 # Add the rotations. 

314 added_rotation = _np.zeros_like(self.q) 

315 added_rotation[0] = p[0] * q[0] - _np.dot(p[1:], q[1:]) 

316 added_rotation[1:] = p[0] * q[1:] + q[0] * p[1:] + _np.cross(p[1:], q[1:]) 

317 return Rotation.from_quaternion(added_rotation) 

318 elif isinstance(other, (list, _np.ndarray)) and len(other) == 3: 

319 # Apply rotation to vector. 

320 return _np.dot(self.get_rotation_matrix(), _np.asarray(other)) 

321 raise NotImplementedError("Error, not implemented, does not make sense anyway!") 

322 

323 def __eq__(self, other): 

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

325 

326 if isinstance(other, Rotation): 

327 return bool( 

328 (_np.linalg.norm(self.q - other.q) < _mpy.eps_quaternion) 

329 or (_np.linalg.norm(self.q + other.q) < _mpy.eps_quaternion) 

330 ) 

331 else: 

332 return object.__eq__(self, other) 

333 

334 def copy(self): 

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

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

337 

338 def __str__(self): 

339 """String representation of object.""" 

340 

341 self.check() 

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

343 

344 

345def add_rotations(rotation_21, rotation_10): 

346 """Multiply a rotation onto another. 

347 

348 Args 

349 ---- 

350 rotation_10: _np.ndarray 

351 Array with the dimensions n x 4 or 4 x 1. 

352 The first rotation that is applied. 

353 rotation_21: _np.ndarray 

354 Array with the dimensions n x 4 or 4 x 1. 

355 The second rotation that is applied. 

356 

357 Return 

358 ---- 

359 rot_new: _np.ndarray 

360 Array with the dimensions n x 4. 

361 This array contains the new quaternions. 

362 """ 

363 

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

365 if isinstance(rotation_10, Rotation): 

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

367 else: 

368 rot1 = _np.transpose(rotation_10) 

369 if isinstance(rotation_21, Rotation): 

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

371 else: 

372 rot2 = _np.transpose(rotation_21) 

373 

374 if rot1.size > rot2.size: 

375 rotnew = _np.zeros_like(rot1) 

376 else: 

377 rotnew = _np.zeros_like(rot2) 

378 

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

380 rotnew[0] = ( 

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

382 ) 

383 rotnew[1] = ( 

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

385 ) 

386 rotnew[2] = ( 

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

388 ) 

389 rotnew[3] = ( 

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

391 ) 

392 

393 return rotnew.transpose() 

394 

395 

396def rotate_coordinates(coordinates, rotation, *, origin=None): 

397 """Rotate all given coordinates. 

398 

399 Args 

400 ---- 

401 coordinates: _np.array 

402 Array of 3D coordinates to be rotated 

403 rotation: Rotation, list(quaternions) (nx4) 

404 The rotation that will be applied to the coordinates. Can also be an 

405 array with a quaternion for each coordinate. 

406 origin: 3D vector 

407 If this is given, the mesh is rotated about this point. Default is 

408 (0,0,0) 

409 """ 

410 

411 if isinstance(rotation, Rotation): 

412 rotation = rotation.get_quaternion().transpose() 

413 

414 # Check if origin has to be added 

415 if origin is None: 

416 origin = [0.0, 0.0, 0.0] 

417 

418 # New position array 

419 coordinates_new = _np.zeros_like(coordinates) 

420 

421 # Evaluate the new positions using the numpy data structure 

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

423 rotation = rotation.transpose() 

424 

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

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

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

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

429 

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

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

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

433 

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

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

436 

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

438 

439 coordinates_new[:, 0] = ( 

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

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

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

443 ) 

444 coordinates_new[:, 1] = ( 

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

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

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

448 ) 

449 coordinates_new[:, 2] = ( 

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

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

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

453 ) 

454 

455 if origin is not None: 

456 coordinates_new += origin 

457 

458 return coordinates_new 

459 

460 

461def smallest_rotation(q: Rotation, t): 

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

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

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

465 

466 Args 

467 ---- 

468 q: Rotation 

469 Starting triad. 

470 t: Vector in R3 

471 Direction of the first basis of the rotated triad. 

472 Return 

473 ---- 

474 q_sr: Rotation 

475 The triad that results from a smallest rotation. 

476 """ 

477 

478 R_old = q.get_rotation_matrix() 

479 g1_old = R_old[:, 0] 

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

481 

482 # Quaternion components of relative rotation 

483 q_rel = _np.zeros(4) 

484 

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

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

487 

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

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

490 

491 return Rotation.from_quaternion(q_rel) * q