test_suite.unit_tests._lib._geometry.test

41 """Unit tests for the lib.geometry.rotations relax module.""" 42

43 - def setUp(self):

44 """Set up data used by the unit tests.""" 45 46 # Axes. 47 self.x_axis_pos = array([1, 0, 0], float64) 48 self.y_axis_pos = array([0, 1, 0], float64) 49 self.z_axis_pos = array([0, 0, 1], float64) 50 51 # Axes (do everything again, this time negative!). 52 self.x_axis_neg = array([-1, 0, 0], float64) 53 self.y_axis_neg = array([0, -1, 0], float64) 54 self.z_axis_neg = array([0, 0, -1], float64)

55 56

57 - def check_return_conversion(self, euler_to_R=None, R_to_euler=None, alpha_start=None, beta_start=None, gamma_start=None, alpha_end=None, beta_end=None, gamma_end=None):

58 """Check the Euler angle to rotation matrix to Euler angle conversion.""" 59 60 # Print out. 61 print_out = "" 62 print_out = print_out + "\n\n# Checking the %s() and %s() conversions.\n\n" % (euler_to_R.__name__, R_to_euler.__name__) 63 64 # A small number. 65 epsilon = 1e-15 66 67 # End angles. 68 if alpha_end == None: 69 alpha_end = alpha_start 70 if beta_end == None: 71 beta_end = beta_start 72 if gamma_end == None: 73 gamma_end = gamma_start 74 75 # Generate the rotation matrix. 76 euler_to_R(alpha_start, beta_start, gamma_start, R) 77 print_out = print_out + "R: |%8.5f, %8.5f, %8.5f|\n" % (R[0, 0], R[0, 1], R[0, 2]) 78 print_out = print_out + " |%8.5f, %8.5f, %8.5f|\n" % (R[1, 0], R[1, 1], R[1, 2]) 79 print_out = print_out + " |%8.5f, %8.5f, %8.5f|\n" % (R[2, 0], R[2, 1], R[2, 2]) 80 81 # Get back the angles. 82 alpha_new, beta_new, gamma_new = R_to_euler(R) 83 84 # Print out. 85 print_out = print_out + "Original angles: (%8.5f, %8.5f, %8.5f)\n" % (alpha_start, beta_start, gamma_start) 86 print_out = print_out + "End angles: (%8.5f, %8.5f, %8.5f)\n" % (alpha_end, beta_end, gamma_end) 87 print_out = print_out + "New angles: (%8.5f, %8.5f, %8.5f)\n" % (alpha_new, beta_new, gamma_new) 88 89 # Wrap the angles. 90 alpha_new = wrap_angles(alpha_new, 0-epsilon, 2*pi-epsilon) 91 beta_new = wrap_angles(beta_new, 0-epsilon, 2*pi-epsilon) 92 gamma_new = wrap_angles(gamma_new, 0-epsilon, 2*pi-epsilon) 93 94 # Print out. 95 print_out = print_out + "New angles (wrapped) (%8.5f, %8.5f, %8.5f)\n" % (alpha_new, beta_new, gamma_new) 96 97 # Second solution required! 98 if abs(beta_end - beta_new) > 1e-7: 99 # Collapse the multiple beta solutions. 100 if beta_new < pi: 101 alpha_new = alpha_new + pi 102 beta_new = pi - beta_new 103 gamma_new = gamma_new + pi 104 else: 105 alpha_new = alpha_new + pi 106 beta_new = 3*pi - beta_new 107 gamma_new = gamma_new + pi 108 109 # Wrap the angles. 110 alpha_new = wrap_angles(alpha_new, 0-epsilon, 2*pi-epsilon) 111 beta_new = wrap_angles(beta_new, 0-epsilon, 2*pi-epsilon) 112 gamma_new = wrap_angles(gamma_new, 0-epsilon, 2*pi-epsilon) 113 114 # Print out. 115 print_out = print_out + "New angles (2nd sol) (%8.5f, %8.5f, %8.5f)\n" % (alpha_new, beta_new, gamma_new) 116 117 # Print out. 118 eps = 1e-5 119 if abs(alpha_new - alpha_end) > eps or abs(beta_new - beta_end) > eps or abs(gamma_new - gamma_end) > eps: 120 print(print_out) 121 122 # Checks. 123 self.assertAlmostEqual(alpha_end, alpha_new) 124 self.assertAlmostEqual(beta_end, beta_new) 125 self.assertAlmostEqual(gamma_end, gamma_new)

126 127

128 - def check_rotation(self, R, x_real_pos, y_real_pos, z_real_pos, x_real_neg, y_real_neg, z_real_neg):

129 """Check that the rotation matrix is correct.""" 130 131 # Rotate the 6 axes. 132 x_new_pos = dot(R, self.x_axis_pos) 133 y_new_pos = dot(R, self.y_axis_pos) 134 z_new_pos = dot(R, self.z_axis_pos) 135 136 x_new_neg = dot(R, self.x_axis_neg) 137 y_new_neg = dot(R, self.y_axis_neg) 138 z_new_neg = dot(R, self.z_axis_neg) 139 140 # Print out. 141 print("Rotated and true axes (beta = pi/4):") 142 print("x pos rot: %s" % x_new_pos) 143 print("x pos real: %s\n" % x_real_pos) 144 print("y pos rot: %s" % y_new_pos) 145 print("y pos real: %s\n" % y_real_pos) 146 print("z pos rot: %s" % z_new_pos) 147 print("z pos real: %s\n" % z_real_pos) 148 149 print("x neg rot: %s" % x_new_neg) 150 print("x neg real: %s\n" % x_real_neg) 151 print("y neg rot: %s" % y_new_neg) 152 print("y neg real: %s\n" % y_real_neg) 153 print("z neg rot: %s" % z_new_neg) 154 print("z neg real: %s\n" % z_real_neg) 155 156 # Checks. 157 for i in range(3): 158 self.assertAlmostEqual(x_new_pos[i], x_real_pos[i]) 159 self.assertAlmostEqual(y_new_pos[i], y_real_pos[i]) 160 self.assertAlmostEqual(z_new_pos[i], z_real_pos[i]) 161 162 self.assertAlmostEqual(x_new_neg[i], x_real_neg[i]) 163 self.assertAlmostEqual(y_new_neg[i], y_real_neg[i]) 164 self.assertAlmostEqual(z_new_neg[i], z_real_neg[i])

165 166

167 - def test_axis_angle_to_R_no_rot(self):

168 """Test the quaternion to rotation matrix conversion for a zero angle rotation.""" 169 170 # Quaternion of zero angle. 171 axis = array([-1, 1, 1], float64) / sqrt(3) 172 angle = 0.0 173 174 # The rotation matrix. 175 axis_angle_to_R(axis, angle, R) 176 print("Rotation matrix:\n%s" % R) 177 178 # The correct result. 179 R_true = eye(3) 180 181 # Checks. 182 for i in range(3): 183 for j in range(3): 184 self.assertEqual(R[i, j], R_true[i, j])

185 186

187 - def test_axis_angle_to_quaternion_no_rot(self):

188 """Test the axis-angle to quaternion conversion for a zero angle rotation.""" 189 190 # Random axis and zero angle. 191 axis = array([-1, 1, 1], float64) / sqrt(3) 192 angle = 0.0 193 194 # The quaternion matrix. 195 quat = zeros(4, float64) 196 axis_angle_to_quaternion(axis, angle, quat) 197 print("Quaternion:\n%s" % quat) 198 199 # The correct result. 200 quat_true = array([1, 0, 0, 0], float64) 201 202 # Checks. 203 for i in range(4): 204 self.assertEqual(quat[i], quat_true[i])

205 206

207 - def test_axis_angle_to_quaternion_180_complex(self):

208 """Test the axis-angle to quaternion conversion for a 180 degree rotation about [1, 1, 1].""" 209 210 # Random axis and zero angle. 211 axis = array([1, 1, 1], float64) / sqrt(3) 212 angle = pi 213 214 # The quaternion matrix. 215 quat = zeros(4, float64) 216 axis_angle_to_quaternion(axis, angle, quat) 217 print("Quaternion:\n%s" % quat) 218 219 # The correct result. 220 quat_true = array([0, 1, 1, 1], float64) / sqrt(3) 221 222 # Checks. 223 for i in range(4): 224 self.assertAlmostEqual(quat[i], quat_true[i])

