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

1# The MIT License (MIT)

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

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

146

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)

155

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

157 return cls.from_rotation_matrix(R)

158

159 @classmethod

160 def from_rotation_vector(cls, rotation_vector):

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

162

163 q = _np.zeros(4)

164 rotation_vector = _np.asarray(rotation_vector)

165 phi = _np.linalg.norm(rotation_vector)

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

167 if phi < _bme.eps_quaternion:

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

169 q[1:] = 0.5 * rotation_vector

170 else:

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

172 return cls.from_quaternion(q)

173

174 def check(self):

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

176 self.check_uniqueness()

177 self.check_quaternion_constraint()

178

179 def check_uniqueness(self):

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

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

182

183 if self.q[0] < 0:

184 self.q *= -1

185

186 def check_quaternion_constraint(self):

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

188

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

190 raise ValueError(

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

192 )

193

194 def get_rotation_matrix(self):

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

196

197 (Krenk (3.50))

198 """

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

200 R = (

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

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

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

204 )

205

206 return R

207

208 def get_quaternion(self):

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

210 return _np.array(self.q)

211

212 def get_numpy_quaternion(self):

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

214 (copy)."""

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

216

217 def get_rotation_vector(self):

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

219

220 self.check()

221

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

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

224

225 if phi < _bme.eps_quaternion:

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

227 scale_factor = 2

228 else:

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

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

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

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

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

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

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

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

237 # that is not 0 is positive.

238 for i_dir in range(3):

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

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

241 scale_factor *= -1

242 break

243 return self.q[1:] * scale_factor

244

245 def get_transformation_matrix(self):

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

247

248 The transformation matrix maps the (infinitesimal)

249 multiplicative rotational increments onto the additive ones.

250 """

251

252 omega = self.get_rotation_vector()

253 omega_norm = _np.linalg.norm(omega)

254

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

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

257 if omega_norm**2 > _bme.eps_quaternion:

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

259 omega_dir = omega / omega_norm

260 omega_skew = skew_matrix(omega)

261 transformation_matrix = (

262 _np.outer(omega_dir, omega_dir)

263 - 0.5 * omega_skew

264 + 0.5

265 * omega_norm

266 / _np.tan(0.5 * omega_norm)

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

268 )

269 else:

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

271 # function is used with automatic differentiation, higher order

272 # terms have to be added!

273 transformation_matrix = _np.identity(3)

274 return transformation_matrix

275

276 def get_transformation_matrix_inv(self):

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

278

279 The inverse of the transformation matrix maps the

280 (infinitesimal) additive rotational increments onto the

281 multiplicative ones.

282 """

283

284 omega = self.get_rotation_vector()

285 omega_norm = _np.linalg.norm(omega)

286

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

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

289 if omega_norm**2 > _bme.eps_quaternion:

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

291 omega_dir = omega / omega_norm

292 omega_skew = skew_matrix(omega)

293 transformation_matrix_inverse = (

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

295 * _np.outer(omega_dir, omega_dir)

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

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

298 )

299 else:

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

301 # function is used with automatic differentiation, higher order

302 # terms have to be added!

303 transformation_matrix_inverse = _np.identity(3)

304 return transformation_matrix_inverse

305

306 def inv(self):

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

308

309 tmp_quaternion = self.q.copy()

310 tmp_quaternion[0] *= -1.0

311 return Rotation.from_quaternion(tmp_quaternion)

312

313 def __mul__(self, other):

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

315

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

317 if isinstance(other, Rotation):

318 # Get quaternions of the two objects.

319 p = self.q

320 q = other.q

321 # Add the rotations.

322 added_rotation = _np.zeros_like(self.q)

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

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

325 return Rotation.from_quaternion(added_rotation)

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

327 # Apply rotation to vector.

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

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

330

331 def __eq__(self, other):

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

333

334 if isinstance(other, Rotation):

335 return bool(

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

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

338 )

339 else:

340 return object.__eq__(self, other)

341

342 def copy(self):

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

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

345

346 def __str__(self):

347 """String representation of object."""

348

349 self.check()

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

351

352

353def add_rotations(

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

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

356) -> _NDArray[_quaternion.quaternion]:

357 """Multiply rotations onto another.

358

359 Args:

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

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

362

363 Returns:

364 An array with the compound quaternions.

365 """

366

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

368 if isinstance(rotation_10, Rotation):

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

370 else:

371 rot1 = _np.transpose(rotation_10)

372 if isinstance(rotation_21, Rotation):

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

374 else:

375 rot2 = _np.transpose(rotation_21)

376

377 if rot1.size > rot2.size:

378 rotnew = _np.zeros_like(rot1)

379 else:

380 rotnew = _np.zeros_like(rot2)

381

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

383 rotnew[0] = (

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

385 )