225 226

227 - def test_axis_angle_to_R_z_30(self):

228 """Test the quaternion to rotation matrix conversion for a 30 degree rotation about z.""" 229 230 # Axis-angle values. 231 axis = array([0, 0, 1], float64) 232 angle = pi / 6 233 234 # Generate the rotation matrix. 235 axis_angle_to_R(axis, angle, R) 236 print("Rotation matrix:\n%s" % R) 237 238 # Rotated pos_axes (real values). 239 x_real_pos = array([cos(pi/6), sin(pi/6), 0], float64) 240 y_real_pos = array([-sin(pi/6), cos(pi/6), 0], float64) 241 z_real_pos = array([0, 0, 1], float64) 242 243 # Rotated neg axes (real values). 244 x_real_neg = array([-cos(pi/6), -sin(pi/6), 0], float64) 245 y_real_neg = array([sin(pi/6), -cos(pi/6), 0], float64) 246 z_real_neg = array([0, 0, -1], float64) 247 248 # Check the rotation. 249 self.check_rotation(R, x_real_pos, y_real_pos, z_real_pos, x_real_neg, y_real_neg, z_real_neg)

250 251

252 - def test_axis_angle_to_R_180_complex(self):

253 """Test the quaternion to rotation matrix conversion for a 180 degree rotation about [1, 1, 1].""" 254 255 # Axis-angle values. 256 axis = array([1, 1, 1], float64) / sqrt(3) 257 angle = 2 * pi / 3 258 259 # Generate the rotation matrix. 260 axis_angle_to_R(axis, angle, R) 261 print("Rotation matrix:\n%s" % R) 262 263 # Rotated pos axes (real values). 264 x_real_pos = array([0, 1, 0], float64) 265 y_real_pos = array([0, 0, 1], float64) 266 z_real_pos = array([1, 0, 0], float64) 267 268 # Rotated neg axes (real values). 269 x_real_neg = array([0, -1, 0], float64) 270 y_real_neg = array([0, 0, -1], float64) 271 z_real_neg = array([-1, 0, 0], float64) 272 273 # Check the rotation. 274 self.check_rotation(R, x_real_pos, y_real_pos, z_real_pos, x_real_neg, y_real_neg, z_real_neg)

275 276

277 - def test_euler_cycle_1(self):

278 """Cycle through all the hard-coded conversion functions returning to the starting point. 279 280 The nested cycling is as follows: 281 - start with random Euler angles alpha, beta, gamma, 282 - convert to a rotation matrix using euler_to_R_xyx(), 283 - xyx cycle: 284 - R_to_euler_xyx() 285 - euler_to_R_xyx(), 286 - R_to_axis_angle(), 287 - axis_angle_to_euler_xyx(), 288 - euler_to_axis_angle_xyx(), 289 - axis_angle_to_R(), 290 - xyz cycle: 291 - R_to_euler_xyz() 292 - euler_to_R_xyz(), 293 - R_to_axis_angle(), 294 - axis_angle_to_euler_xyz(), 295 - euler_to_axis_angle_xyz(), 296 - axis_angle_to_R(), 297 - xzx cycle: 298 - R_to_euler_xzx() 299 - euler_to_R_xzx(), 300 - R_to_axis_angle(), 301 - axis_angle_to_euler_xzx(), 302 - euler_to_axis_angle_xzx(), 303 - axis_angle_to_R(), 304 - xzy cycle: 305 - R_to_euler_xzy() 306 - euler_to_R_xzy(), 307 - R_to_axis_angle(), 308 - axis_angle_to_euler_xzy(), 309 - euler_to_axis_angle_xzy(), 310 - axis_angle_to_R(), 311 - yxy cycle: 312 - R_to_euler_yxy() 313 - euler_to_R_yxy(), 314 - R_to_axis_angle(), 315 - axis_angle_to_euler_yxy(), 316 - euler_to_axis_angle_yxy(), 317 - axis_angle_to_R(), 318 - yxz cycle: 319 - R_to_euler_yxz() 320 - euler_to_R_yxz(), 321 - R_to_axis_angle(), 322 - axis_angle_to_euler_yxz(), 323 - euler_to_axis_angle_yxz(), 324 - axis_angle_to_R(), 325 - yzx cycle: 326 - R_to_euler_yzx() 327 - euler_to_R_yzx(), 328 - R_to_axis_angle(), 329 - axis_angle_to_euler_yzx(), 330 - euler_to_axis_angle_yzx(), 331 - axis_angle_to_R(), 332 - yzy cycle: 333 - R_to_euler_yzy() 334 - euler_to_R_yzy(), 335 - R_to_axis_angle(), 336 - axis_angle_to_euler_yzy(), 337 - euler_to_axis_angle_yzy(), 338 - axis_angle_to_R(), 339 - zxy cycle: 340 - R_to_euler_zxy() 341 - euler_to_R_zxy(), 342 - R_to_axis_angle(), 343 - axis_angle_to_euler_zxy(), 344 - euler_to_axis_angle_zxy(), 345 - axis_angle_to_R(), 346 - zxz cycle: 347 - R_to_euler_zxz() 348 - euler_to_R_zxz(), 349 - R_to_axis_angle(), 350 - axis_angle_to_euler_zxz(), 351 - euler_to_axis_angle_zxz(), 352 - axis_angle_to_R(), 353 - zyx cycle: 354 - R_to_euler_zyx() 355 - euler_to_R_zyx(), 356 - R_to_axis_angle(), 357 - axis_angle_to_euler_zyx(), 358 - euler_to_axis_angle_zyx(), 359 - axis_angle_to_R(), 360 - zyz cycle: 361 - R_to_euler_zyz() 362 - euler_to_R_zyz(), 363 - R_to_axis_angle(), 364 - axis_angle_to_euler_zyz(), 365 - euler_to_axis_angle_zyz(), 366 - axis_angle_to_R(), 367 - return to start with R_to_euler_xyx(). 368 """ 369 370 # Starting angles. 371 alpha_init = uniform(0, 2*pi) 372 beta_init = uniform(0, pi) 373 gamma_init = uniform(0, 2*pi) 374 375 # The start point. 376 euler_to_R_xyx(alpha_init, beta_init, gamma_init, R) 377 euler_to_R_xyx(alpha_init, beta_init, gamma_init, R2) 378 379 # Print out. 380 print("Original data:") 381 print("alpha: %s" % alpha_init) 382 print("beta: %s" % beta_init) 383 print("gamma: %s\n" % gamma_init) 384 print("R:\n%s\n" % R2) 385 386 # The different notations. 387 sets = ['xyx', 'xyz', 'xzx', 'xzy', 'yxy', 'yxz', 'yzx', 'yzy', 'zxy', 'zxz', 'zyx', 'zyz'] 388 shuffle(sets) 389 390 # Cycle over the notations. 391 for set in sets: 392 # Header printout. 393 print("\n\n# %s cycle.\n" % set) 394 395 # Alias the functions. 396 axis_angle_to_euler = globals()['axis_angle_to_euler_'+set] 397 euler_to_axis_angle = globals()['euler_to_axis_angle_'+set] 398 euler_to_R = globals()['euler_to_R_'+set] 399 R_to_euler = globals()['R_to_euler_'+set] 400 401 # R -> Euler. 402 a, b, g = R_to_euler(R) 403 print("R -> Euler: [%-8.5f, %-8.5f, %-8.5f]\n" % (a, b, g)) 404 405 # Euler -> R 406 euler_to_R(a, b, g, R) 407 print("Euler -> R:\n%s\n" % R) 408 409 # R -> axis, angle. 410 axis, angle = R_to_axis_angle(R) 411 print("R -> axis, angle: [%-8.5f, %-8.5f, %-8.5f], %s\n" % (axis[0], axis[1], axis[2], angle)) 412 413 # axis, angle -> Euler. 414 a, b, g = axis_angle_to_euler(axis, angle) 415 print("axis, angle -> Euler: [%-8.5f, %-8.5f, %-8.5f]\n" % (a, b, g)) 416 417 # Euler -> axis, angle. 418 axis, angle = euler_to_axis_angle(a, b, g) 419 print("Euler -> axis, angle: [%-8.5f, %-8.5f, %-8.5f], %s\n" % (axis[0], axis[1], axis[2], angle)) 420 421 # axis, angle -> R. 422 axis_angle_to_R(axis, angle, R) 423 print("axis, angle -> R:\n%s\n" % R) 424 425 # Print out the rotation matrix. 426 print("Rotation matrix difference:\n%s\n" % (R2-R)) 427 428 # Check the rotation matrix. 429 for i in range(3): 430 for j in range(3): 431 self.assertAlmostEqual(R[i, j], R2[i, j]) 432 433 # The end point. 434 alpha_end, beta_end, gamma_end = R_to_euler_xyx(R) 435 436 # Print out. 437 print("End data:") 438 print("alpha: %s" % alpha_end) 439 print("beta: %s" % beta_end) 440 print("gamma: %s\n" % gamma_end) 441 print("R:\n%s\n" % R) 442 443 # Checks. 444 self.assertAlmostEqual(alpha_init, alpha_end) 445 self.assertAlmostEqual(beta_init, beta_end) 446 self.assertAlmostEqual(gamma_init, gamma_end)

447 448

449 - def test_euler_zyz_to_euler_zyz(self):

450 """Bounce around all the conversion functions to see if the original angles are returned.""" 451 452 # Starting angles. 453 alpha = uniform(0, 2*pi) 454 beta = uniform(0, pi) 455 gamma = uniform(0, 2*pi) 456 457 # Print out. 458 print("Original angles:") 459 print("alpha: %s" % alpha) 460 print("beta: %s" % beta) 461 print("gamma: %s\n" % gamma) 462 463 # Init. 464 axis = zeros(3, float64) 465 quat = zeros(4, float64) 466 467 # 1) Euler angles to rotation matrix. 468 euler_to_R_zyz(alpha, beta, gamma, R) 469 470 # 2) Rotation matrix to axis-angle. 471 axis, angle = R_to_axis_angle(R) 472 473 # 3) Axis-angle to quaternion. 474 axis_angle_to_quaternion(axis, angle, quat) 475 476 # 4) Quaternion to axis-angle. 477 axis, angle = quaternion_to_axis_angle(quat) 478 479 # 5) Axis-angle to rotation matrix. 480 axis_angle_to_R(axis, angle, R) 481 482 # 6) Rotation matrix to quaternion. 483 R_to_quaternion(R, quat) 484 485 # 7) Quaternion to rotation matrix. 486 quaternion_to_R(quat, R) 487 488 # 8) Rotation matrix to Euler angles. 489 alpha_new, beta_new, gamma_new = R_to_euler_zyz(R) 490 491 # 9) Euler angles to axis-angle. 492 axis, angle = euler_to_axis_angle_zyz(alpha_new, beta_new, gamma_new) 493 494 # 10) Axis-angle to Euler angles. 495 alpha_new, beta_new, gamma_new = axis_angle_to_euler_zyz(axis, angle) 496 497 # Wrap the angles. 498 alpha_new = wrap_angles(alpha_new, 0, 2*pi) 499 beta_new = wrap_angles(beta_new, 0, 2*pi) 500 gamma_new = wrap_angles(gamma_new, 0, 2*pi) 501 502 # Print out. 503 print("New angles:") 504 print("alpha: %s" % alpha_new) 505 print("beta: %s" % beta_new) 506 print("gamma: %s\n" % gamma_new) 507 508 # Checks. 509 self.assertAlmostEqual(alpha, alpha_new) 510 self.assertAlmostEqual(beta, beta_new) 511 self.assertAlmostEqual(gamma, gamma_new)

512 513

514 - def test_euler_zyz_to_R_alpha_30(self):

515 """Test the rotation matrix from zyz Euler angle conversion using a beta angle of pi/4.""" 516 517 # Generate the rotation matrix. 518 euler_to_R_zyz(pi/6, 0.0, 0.0, R) 519 520 # Rotated pos axes (real values). 521 x_real_pos = array([cos(pi/6), sin(pi/6), 0], float64) 522 y_real_pos = array([-sin(pi/6), cos(pi/6), 0], float64) 523 z_real_pos = array([0, 0, 1], float64) 524 525 # Rotated neg axes (real values). 526 x_real_neg = array([-cos(pi/6), -sin(pi/6), 0], float64) 527 y_real_neg = array([sin(pi/6), -cos(pi/6), 0], float64) 528 z_real_neg = array([0, 0, -1], float64) 529 530 # Check the rotation. 531 self.check_rotation(R, x_real_pos, y_real_pos, z_real_pos, x_real_neg, y_real_neg, z_real_neg)

532 533

534 - def test_euler_zyz_to_R_beta_45(self):

535 """Test the rotation matrix from zyz Euler angle conversion using a beta angle of pi/4.""" 536 537 # Generate the rotation matrix. 538 euler_to_R_zyz(0.0, pi/4, 0.0, R) 539 540 # Rotated pos axes (real values). 541 x_real_pos = array([cos(pi/4), 0, -sin(pi/4)], float64) 542 y_real_pos = array([0, 1, 0], float64) 543 z_real_pos = array([sin(pi/4), 0, cos(pi/4)], float64) 544 545 # Rotated neg axes (real values). 546 x_real_neg = array([-cos(pi/4), 0, sin(pi/4)], float64) 547 y_real_neg = array([0, -1, 0], float64) 548 z_real_neg = array([-sin(pi/4), 0, -cos(pi/4)], float64) 549 550 # Check the rotation. 551 self.check_rotation(R, x_real_pos, y_real_pos, z_real_pos, x_real_neg, y_real_neg, z_real_neg)

552 553

554 - def test_euler_zyz_to_R_gamma_15(self):

555 """Test the rotation matrix from zyz Euler angle conversion using a beta angle of pi/4.""" 556 557 # Generate the rotation matrix. 558 euler_to_R_zyz(0.0, 0.0, pi/12, R) 559 560 # Rotated pos axes (real values). 561 x_real_pos = array([cos(pi/12), sin(pi/12), 0], float64) 562 y_real_pos = array([-sin(pi/12), cos(pi/12), 0], float64) 563 z_real_pos = array([0, 0, 1], float64) 564 565 # Rotated neg axes (real values). 566 x_real_neg = array([-cos(pi/12), -sin(pi/12), 0], float64) 567 y_real_neg = array([sin(pi/12), -cos(pi/12), 0], float64) 568 z_real_neg = array([0, 0, -1], float64) 569 570 # Check the rotation. 571 self.check_rotation(R, x_real_pos, y_real_pos, z_real_pos, x_real_neg, y_real_neg, z_real_neg)

572 573

574 - def test_euler_zyz_to_R_alpha_15_gamma_15(self):

575 """Test the rotation matrix from zyz Euler angle conversion using a beta angle of pi/4.""" 576 577 # Generate the rotation matrix. 578 euler_to_R_zyz(pi/12, 0.0, pi/12, R) 579 580 # Rotated pos axes (real values). 581 x_real_pos = array([cos(pi/6), sin(pi/6), 0], float64) 582 y_real_pos = array([-sin(pi/6), cos(pi/6), 0], float64) 583 z_real_pos = array([0, 0, 1], float64) 584 585 # Rotated neg axes (real values). 586 x_real_neg = array([-cos(pi/6), -sin(pi/6), 0], float64) 587 y_real_neg = array([sin(pi/6), -cos(pi/6), 0], float64) 588 z_real_neg = array([0, 0, -1], float64) 589 590 # Check the rotation. 591 self.check_rotation(R, x_real_pos, y_real_pos, z_real_pos, x_real_neg, y_real_neg, z_real_neg)

592 593

594 - def test_R_to_axis_angle_no_rot(self):

595 """Test the rotation matrix to axis-angle conversion.""" 596 597 # Generate the rotation matrix. 598 R = array([[1, 0, 0], [0, 1, 0], [0, 0, 1]], float64) 599 600 # Get the axis and angle. 601 axis, angle = R_to_axis_angle(R) 602 603 # Test the angle. 604 self.assertEqual(angle, 0.0)