386 rotnew[1] = (

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

388 )

389 rotnew[2] = (

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

391 )

392 rotnew[3] = (

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

394 )

395

396 return rotnew.transpose()

397

398

399def rotate_coordinates(

400 coordinates: _NDArray,

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

402 *,

403 origin=None,

404):

405 """Rotate all given coordinates.

406

407 Args:

408 coordinates: Array of 3D coordinates to be rotated

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

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

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

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

413 """

414

415 if isinstance(rotation, Rotation):

416 rotation = rotation.get_quaternion().transpose()

417

418 # Check if origin has to be added

419 if origin is None:

420 origin = [0.0, 0.0, 0.0]

421

422 # New position array

423 coordinates_new = _np.zeros_like(coordinates)

424

425 # Evaluate the new positions using the numpy data structure

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

427 rotation = rotation.transpose()

428

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

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

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

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

433

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

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

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

437

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

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

440

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

442

443 coordinates_new[:, 0] = (

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

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

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

447 )

448 coordinates_new[:, 1] = (

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

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

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

452 )

453 coordinates_new[:, 2] = (

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

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

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

457 )

458

459 if origin is not None:

460 coordinates_new += origin

461

462 return coordinates_new

463

464

465def smallest_rotation(q: Rotation, t):

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

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

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

469

470 Args

471 ----

472 q: Rotation

473 Starting triad.

474 t: Vector in R3

475 Direction of the first basis of the rotated triad.

476 Return

477 ----

478 q_sr: Rotation

479 The triad that results from a smallest rotation.

480 """

481

482 R_old = q.get_rotation_matrix()

483 g1_old = R_old[:, 0]

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

485

486 # Quaternion components of relative rotation

487 q_rel = _np.zeros(4)

488

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

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

491

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

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

494

495 return Rotation.from_quaternion(q_rel) * q

496

497

498def get_rotation_vector_series(

499 rotation_vectors: list | _NDArray | list[Rotation],

500) -> _NDArray:

501 """Return an array containing the rotation vectors representing the given

502 rotation vectors.

503

504 The main feature of this function is, that the returned rotation

505 vectors don't have jumps when the rotation angle exceeds 2*pi. We

506 return a "continuous" series of rotation vectors that can be used to

507 interpolate between them.

508

509 Note: Interpolating between the returned rotation vectors will in general

510 result in a non-objective interpolation.

511

512 Args:

513 rotation_vectors: A list or array containing the rotation vectors, has

514 to be in order.

515

516 Returns:

517 An array containing the "continuous" rotation vectors.

518 """

519

520 if isinstance(rotation_vectors, list):

521 rotation_vector_array = _np.zeros((len(rotation_vectors), 3))

522 for i_rotation, rotation_entry in enumerate(rotation_vectors):

523 if isinstance(rotation_entry, Rotation):

524 rotation_vector_array[i_rotation] = rotation_entry.get_rotation_vector()

525 else:

526 rotation_vector_array[i_rotation] = rotation_entry

527

528 def closest_multiple_of_two_pi(x: float) -> float:

529 """Given the value x, return the multiple of 2*pi that is closest to

530 it."""

531 return 2.0 * _np.pi * round(x / (2 * _np.pi))

532

533 rotation_vectors_continuous = _np.zeros((len(rotation_vector_array), 3))

534 rotation_vectors_continuous[0, :] = rotation_vector_array[0]

535

536 for i, current_rotation_vector in enumerate(rotation_vector_array[1:]):

537 rotation_vector_last = rotation_vector_array[i]

538 theta = _np.linalg.norm(current_rotation_vector)

539

540 if theta < _bme.eps_quaternion:

541 # The current rotation is an identity rotation.

542 theta_last = _np.linalg.norm(rotation_vector_last)

543 if theta_last < _bme.eps_quaternion:

544 # The last rotation was also an identity rotation, so simply add

545 # an empty rotation vector

546 rotation_vectors_continuous[i + 1] = [0.0, 0.0, 0.0]

547 else:

548 # The last rotation was not a zero rotation. In this case we simply

549 # take the direction of the last rotation vector and set it to to the

550 # closest length which is a multiple of 2*pi.

551 rotation_vectors_continuous[i + 1] = (

552 rotation_vector_last

553 / theta_last

554 * closest_multiple_of_two_pi(theta_last)

555 )

556 else:

557 axis = current_rotation_vector / theta

558 # This is the offset from the current rotation vector to the previous one

559 # in the sense of a multiple of 2*pi.

560 multiple_of_two_pi = closest_multiple_of_two_pi(

561 (_np.dot(axis, rotation_vector_last) - theta)

562 )

563 rotation_vectors_continuous[i + 1] = (multiple_of_two_pi + theta) * axis

564 return rotation_vectors_continuous

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

225 statements