605 606

607 - def test_R_to_axis_angle_180_complex(self):

608 """Test the rotation matrix to axis-angle conversion.""" 609 610 # Generate the rotation matrix. 611 R = array([[0, 0, 1], [1, 0, 0], [0, 1, 0]], float64) 612 613 # Get the axis and angle. 614 axis, angle = R_to_axis_angle(R) 615 616 # Test the angle. 617 self.assertAlmostEqual(angle, 2 * pi / 3) 618 619 # Test the vector. 620 for i in range(3): 621 self.assertAlmostEqual(axis[i], 1.0/sqrt(3))

622 623

624 - def test_R_to_euler_to_R_xyx(self):

625 """Test the rotation matrix to xyx Euler angle conversion and back again.""" 626 627 # Random rotation matrix. 628 R_random_hypersphere(R) 629 R_orig = deepcopy(R) 630 631 # Convert. 632 a, b, g = R_to_euler_xyx(R) 633 euler_to_R_xyx(a, b, g, R2) 634 635 # Check the rotation matrix. 636 for i in range(3): 637 for j in range(3): 638 self.assertAlmostEqual(R_orig[i, j], R2[i, j])

639 640

641 - def test_R_to_euler_to_R_xyz(self):

642 """Test the rotation matrix to xyz Euler angle conversion and back again.""" 643 644 # Random rotation matrix. 645 R_random_hypersphere(R) 646 R_orig = deepcopy(R) 647 648 # Convert. 649 a, b, g = R_to_euler_xyz(R) 650 euler_to_R_xyz(a, b, g, R2) 651 652 # Check the rotation matrix. 653 for i in range(3): 654 for j in range(3): 655 self.assertAlmostEqual(R_orig[i, j], R2[i, j])

656 657

658 - def test_R_to_euler_to_R_xzx(self):

659 """Test the rotation matrix to xzx Euler angle conversion and back again.""" 660 661 # Random rotation matrix. 662 R_random_hypersphere(R) 663 R_orig = deepcopy(R) 664 665 # Convert. 666 a, b, g = R_to_euler_xzx(R) 667 euler_to_R_xzx(a, b, g, R2) 668 669 # Check the rotation matrix. 670 for i in range(3): 671 for j in range(3): 672 self.assertAlmostEqual(R_orig[i, j], R2[i, j])

673 674

675 - def test_R_to_euler_to_R_xzy(self):

676 """Test the rotation matrix to xzy Euler angle conversion and back again.""" 677 678 # Random rotation matrix. 679 R_random_hypersphere(R) 680 R_orig = deepcopy(R) 681 682 # Convert. 683 a, b, g = R_to_euler_xzy(R) 684 euler_to_R_xzy(a, b, g, R2) 685 686 # Check the rotation matrix. 687 for i in range(3): 688 for j in range(3): 689 self.assertAlmostEqual(R_orig[i, j], R2[i, j])

690 691

692 - def test_R_to_euler_to_R_yxy(self):

693 """Test the rotation matrix to yxy Euler angle conversion and back again.""" 694 695 # Random rotation matrix. 696 R_random_hypersphere(R) 697 R_orig = deepcopy(R) 698 699 # Convert. 700 a, b, g = R_to_euler_yxy(R) 701 euler_to_R_yxy(a, b, g, R2) 702 703 # Check the rotation matrix. 704 for i in range(3): 705 for j in range(3): 706 self.assertAlmostEqual(R_orig[i, j], R2[i, j])

707 708

709 - def test_R_to_euler_to_R_yxz(self):

710 """Test the rotation matrix to yxz Euler angle conversion and back again.""" 711 712 # Random rotation matrix. 713 R_random_hypersphere(R) 714 R_orig = deepcopy(R) 715 716 # Convert. 717 a, b, g = R_to_euler_yxz(R) 718 euler_to_R_yxz(a, b, g, R2) 719 720 # Check the rotation matrix. 721 for i in range(3): 722 for j in range(3): 723 self.assertAlmostEqual(R_orig[i, j], R2[i, j])

724 725

726 - def test_R_to_euler_to_R_yzx(self):

727 """Test the rotation matrix to yzx Euler angle conversion and back again.""" 728 729 # Random rotation matrix. 730 R_random_hypersphere(R) 731 R_orig = deepcopy(R) 732 733 # Convert. 734 a, b, g = R_to_euler_yzx(R) 735 euler_to_R_yzx(a, b, g, R2) 736 737 # Check the rotation matrix. 738 for i in range(3): 739 for j in range(3): 740 self.assertAlmostEqual(R_orig[i, j], R2[i, j])

741 742

743 - def test_R_to_euler_to_R_yzy(self):

744 """Test the rotation matrix to yzy Euler angle conversion and back again.""" 745 746 # Random rotation matrix. 747 R_random_hypersphere(R) 748 R_orig = deepcopy(R) 749 750 # Convert. 751 a, b, g = R_to_euler_yzy(R) 752 euler_to_R_yzy(a, b, g, R2) 753 754 # Check the rotation matrix. 755 for i in range(3): 756 for j in range(3): 757 self.assertAlmostEqual(R_orig[i, j], R2[i, j])

758 759

760 - def test_R_to_euler_to_R_zxy(self):

761 """Test the rotation matrix to zxy Euler angle conversion and back again.""" 762 763 # Random rotation matrix. 764 R_random_hypersphere(R) 765 R_orig = deepcopy(R) 766 767 # Convert. 768 a, b, g = R_to_euler_zxy(R) 769 euler_to_R_zxy(a, b, g, R2) 770 771 # Check the rotation matrix. 772 for i in range(3): 773 for j in range(3): 774 self.assertAlmostEqual(R_orig[i, j], R2[i, j])

775 776

777 - def test_R_to_euler_to_R_zxz(self):

778 """Test the rotation matrix to zxz Euler angle conversion and back again.""" 779 780 # Random rotation matrix. 781 R_random_hypersphere(R) 782 R_orig = deepcopy(R) 783 784 # Convert. 785 a, b, g = R_to_euler_zxz(R) 786 euler_to_R_zxz(a, b, g, R2) 787 788 # Check the rotation matrix. 789 for i in range(3): 790 for j in range(3): 791 self.assertAlmostEqual(R_orig[i, j], R2[i, j])

792 793

794 - def test_R_to_euler_to_R_zyx(self):

795 """Test the rotation matrix to zyx Euler angle conversion and back again.""" 796 797 # Random rotation matrix. 798 R_random_hypersphere(R) 799 R_orig = deepcopy(R) 800 801 # Convert. 802 a, b, g = R_to_euler_zyx(R) 803 euler_to_R_zyx(a, b, g, R2) 804 805 # Check the rotation matrix. 806 for i in range(3): 807 for j in range(3): 808 self.assertAlmostEqual(R_orig[i, j], R2[i, j])

809 810

811 - def test_R_to_euler_to_R_zyz(self):

812 """Test the rotation matrix to zyz Euler angle conversion and back again.""" 813 814 # Random rotation matrix. 815 R_random_hypersphere(R) 816 R_orig = deepcopy(R) 817 818 # Convert. 819 a, b, g = R_to_euler_zyz(R) 820 euler_to_R_zyz(a, b, g, R2) 821 822 # Check the rotation matrix. 823 for i in range(3): 824 for j in range(3): 825 self.assertAlmostEqual(R_orig[i, j], R2[i, j])

826 827

828 - def test_R_to_euler_xyx(self):

829 """Test the rotation matrix to xyx Euler angle conversion.""" 830 831 # Check random numbers, then the problematic angles. 832 self.check_return_conversion(euler_to_R_xyx, R_to_euler_xyx, uniform(0, 2*pi), uniform(0, pi), uniform(0, 2*pi)) 833 self.check_return_conversion(euler_to_R_xyx, R_to_euler_xyx, 0.0, 0.0, 0.0) 834 self.check_return_conversion(euler_to_R_xyx, R_to_euler_xyx, 1.0, 0.0, 0.0) 835 self.check_return_conversion(euler_to_R_xyx, R_to_euler_xyx, 0.0, 1.0, 0.0) 836 self.check_return_conversion(euler_to_R_xyx, R_to_euler_xyx, 0.0, 0.0, 1.0, alpha_end=1.0, gamma_end=0.0) 837 self.check_return_conversion(euler_to_R_xyx, R_to_euler_xyx, 1.0, 1.0, 0.0) 838 self.check_return_conversion(euler_to_R_xyx, R_to_euler_xyx, 0.0, 1.0, 1.0) 839 self.check_return_conversion(euler_to_R_xyx, R_to_euler_xyx, 1.0, 0.0, 1.0, alpha_end=2.0, gamma_end=0.0) 840 self.check_return_conversion(euler_to_R_xyx, R_to_euler_xyx, 1.0, 1.0, 1.0) 841 self.check_return_conversion(euler_to_R_xyx, R_to_euler_xyx, 1.0, pi/2, 0.5) 842 self.check_return_conversion(euler_to_R_xyx, R_to_euler_xyx, 1.0, pi, 0.5, alpha_end=0.5, gamma_end=0.0)

843 844

845 - def test_R_to_euler_xyz(self):

846 """Test the rotation matrix to xyz Euler angle conversion.""" 847 848 # Check random numbers, then the problematic angles. 849 self.check_return_conversion(euler_to_R_xyz, R_to_euler_xyz, uniform(0, 2*pi), uniform(0, pi), uniform(0, 2*pi)) 850 self.check_return_conversion(euler_to_R_xyz, R_to_euler_xyz, 0.0, 0.0, 0.0) 851 self.check_return_conversion(euler_to_R_xyz, R_to_euler_xyz, 1.0, 0.0, 0.0) 852 self.check_return_conversion(euler_to_R_xyz, R_to_euler_xyz, 0.0, 1.0, 0.0) 853 self.check_return_conversion(euler_to_R_xyz, R_to_euler_xyz, 0.0, 0.0, 1.0) 854 self.check_return_conversion(euler_to_R_xyz, R_to_euler_xyz, 1.0, 1.0, 0.0) 855 self.check_return_conversion(euler_to_R_xyz, R_to_euler_xyz, 0.0, 1.0, 1.0) 856 self.check_return_conversion(euler_to_R_xyz, R_to_euler_xyz, 1.0, 0.0, 1.0) 857 self.check_return_conversion(euler_to_R_xyz, R_to_euler_xyz, 1.0, 1.0, 1.0) 858 self.check_return_conversion(euler_to_R_xyz, R_to_euler_xyz, 1.0, pi/2, 0.5, alpha_end=0.5, gamma_end=0.0) 859 self.check_return_conversion(euler_to_R_xyz, R_to_euler_xyz, 1.0, pi, 0.5)

860 861

862 - def test_R_to_euler_xzx(self):

863 """Test the rotation matrix to xzx Euler angle conversion.""" 864 865 # Check random numbers, then the problematic angles. 866 self.check_return_conversion(euler_to_R_xzx, R_to_euler_xzx, uniform(0, 2*pi), uniform(0, pi), uniform(0, 2*pi)) 867 self.check_return_conversion(euler_to_R_xzx, R_to_euler_xzx, 0.0, 0.0, 0.0) 868 self.check_return_conversion(euler_to_R_xzx, R_to_euler_xzx, 1.0, 0.0, 0.0) 869 self.check_return_conversion(euler_to_R_xzx, R_to_euler_xzx, 0.0, 1.0, 0.0) 870 self.check_return_conversion(euler_to_R_xzx, R_to_euler_xzx, 0.0, 0.0, 1.0, alpha_end=1.0, gamma_end=0.0) 871 self.check_return_conversion(euler_to_R_xzx, R_to_euler_xzx, 1.0, 1.0, 0.0) 872 self.check_return_conversion(euler_to_R_xzx, R_to_euler_xzx, 0.0, 1.0, 1.0) 873 self.check_return_conversion(euler_to_R_xzx, R_to_euler_xzx, 1.0, 0.0, 1.0, alpha_end=2.0, gamma_end=0.0) 874 self.check_return_conversion(euler_to_R_xzx, R_to_euler_xzx, 1.0, 1.0, 1.0) 875 self.check_return_conversion(euler_to_R_xzx, R_to_euler_xzx, 1.0, pi/2, 0.5) 876 self.check_return_conversion(euler_to_R_xzx, R_to_euler_xzx, 1.0, pi, 0.5, alpha_end=0.5, gamma_end=0.0)

877 878

879 - def test_R_to_euler_xzy(self):

880 """Test the rotation matrix to xzy Euler angle conversion.""" 881 882 # Check random numbers, then the problematic angles. 883 self.check_return_conversion(euler_to_R_xzy, R_to_euler_xzy, uniform(0, 2*pi), uniform(0, pi), uniform(0, 2*pi)) 884 self.check_return_conversion(euler_to_R_xzy, R_to_euler_xzy, 0.0, 0.0, 0.0) 885 self.check_return_conversion(euler_to_R_xzy, R_to_euler_xzy, 1.0, 0.0, 0.0) 886 self.check_return_conversion(euler_to_R_xzy, R_to_euler_xzy, 0.0, 1.0, 0.0) 887 self.check_return_conversion(euler_to_R_xzy, R_to_euler_xzy, 0.0, 0.0, 1.0) 888 self.check_return_conversion(euler_to_R_xzy, R_to_euler_xzy, 1.0, 1.0, 0.0) 889 self.check_return_conversion(euler_to_R_xzy, R_to_euler_xzy, 0.0, 1.0, 1.0) 890 self.check_return_conversion(euler_to_R_xzy, R_to_euler_xzy, 1.0, 0.0, 1.0) 891 self.check_return_conversion(euler_to_R_xzy, R_to_euler_xzy, 1.0, 1.0, 1.0) 892 self.check_return_conversion(euler_to_R_xzy, R_to_euler_xzy, 1.0, pi/2, 0.5, alpha_end=1.5, gamma_end=0.0) 893 self.check_return_conversion(euler_to_R_xzy, R_to_euler_xzy, 1.0, pi, 0.5)

894 895

896 - def test_R_to_euler_yxy(self):

897 """Test the rotation matrix to yxy Euler angle conversion.""" 898 899 # Check random numbers, then the problematic angles. 900 self.check_return_conversion(euler_to_R_yxy, R_to_euler_yxy, uniform(0, 2*pi), uniform(0, pi), uniform(0, 2*pi)) 901 self.check_return_conversion(euler_to_R_yxy, R_to_euler_yxy, 0.0, 0.0, 0.0) 902 self.check_return_conversion(euler_to_R_yxy, R_to_euler_yxy, 1.0, 0.0, 0.0) 903 self.check_return_conversion(euler_to_R_yxy, R_to_euler_yxy, 0.0, 1.0, 0.0) 904 self.check_return_conversion(euler_to_R_yxy, R_to_euler_yxy, 0.0, 0.0, 1.0, alpha_end=1.0, gamma_end=0.0) 905 self.check_return_conversion(euler_to_R_yxy, R_to_euler_yxy, 1.0, 1.0, 0.0) 906 self.check_return_conversion(euler_to_R_yxy, R_to_euler_yxy, 0.0, 1.0, 1.0) 907 self.check_return_conversion(euler_to_R_yxy, R_to_euler_yxy, 1.0, 0.0, 1.0, alpha_end=2.0, gamma_end=0.0) 908 self.check_return_conversion(euler_to_R_yxy, R_to_euler_yxy, 1.0, 1.0, 1.0) 909 self.check_return_conversion(euler_to_R_yxy, R_to_euler_yxy, 1.0, pi/2, 0.5) 910 self.check_return_conversion(euler_to_R_yxy, R_to_euler_yxy, 1.0, pi, 0.5, alpha_end=0.5, gamma_end=0.0)

911 912

913 - def test_R_to_euler_yxz(self):

914 """Test the rotation matrix to yxz Euler angle conversion.""" 915 916 # Check random numbers, then the problematic angles. 917 self.check_return_conversion(euler_to_R_yxz, R_to_euler_yxz, uniform(0, 2*pi), uniform(0, pi), uniform(0, 2*pi)) 918 self.check_return_conversion(euler_to_R_yxz, R_to_euler_yxz, 0.0, 0.0, 0.0) 919 self.check_return_conversion(euler_to_R_yxz, R_to_euler_yxz, 1.0, 0.0, 0.0) 920 self.check_return_conversion(euler_to_R_yxz, R_to_euler_yxz, 0.0, 1.0, 0.0) 921 self.check_return_conversion(euler_to_R_yxz, R_to_euler_yxz, 0.0, 0.0, 1.0) 922 self.check_return_conversion(euler_to_R_yxz, R_to_euler_yxz, 1.0, 1.0, 0.0) 923 self.check_return_conversion(euler_to_R_yxz, R_to_euler_yxz, 0.0, 1.0, 1.0) 924 self.check_return_conversion(euler_to_R_yxz, R_to_euler_yxz, 1.0, 0.0, 1.0) 925 self.check_return_conversion(euler_to_R_yxz, R_to_euler_yxz, 1.0, 1.0, 1.0) 926 self.check_return_conversion(euler_to_R_yxz, R_to_euler_yxz, 1.0, pi/2, 0.5, alpha_end=1.5, gamma_end=0.0) 927 self.check_return_conversion(euler_to_R_yxz, R_to_euler_yxz, 1.0, pi, 0.5)

928 929

930 - def test_R_to_euler_yzx(self):

931 """Test the rotation matrix to yzx Euler angle conversion.""" 932 933 # Check random numbers, then the problematic angles. 934 self.check_return_conversion(euler_to_R_yzx, R_to_euler_yzx, uniform(0, 2*pi), uniform(0, pi), uniform(0, 2*pi)) 935 self.check_return_conversion(euler_to_R_yzx, R_to_euler_yzx, 0.0, 0.0, 0.0) 936 self.check_return_conversion(euler_to_R_yzx, R_to_euler_yzx, 1.0, 0.0, 0.0) 937 self.check_return_conversion(euler_to_R_yzx, R_to_euler_yzx, 0.0, 1.0, 0.0) 938 self.check_return_conversion(euler_to_R_yzx, R_to_euler_yzx, 0.0, 0.0, 1.0) 939 self.check_return_conversion(euler_to_R_yzx, R_to_euler_yzx, 1.0, 1.0, 0.0) 940 self.check_return_conversion(euler_to_R_yzx, R_to_euler_yzx, 0.0, 1.0, 1.0) 941 self.check_return_conversion(euler_to_R_yzx, R_to_euler_yzx, 1.0, 0.0, 1.0) 942 self.check_return_conversion(euler_to_R_yzx, R_to_euler_yzx, 1.0, 1.0, 1.0) 943 self.check_return_conversion(euler_to_R_yzx, R_to_euler_yzx, 1.0, pi/2, 0.5, alpha_end=0.5, gamma_end=0.0) 944 self.check_return_conversion(euler_to_R_yzx, R_to_euler_yzx, 1.0, pi, 0.5)

945 946

947 - def test_R_to_euler_yzy(self):

948 """Test the rotation matrix to yzy Euler angle conversion.""" 949 950 # Check random numbers, then the problematic angles. 951 self.check_return_conversion(euler_to_R_yzy, R_to_euler_yzy, uniform(0, 2*pi), uniform(0, pi), uniform(0, 2*pi)) 952 self.check_return_conversion(euler_to_R_yzy, R_to_euler_yzy, 0.0, 0.0, 0.0) 953 self.check_return_conversion(euler_to_R_yzy, R_to_euler_yzy, 1.0, 0.0, 0.0) 954 self.check_return_conversion(euler_to_R_yzy, R_to_euler_yzy, 0.0, 1.0, 0.0) 955 self.check_return_conversion(euler_to_R_yzy, R_to_euler_yzy, 0.0, 0.0, 1.0, alpha_end=1.0, gamma_end=0.0) 956 self.check_return_conversion(euler_to_R_yzy, R_to_euler_yzy, 1.0, 1.0, 0.0) 957 self.check_return_conversion(euler_to_R_yzy, R_to_euler_yzy, 0.0, 1.0, 1.0) 958 self.check_return_conversion(euler_to_R_yzy, R_to_euler_yzy, 1.0, 0.0, 1.0, alpha_end=2.0, gamma_end=0.0) 959 self.check_return_conversion(euler_to_R_yzy, R_to_euler_yzy, 1.0, 1.0, 1.0) 960 self.check_return_conversion(euler_to_R_yzy, R_to_euler_yzy, 1.0, pi/2, 0.5) 961 self.check_return_conversion(euler_to_R_yzy, R_to_euler_yzy, 1.0, pi, 0.5, alpha_end=0.5, gamma_end=0.0)

962 963

964 - def test_R_to_euler_zxy(self):

965 """Test the rotation matrix to zxy Euler angle conversion.""" 966 967 # Check random numbers, then the problematic angles. 968 self.check_return_conversion(euler_to_R_zxy, R_to_euler_zxy, uniform(0, 2*pi), uniform(0, pi), uniform(0, 2*pi)) 969 self.check_return_conversion(euler_to_R_zxy, R_to_euler_zxy, 0.0, 0.0, 0.0) 970 self.check_return_conversion(euler_to_R_zxy, R_to_euler_zxy, 1.0, 0.0, 0.0) 971 self.check_return_conversion(euler_to_R_zxy, R_to_euler_zxy, 0.0, 1.0, 0.0) 972 self.check_return_conversion(euler_to_R_zxy, R_to_euler_zxy, 0.0, 0.0, 1.0) 973 self.check_return_conversion(euler_to_R_zxy, R_to_euler_zxy, 1.0, 1.0, 0.0) 974 self.check_return_conversion(euler_to_R_zxy, R_to_euler_zxy, 0.0, 1.0, 1.0) 975 self.check_return_conversion(euler_to_R_zxy, R_to_euler_zxy, 1.0, 0.0, 1.0) 976 self.check_return_conversion(euler_to_R_zxy, R_to_euler_zxy, 1.0, 1.0, 1.0) 977 self.check_return_conversion(euler_to_R_zxy, R_to_euler_zxy, 1.0, pi/2, 0.5, alpha_end=0.5, gamma_end=0.0) 978 self.check_return_conversion(euler_to_R_zxy, R_to_euler_zxy, 1.0, pi, 0.5)

979 980

981 - def test_R_to_euler_zxz(self):

982 """Test the rotation matrix to zxz Euler angle conversion.""" 983 984 # Check random numbers, then the problematic angles. 985 self.check_return_conversion(euler_to_R_zxz, R_to_euler_zxz, uniform(0, 2*pi), uniform(0, pi), uniform(0, 2*pi)) 986 self.check_return_conversion(euler_to_R_zxz, R_to_euler_zxz, 0.0, 0.0, 0.0) 987 self.check_return_conversion(euler_to_R_zxz, R_to_euler_zxz, 1.0, 0.0, 0.0) 988 self.check_return_conversion(euler_to_R_zxz, R_to_euler_zxz, 0.0, 1.0, 0.0) 989 self.check_return_conversion(euler_to_R_zxz, R_to_euler_zxz, 0.0, 0.0, 1.0, alpha_end=1.0, gamma_end=0.0) 990 self.check_return_conversion(euler_to_R_zxz, R_to_euler_zxz, 1.0, 1.0, 0.0) 991 self.check_return_conversion(euler_to_R_zxz, R_to_euler_zxz, 0.0, 1.0, 1.0) 992 self.check_return_conversion(euler_to_R_zxz, R_to_euler_zxz, 1.0, 0.0, 1.0, alpha_end=2.0, gamma_end=0.0) 993 self.check_return_conversion(euler_to_R_zxz, R_to_euler_zxz, 1.0, 1.0, 1.0) 994 self.check_return_conversion(euler_to_R_zxz, R_to_euler_zxz, 1.0, pi/2, 0.5) 995 self.check_return_conversion(euler_to_R_zxz, R_to_euler_zxz, 1.0, pi, 0.5, alpha_end=0.5, gamma_end=0.0)

996 997

998 - def test_R_to_euler_zyx(self):

999 """Test the rotation matrix to zyx Euler angle conversion.""" 1000 1001 # Check random numbers, then the problematic angles. 1002 self.check_return_conversion(euler_to_R_zyx, R_to_euler_zyx, 5.0, 2.0, 1.0) 1003 self.check_return_conversion(euler_to_R_zyx, R_to_euler_zyx, uniform(0, 2*pi), uniform(0, pi), uniform(0, 2*pi)) 1004 self.check_return_conversion(euler_to_R_zyx, R_to_euler_zyx, 0.0, 0.0, 0.0) 1005 self.check_return_conversion(euler_to_R_zyx, R_to_euler_zyx, 1.0, 0.0, 0.0) 1006 self.check_return_conversion(euler_to_R_zyx, R_to_euler_zyx, 0.0, 1.0, 0.0) 1007 self.check_return_conversion(euler_to_R_zyx, R_to_euler_zyx, 0.0, 0.0, 1.0) 1008 self.check_return_conversion(euler_to_R_zyx, R_to_euler_zyx, 1.0, 1.0, 0.0) 1009 self.check_return_conversion(euler_to_R_zyx, R_to_euler_zyx, 0.0, 1.0, 1.0) 1010 self.check_return_conversion(euler_to_R_zyx, R_to_euler_zyx, 1.0, 0.0, 1.0) 1011 self.check_return_conversion(euler_to_R_zyx, R_to_euler_zyx, 1.0, 1.0, 1.0) 1012 self.check_return_conversion(euler_to_R_zyx, R_to_euler_zyx, 1.0, pi/2, 0.5, alpha_end=1.5, gamma_end=0.0) 1013 self.check_return_conversion(euler_to_R_zyx, R_to_euler_zyx, 1.0, pi, 0.5, alpha_end=1.0+pi, beta_end=0.0, gamma_end=0.5+pi)

1014 1015

1016 - def test_R_to_euler_zyz(self):

1017 """Test the rotation matrix to zyz Euler angle conversion.""" 1018 1019 # Check random numbers, then the problematic angles. 1020 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, uniform(0, 2*pi), uniform(0, pi), uniform(0, 2*pi)) 1021 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, 0.0, 0.0, 0.0) 1022 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, 1.0, 0.0, 0.0) 1023 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, 0.0, 1.0, 0.0) 1024 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, 0.0, 0.0, 1.0, alpha_end=1.0, gamma_end=0.0) 1025 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, 1.0, 1.0, 0.0) 1026 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, 0.0, 1.0, 1.0) 1027 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, 1.0, 0.0, 1.0, alpha_end=2.0, gamma_end=0.0) 1028 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, 1.0, 1.0, 1.0) 1029 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, 1.0, pi/2, 0.5) 1030 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, 1.0, pi, 0.5, alpha_end=0.5, gamma_end=0.0) 1031 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, 1.0, -pi/2, 0.5, alpha_end=1.0+pi, beta_end=pi/2, gamma_end=0.5+pi) 1032 self.check_return_conversion(euler_to_R_zyz, R_to_euler_zyz, 1.0, 1.5*pi, 0.5, alpha_end=1.0+pi, beta_end=pi/2, gamma_end=0.5+pi)

1033 1034

1035 - def test_R_to_quaternion_no_rot(self):

1036 """Test the rotation matrix to quaternion conversion for a zero angle rotation.""" 1037 1038 # Generate the rotation matrix. 1039 R = array([[1, 0, 0], [0, 1, 0], [0, 0, 1]], float64) 1040 1041 # The quaternion. 1042 quat = zeros(4, float64) 1043 R_to_quaternion(R, quat) 1044 print("Quaternion:\n%s" % quat) 1045 1046 # The correct result. 1047 quat_true = array([1, 0, 0, 0], float64) 1048 1049 # Checks. 1050 self.assertEqual(norm(quat), 1) 1051 for i in range(4): 1052 self.assertAlmostEqual(quat[i], quat_true[i])

1053 1054

1055 - def test_R_to_quaternion_180_complex(self):

1056 """Test the rotation matrix to quaternion conversion for a 180 degree rotation about [1, 1, 1].""" 1057 1058 # Generate the rotation matrix. 1059 R = array([[0, 0, 1], [1, 0, 0], [0, 1, 0]], float64) 1060 1061 # The quaternion. 1062 quat = zeros(4, float64) 1063 R_to_quaternion(R, quat) 1064 print("Quaternion:\n%s" % quat) 1065 1066 # The correct result. 1067 quat_true = array([1, 1, 1, 1], float64) / 2 1068 1069 # Checks. 1070 self.assertEqual(norm(quat), 1) 1071 for i in range(4): 1072 self.assertAlmostEqual(quat[i], quat_true[i])

1073 1074

1075 - def test_reverse_euler_zyz_a0_b0_g0(self):

1076 """Test the reverse Euler angle conversion for {0, 0, 0}.""" 1077 1078 # The forward angles. 1079 euler = [0.0, 0.0, 0.0] 1080 1081 # Convert twice. 1082 a, b, g = reverse_euler_zyz(euler[0], euler[1], euler[2]) 1083 a, b, g = reverse_euler_zyz(a, b, g) 1084 1085 # Check the reversed, reverse angles. 1086 self.assertAlmostEqual(a, euler[0]) 1087 self.assertAlmostEqual(b, euler[1]) 1088 self.assertAlmostEqual(g, euler[2])

1089 1090

1091 - def test_reverse_euler_zyz_a1_b0_g0(self):

1092 """Test the reverse Euler angle conversion for {1, 0, 0}.""" 1093 1094 # The forward angles. 1095 euler = [1.0, 0.0, 0.0] 1096 1097 # Convert twice. 1098 a, b, g = reverse_euler_zyz(euler[0], euler[1], euler[2]) 1099 a, b, g = reverse_euler_zyz(a, b, g) 1100 1101 # Check the reversed, reverse angles. 1102 self.assertAlmostEqual(a, euler[0]) 1103 self.assertAlmostEqual(b, euler[1]) 1104 self.assertAlmostEqual(g, euler[2])

1105 1106

1107 - def test_reverse_euler_zyz_a0_b1_g0(self):

1108 """Test the reverse Euler angle conversion for {0, 1, 0}.""" 1109 1110 # The forward angles. 1111 euler = [0.0, 1.0, 0.0] 1112 1113 # Convert twice. 1114 a, b, g = reverse_euler_zyz(euler[0], euler[1], euler[2]) 1115 a, b, g = reverse_euler_zyz(a, b, g) 1116 1117 # Check the reversed, reverse angles. 1118 self.assertAlmostEqual(a, euler[0]) 1119 self.assertAlmostEqual(b, euler[1]) 1120 self.assertAlmostEqual(g, euler[2])

1121 1122

1123 - def test_reverse_euler_zyz_a0_b0_g1(self):

1124 """Test the reverse Euler angle conversion for {0, 0, 1}.""" 1125 1126 # The forward angles. 1127 euler = [0.0, 0.0, 1.0] 1128 1129 # Convert twice. 1130 a, b, g = reverse_euler_zyz(euler[0], euler[1], euler[2]) 1131 a, b, g = reverse_euler_zyz(a, b, g) 1132 1133 # Check the reversed, reverse angles. 1134 self.assertAlmostEqual(a, euler[0]+euler[2]) 1135 self.assertAlmostEqual(b, euler[1]) 1136 self.assertAlmostEqual(g, 0.0)

1137 1138

1139 - def test_reverse_euler_zyz_a1_b1_g0(self):

1140 """Test the reverse Euler angle conversion for {1, 1, 0}.""" 1141 1142 # The forward angles. 1143 euler = [1.0, 1.0, 0.0] 1144 1145 # Convert twice. 1146 a, b, g = reverse_euler_zyz(euler[0], euler[1], euler[2]) 1147 a, b, g = reverse_euler_zyz(a, b, g) 1148 1149 # Check the reversed, reverse angles. 1150 self.assertAlmostEqual(a, euler[0]) 1151 self.assertAlmostEqual(b, euler[1]) 1152 self.assertAlmostEqual(g, euler[2])

1153 1154

1155 - def test_reverse_euler_zyz_a0_b1_g1(self):

1156 """Test the reverse Euler angle conversion for {0, 1, 1}.""" 1157 1158 # The forward angles. 1159 euler = [0.0, 1.0, 1.0] 1160 1161 # Convert twice. 1162 a, b, g = reverse_euler_zyz(euler[0], euler[1], euler[2]) 1163 a, b, g = reverse_euler_zyz(a, b, g) 1164 1165 # Check the reversed, reverse angles. 1166 self.assertAlmostEqual(a, euler[0]) 1167 self.assertAlmostEqual(b, euler[1]) 1168 self.assertAlmostEqual(g, euler[2])

1169 1170

1171 - def test_reverse_euler_zyz_a1_b0_g1(self):

1172 """Test the reverse Euler angle conversion for {1, 0, 1}.""" 1173 1174 # The forward angles. 1175 euler = [1.0, 0.0, 1.0] 1176 1177 # Convert twice. 1178 a, b, g = reverse_euler_zyz(euler[0], euler[1], euler[2]) 1179 a, b, g = reverse_euler_zyz(a, b, g) 1180 1181 # Check the reversed, reverse angles. 1182 self.assertAlmostEqual(a, euler[0]+euler[2]) 1183 self.assertAlmostEqual(b, euler[1]) 1184 self.assertAlmostEqual(g, 0.0)

1185 1186

1187 - def test_reverse_euler_zyz_a1_b1_g1(self):

1188 """Test the reverse Euler angle conversion for {1, 1, 1}.""" 1189 1190 # The forward angles. 1191 euler = [1.0, 1.0, 1.0] 1192 1193 # Convert twice. 1194 a, b, g = reverse_euler_zyz(euler[0], euler[1], euler[2]) 1195 a, b, g = reverse_euler_zyz(a, b, g) 1196 1197 # Check the reversed, reverse angles. 1198 self.assertAlmostEqual(a, euler[0]) 1199 self.assertAlmostEqual(b, euler[1]) 1200 self.assertAlmostEqual(g, euler[2])

1201 1202

1203 - def test_reverse_euler_zyz_a1_b0_5_g3(self):

1204 """Test the reverse Euler angle conversion for {1, 0.5, 3}.""" 1205 1206 # The forward angles. 1207 euler = [1.0, 0.5, 3.0] 1208 1209 # Convert twice. 1210 a, b, g = reverse_euler_zyz(euler[0], euler[1], euler[2]) 1211 a, b, g = reverse_euler_zyz(a, b, g) 1212 1213 # Check the reversed, reverse angles. 1214 self.assertAlmostEqual(a, euler[0]) 1215 self.assertAlmostEqual(b, euler[1]) 1216 self.assertAlmostEqual(g, euler[2])

1217 1218

1219 - def test_quaternion_to_axis_angle_no_rot(self):

1220 """Test the quaternion to rotation matrix conversion for a zero angle rotation.""" 1221 1222 # Quaternion of zero angle. 1223 quat = array([1, 0, 0, 0], float64) 1224 1225 # The axis and angle. 1226 axis, angle = quaternion_to_axis_angle(quat) 1227 print("Axis: %s" % axis) 1228 print("Angle: %s" % angle) 1229 1230 # The correct result. 1231 self.assertEqual(angle, 0.0) 1232 for i in range(3): 1233 self.assertEqual(axis[i], 0.0)

1234 1235

1236 - def test_quaternion_to_axis_angle_180_complex(self):

1237 """Test the quaternion to axis-angle conversion for a 180 degree rotation about [1, 1, 1].""" 1238 1239 # The quaternion. 1240 quat = array([0, 1, 1, 1], float64) / sqrt(3) 1241 1242 # The axis and angle. 1243 axis, angle = quaternion_to_axis_angle(quat) 1244 print("Axis: %s" % axis) 1245 print("Angle: %s" % angle) 1246 1247 # The correct result. 1248 self.assertEqual(angle, pi) 1249 for i in range(3): 1250 self.assertEqual(axis[i], 1.0 / sqrt(3))

1251 1252 1253

1254 - def test_quaternion_to_R_no_rot(self):

1255 """Test the quaternion to rotation matrix conversion for a zero angle rotation.""" 1256 1257 # Quaternion of zero angle. 1258 quat = array([1, 0, 0, 0], float64) 1259 1260 # The rotation matrix. 1261 quaternion_to_R(quat, R) 1262 print("Rotation matrix:\n%s" % R) 1263 1264 # The correct result. 1265 R_true = eye(3) 1266 1267 # Checks. 1268 for i in range(3): 1269 for j in range(3): 1270 self.assertEqual(R[i, j], R_true[i, j])

1271 1272

1273 - def test_quaternion_to_R_z_30(self):

1274 """Test the quaternion to rotation matrix conversion for a 30 degree rotation about z.""" 1275 1276 # Axis-angle values. 1277 axis = array([0, 0, 1], float64) 1278 angle = pi / 6 1279 1280 # First element. 1281 w = cos(angle / 2) 1282 1283 # Vector reduction (quaternion normalisation). 1284 factor = sqrt(1 - w**2) 1285 axis = axis * factor 1286 1287 # Quaternion. 1288 quat = zeros(4, float64) 1289 quat[0] = w 1290 quat[1:] = axis 1291 print("Quat: %s" % quat) 1292 print("Quat norm: %s" % norm(quat)) 1293 1294 # Quaternion check. 1295 w_check = cos(asin(sqrt(quat[1]**2 + quat[2]**2 + quat[3]**2))) 1296 self.assertEqual(w, w_check) 1297 self.assertEqual(norm(quat), 1) 1298 1299 # Generate the rotation matrix. 1300 quaternion_to_R(quat, R) 1301 print("Rotation matrix:\n%s" % R) 1302 1303 # Rotated pos axes (real values). 1304 x_real_pos = array([cos(pi/6), sin(pi/6), 0], float64) 1305 y_real_pos = array([-sin(pi/6), cos(pi/6), 0], float64) 1306 z_real_pos = array([0, 0, 1], float64) 1307 1308 # Rotated neg axes (real values). 1309 x_real_neg = array([-cos(pi/6), -sin(pi/6), 0], float64) 1310 y_real_neg = array([sin(pi/6), -cos(pi/6), 0], float64) 1311 z_real_neg = array([0, 0, -1], float64) 1312 1313 # Check the rotation. 1314 self.check_rotation(R, x_real_pos, y_real_pos, z_real_pos, x_real_neg, y_real_neg, z_real_neg)

1315 1316

1317 - def test_quaternion_to_R_180_complex(self):

1318 """Test the quaternion to rotation matrix conversion for a 180 degree rotation about [1, 1, 1].""" 1319 1320 # Axis-angle values. 1321 axis = array([1, 1, 1], float64) / sqrt(3) 1322 angle = 2 * pi / 3 1323 1324 # First element. 1325 w = cos(angle / 2) 1326 1327 # Vector reduction (quaternion normalisation). 1328 factor = sqrt(1 - w**2) 1329 axis = axis * factor 1330 1331 # Quaternion. 1332 quat = zeros(4, float64) 1333 quat[0] = w 1334 quat[1:] = axis 1335 print("Quat: %s" % quat) 1336 print("Quat norm: %s" % norm(quat)) 1337 1338 # Quaternion check. 1339 w_check = cos(asin(sqrt(quat[1]**2 + quat[2]**2 + quat[3]**2))) 1340 self.assertAlmostEqual(w, w_check) 1341 self.assertEqual(norm(quat), 1) 1342 1343 # Generate the rotation matrix. 1344 quaternion_to_R(quat, R) 1345 print("Rotation matrix:\n%s" % R) 1346 1347 # Rotated pos axes (real values). 1348 x_real_pos = array([0, 1, 0], float64) 1349 y_real_pos = array([0, 0, 1], float64) 1350 z_real_pos = array([1, 0, 0], float64) 1351 1352 # Rotated neg axes (real values). 1353 x_real_neg = array([0, -1, 0], float64) 1354 y_real_neg = array([0, 0, -1], float64) 1355 z_real_neg = array([-1, 0, 0], float64) 1356 1357 # Check the rotation. 1358 self.check_rotation(R, x_real_pos, y_real_pos, z_real_pos, x_real_neg, y_real_neg, z_real_neg)

Source Code for Module test_suite.unit_tests._lib._geometry.test_rotations