1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23 from copy import deepcopy
24 from math import asin, cos, pi, sin, sqrt
25 from numpy import array, dot, eye, float64, zeros
26 from numpy.linalg import norm
27 from random import shuffle, uniform
28 from unittest import TestCase
29
30
31 from lib.geometry.angles import wrap_angles
32 from lib.geometry.rotations import axis_angle_to_euler_xyx, axis_angle_to_euler_xyz, axis_angle_to_euler_xzx, axis_angle_to_euler_xzy, axis_angle_to_euler_yxy, axis_angle_to_euler_yxz, axis_angle_to_euler_yzx, axis_angle_to_euler_yzy, axis_angle_to_euler_zxy, axis_angle_to_euler_zxz, axis_angle_to_euler_zyx, axis_angle_to_euler_zyz, axis_angle_to_R, axis_angle_to_quaternion, euler_to_axis_angle_xyx, euler_to_axis_angle_xyz, euler_to_axis_angle_xzx, euler_to_axis_angle_xzy, euler_to_axis_angle_yxy, euler_to_axis_angle_yxz, euler_to_axis_angle_yzx, euler_to_axis_angle_yzy, euler_to_axis_angle_zxy, euler_to_axis_angle_zxz, euler_to_axis_angle_zyx, euler_to_axis_angle_zyz, euler_to_R_xyx, euler_to_R_xyz, euler_to_R_xzx, euler_to_R_xzy, euler_to_R_yxy, euler_to_R_yxz, euler_to_R_yzx, euler_to_R_yzy, euler_to_R_zxy, euler_to_R_zxz, euler_to_R_zyx, euler_to_R_zyz, R_random_hypersphere, R_to_axis_angle, R_to_euler_xyx, R_to_euler_xyz, R_to_euler_xzx, R_to_euler_xzy, R_to_euler_yxy, R_to_euler_yxz, R_to_euler_yzx, R_to_euler_yzy, R_to_euler_zxy, R_to_euler_zxz, R_to_euler_zyx, R_to_euler_zyz, R_to_quaternion, reverse_euler_zyz, quaternion_to_axis_angle, quaternion_to_R
33
34
35
36 R = zeros((3, 3), float64)
37 R2 = zeros((3, 3), float64)
38
39
41 """Unit tests for the lib.geometry.rotations relax module."""
42
44 """Set up data used by the unit tests."""
45
46
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
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
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
65 epsilon = 1e-15
66
67
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
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
82 alpha_new, beta_new, gamma_new = R_to_euler(R)
83
84
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
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
95 print_out = print_out + "New angles (wrapped) (%8.5f, %8.5f, %8.5f)\n" % (alpha_new, beta_new, gamma_new)
96
97
98 if abs(beta_end - beta_new) > 1e-7:
99
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
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
115 print_out = print_out + "New angles (2nd sol) (%8.5f, %8.5f, %8.5f)\n" % (alpha_new, beta_new, gamma_new)
116
117
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
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
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
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
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
168 """Test the quaternion to rotation matrix conversion for a zero angle rotation."""
169
170
171 axis = array([-1, 1, 1], float64) / sqrt(3)
172 angle = 0.0
173
174
175 axis_angle_to_R(axis, angle, R)
176 print("Rotation matrix:\n%s" % R)
177
178
179 R_true = eye(3)
180
181
182 for i in range(3):
183 for j in range(3):
184 self.assertEqual(R[i, j], R_true[i, j])
185
186
188 """Test the axis-angle to quaternion conversion for a zero angle rotation."""
189
190
191 axis = array([-1, 1, 1], float64) / sqrt(3)
192 angle = 0.0
193
194
195 quat = zeros(4, float64)
196 axis_angle_to_quaternion(axis, angle, quat)
197 print("Quaternion:\n%s" % quat)
198
199
200 quat_true = array([1, 0, 0, 0], float64)
201
202
203 for i in range(4):
204 self.assertEqual(quat[i], quat_true[i])
205
206
208 """Test the axis-angle to quaternion conversion for a 180 degree rotation about [1, 1, 1]."""
209
210
211 axis = array([1, 1, 1], float64) / sqrt(3)
212 angle = pi
213
214
215 quat = zeros(4, float64)
216 axis_angle_to_quaternion(axis, angle, quat)
217 print("Quaternion:\n%s" % quat)
218
219
220 quat_true = array([0, 1, 1, 1], float64) / sqrt(3)
221
222
223 for i in range(4):
224 self.assertAlmostEqual(quat[i], quat_true[i])
225
226
228 """Test the quaternion to rotation matrix conversion for a 30 degree rotation about z."""
229
230
231 axis = array([0, 0, 1], float64)
232 angle = pi / 6
233
234
235 axis_angle_to_R(axis, angle, R)
236 print("Rotation matrix:\n%s" % R)
237
238
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
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
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
253 """Test the quaternion to rotation matrix conversion for a 180 degree rotation about [1, 1, 1]."""
254
255
256 axis = array([1, 1, 1], float64) / sqrt(3)
257 angle = 2 * pi / 3
258
259
260 axis_angle_to_R(axis, angle, R)
261 print("Rotation matrix:\n%s" % R)
262
263
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
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
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
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
371 alpha_init = uniform(0, 2*pi)
372 beta_init = uniform(0, pi)
373 gamma_init = uniform(0, 2*pi)
374
375
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
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
387 sets = ['xyx', 'xyz', 'xzx', 'xzy', 'yxy', 'yxz', 'yzx', 'yzy', 'zxy', 'zxz', 'zyx', 'zyz']
388 shuffle(sets)
389
390
391 for set in sets:
392
393 print("\n\n# %s cycle.\n" % set)
394
395
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
402 a, b, g = R_to_euler(R)
403 print("R -> Euler: [%-8.5f, %-8.5f, %-8.5f]\n" % (a, b, g))
404
405
406 euler_to_R(a, b, g, R)
407 print("Euler -> R:\n%s\n" % R)
408
409
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
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
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
422 axis_angle_to_R(axis, angle, R)
423 print("axis, angle -> R:\n%s\n" % R)
424
425
426 print("Rotation matrix difference:\n%s\n" % (R2-R))
427
428
429 for i in range(3):
430 for j in range(3):
431 self.assertAlmostEqual(R[i, j], R2[i, j])
432
433
434 alpha_end, beta_end, gamma_end = R_to_euler_xyx(R)
435
436
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
444 self.assertAlmostEqual(alpha_init, alpha_end)
445 self.assertAlmostEqual(beta_init, beta_end)
446 self.assertAlmostEqual(gamma_init, gamma_end)
447
448
450 """Bounce around all the conversion functions to see if the original angles are returned."""
451
452
453 alpha = uniform(0, 2*pi)
454 beta = uniform(0, pi)
455 gamma = uniform(0, 2*pi)
456
457
458 print("Original angles:")
459 print("alpha: %s" % alpha)
460 print("beta: %s" % beta)
461 print("gamma: %s\n" % gamma)
462
463
464 axis = zeros(3, float64)
465 quat = zeros(4, float64)
466
467
468 euler_to_R_zyz(alpha, beta, gamma, R)
469
470
471 axis, angle = R_to_axis_angle(R)
472
473
474 axis_angle_to_quaternion(axis, angle, quat)
475
476
477 axis, angle = quaternion_to_axis_angle(quat)
478
479
480 axis_angle_to_R(axis, angle, R)
481
482
483 R_to_quaternion(R, quat)
484
485
486 quaternion_to_R(quat, R)
487
488
489 alpha_new, beta_new, gamma_new = R_to_euler_zyz(R)
490
491
492 axis, angle = euler_to_axis_angle_zyz(alpha_new, beta_new, gamma_new)
493
494
495 alpha_new, beta_new, gamma_new = axis_angle_to_euler_zyz(axis, angle)
496
497
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
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
509 self.assertAlmostEqual(alpha, alpha_new)
510 self.assertAlmostEqual(beta, beta_new)
511 self.assertAlmostEqual(gamma, gamma_new)
512
513
515 """Test the rotation matrix from zyz Euler angle conversion using a beta angle of pi/4."""
516
517
518 euler_to_R_zyz(pi/6, 0.0, 0.0, R)
519
520
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
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
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
535 """Test the rotation matrix from zyz Euler angle conversion using a beta angle of pi/4."""
536
537
538 euler_to_R_zyz(0.0, pi/4, 0.0, R)
539
540
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
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
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
555 """Test the rotation matrix from zyz Euler angle conversion using a beta angle of pi/4."""
556
557
558 euler_to_R_zyz(0.0, 0.0, pi/12, R)
559
560
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
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
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
575 """Test the rotation matrix from zyz Euler angle conversion using a beta angle of pi/4."""
576
577
578 euler_to_R_zyz(pi/12, 0.0, pi/12, R)
579
580
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
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
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
595 """Test the rotation matrix to axis-angle conversion."""
596
597
598 R = array([[1, 0, 0], [0, 1, 0], [0, 0, 1]], float64)
599
600
601 axis, angle = R_to_axis_angle(R)
602
603
604 self.assertEqual(angle, 0.0)
605
606
608 """Test the rotation matrix to axis-angle conversion."""
609
610
611 R = array([[0, 0, 1], [1, 0, 0], [0, 1, 0]], float64)
612
613
614 axis, angle = R_to_axis_angle(R)
615
616
617 self.assertAlmostEqual(angle, 2 * pi / 3)
618
619
620 for i in range(3):
621 self.assertAlmostEqual(axis[i], 1.0/sqrt(3))
622
623
625 """Test the rotation matrix to xyx Euler angle conversion and back again."""
626
627
628 R_random_hypersphere(R)
629 R_orig = deepcopy(R)
630
631
632 a, b, g = R_to_euler_xyx(R)
633 euler_to_R_xyx(a, b, g, R2)
634
635
636 for i in range(3):
637 for j in range(3):
638 self.assertAlmostEqual(R_orig[i, j], R2[i, j])
639
640
642 """Test the rotation matrix to xyz Euler angle conversion and back again."""
643
644
645 R_random_hypersphere(R)
646 R_orig = deepcopy(R)
647
648
649 a, b, g = R_to_euler_xyz(R)
650 euler_to_R_xyz(a, b, g, R2)
651
652
653 for i in range(3):
654 for j in range(3):
655 self.assertAlmostEqual(R_orig[i, j], R2[i, j])
656
657
659 """Test the rotation matrix to xzx Euler angle conversion and back again."""
660
661
662 R_random_hypersphere(R)
663 R_orig = deepcopy(R)
664
665
666 a, b, g = R_to_euler_xzx(R)
667 euler_to_R_xzx(a, b, g, R2)
668
669
670 for i in range(3):
671 for j in range(3):
672 self.assertAlmostEqual(R_orig[i, j], R2[i, j])
673
674
676 """Test the rotation matrix to xzy Euler angle conversion and back again."""
677
678
679 R_random_hypersphere(R)
680 R_orig = deepcopy(R)
681
682
683 a, b, g = R_to_euler_xzy(R)
684 euler_to_R_xzy(a, b, g, R2)
685
686
687 for i in range(3):
688 for j in range(3):
689 self.assertAlmostEqual(R_orig[i, j], R2[i, j])
690
691
693 """Test the rotation matrix to yxy Euler angle conversion and back again."""
694
695
696 R_random_hypersphere(R)
697 R_orig = deepcopy(R)
698
699
700 a, b, g = R_to_euler_yxy(R)
701 euler_to_R_yxy(a, b, g, R2)
702
703
704 for i in range(3):
705 for j in range(3):
706 self.assertAlmostEqual(R_orig[i, j], R2[i, j])
707
708
710 """Test the rotation matrix to yxz Euler angle conversion and back again."""
711
712
713 R_random_hypersphere(R)
714 R_orig = deepcopy(R)
715
716
717 a, b, g = R_to_euler_yxz(R)
718 euler_to_R_yxz(a, b, g, R2)
719
720
721 for i in range(3):
722 for j in range(3):
723 self.assertAlmostEqual(R_orig[i, j], R2[i, j])
724
725
727 """Test the rotation matrix to yzx Euler angle conversion and back again."""
728
729
730 R_random_hypersphere(R)
731 R_orig = deepcopy(R)
732
733
734 a, b, g = R_to_euler_yzx(R)
735 euler_to_R_yzx(a, b, g, R2)
736
737
738 for i in range(3):
739 for j in range(3):
740 self.assertAlmostEqual(R_orig[i, j], R2[i, j])
741
742
744 """Test the rotation matrix to yzy Euler angle conversion and back again."""
745
746
747 R_random_hypersphere(R)
748 R_orig = deepcopy(R)
749
750
751 a, b, g = R_to_euler_yzy(R)
752 euler_to_R_yzy(a, b, g, R2)
753
754
755 for i in range(3):
756 for j in range(3):
757 self.assertAlmostEqual(R_orig[i, j], R2[i, j])
758
759
761 """Test the rotation matrix to zxy Euler angle conversion and back again."""
762
763
764 R_random_hypersphere(R)
765 R_orig = deepcopy(R)
766
767
768 a, b, g = R_to_euler_zxy(R)
769 euler_to_R_zxy(a, b, g, R2)
770
771
772 for i in range(3):
773 for j in range(3):
774 self.assertAlmostEqual(R_orig[i, j], R2[i, j])
775
776
778 """Test the rotation matrix to zxz Euler angle conversion and back again."""
779
780
781 R_random_hypersphere(R)
782 R_orig = deepcopy(R)
783
784
785 a, b, g = R_to_euler_zxz(R)
786 euler_to_R_zxz(a, b, g, R2)
787
788
789 for i in range(3):
790 for j in range(3):
791 self.assertAlmostEqual(R_orig[i, j], R2[i, j])
792
793
795 """Test the rotation matrix to zyx Euler angle conversion and back again."""
796
797
798 R_random_hypersphere(R)
799 R_orig = deepcopy(R)
800
801
802 a, b, g = R_to_euler_zyx(R)
803 euler_to_R_zyx(a, b, g, R2)
804
805
806 for i in range(3):
807 for j in range(3):
808 self.assertAlmostEqual(R_orig[i, j], R2[i, j])
809
810
812 """Test the rotation matrix to zyz Euler angle conversion and back again."""
813
814
815 R_random_hypersphere(R)
816 R_orig = deepcopy(R)
817
818
819 a, b, g = R_to_euler_zyz(R)
820 euler_to_R_zyz(a, b, g, R2)
821
822
823 for i in range(3):
824 for j in range(3):
825 self.assertAlmostEqual(R_orig[i, j], R2[i, j])
826
827
829 """Test the rotation matrix to xyx Euler angle conversion."""
830
831
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
846 """Test the rotation matrix to xyz Euler angle conversion."""
847
848
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
863 """Test the rotation matrix to xzx Euler angle conversion."""
864
865
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
880 """Test the rotation matrix to xzy Euler angle conversion."""
881
882
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
897 """Test the rotation matrix to yxy Euler angle conversion."""
898
899
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
914 """Test the rotation matrix to yxz Euler angle conversion."""
915
916
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
931 """Test the rotation matrix to yzx Euler angle conversion."""
932
933
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
948 """Test the rotation matrix to yzy Euler angle conversion."""
949
950
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
965 """Test the rotation matrix to zxy Euler angle conversion."""
966
967
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
982 """Test the rotation matrix to zxz Euler angle conversion."""
983
984
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
999 """Test the rotation matrix to zyx Euler angle conversion."""
1000
1001
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
1017 """Test the rotation matrix to zyz Euler angle conversion."""
1018
1019
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
1036 """Test the rotation matrix to quaternion conversion for a zero angle rotation."""
1037
1038
1039 R = array([[1, 0, 0], [0, 1, 0], [0, 0, 1]], float64)
1040
1041
1042 quat = zeros(4, float64)
1043 R_to_quaternion(R, quat)
1044 print("Quaternion:\n%s" % quat)
1045
1046
1047 quat_true = array([1, 0, 0, 0], float64)
1048
1049
1050 self.assertEqual(norm(quat), 1)
1051 for i in range(4):
1052 self.assertAlmostEqual(quat[i], quat_true[i])
1053
1054
1056 """Test the rotation matrix to quaternion conversion for a 180 degree rotation about [1, 1, 1]."""
1057
1058
1059 R = array([[0, 0, 1], [1, 0, 0], [0, 1, 0]], float64)
1060
1061
1062 quat = zeros(4, float64)
1063 R_to_quaternion(R, quat)
1064 print("Quaternion:\n%s" % quat)
1065
1066
1067 quat_true = array([1, 1, 1, 1], float64) / 2
1068
1069
1070 self.assertEqual(norm(quat), 1)
1071 for i in range(4):
1072 self.assertAlmostEqual(quat[i], quat_true[i])
1073
1074
1076 """Test the reverse Euler angle conversion for {0, 0, 0}."""
1077
1078
1079 euler = [0.0, 0.0, 0.0]
1080
1081
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
1086 self.assertAlmostEqual(a, euler[0])
1087 self.assertAlmostEqual(b, euler[1])
1088 self.assertAlmostEqual(g, euler[2])
1089
1090
1092 """Test the reverse Euler angle conversion for {1, 0, 0}."""
1093
1094
1095 euler = [1.0, 0.0, 0.0]
1096
1097
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
1102 self.assertAlmostEqual(a, euler[0])
1103 self.assertAlmostEqual(b, euler[1])
1104 self.assertAlmostEqual(g, euler[2])
1105
1106
1108 """Test the reverse Euler angle conversion for {0, 1, 0}."""
1109
1110
1111 euler = [0.0, 1.0, 0.0]
1112
1113
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
1118 self.assertAlmostEqual(a, euler[0])
1119 self.assertAlmostEqual(b, euler[1])
1120 self.assertAlmostEqual(g, euler[2])
1121
1122
1124 """Test the reverse Euler angle conversion for {0, 0, 1}."""
1125
1126
1127 euler = [0.0, 0.0, 1.0]
1128
1129
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
1134 self.assertAlmostEqual(a, euler[0]+euler[2])
1135 self.assertAlmostEqual(b, euler[1])
1136 self.assertAlmostEqual(g, 0.0)
1137
1138
1140 """Test the reverse Euler angle conversion for {1, 1, 0}."""
1141
1142
1143 euler = [1.0, 1.0, 0.0]
1144
1145
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
1150 self.assertAlmostEqual(a, euler[0])
1151 self.assertAlmostEqual(b, euler[1])
1152 self.assertAlmostEqual(g, euler[2])
1153
1154
1156 """Test the reverse Euler angle conversion for {0, 1, 1}."""
1157
1158
1159 euler = [0.0, 1.0, 1.0]
1160
1161
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
1166 self.assertAlmostEqual(a, euler[0])
1167 self.assertAlmostEqual(b, euler[1])
1168 self.assertAlmostEqual(g, euler[2])
1169
1170
1172 """Test the reverse Euler angle conversion for {1, 0, 1}."""
1173
1174
1175 euler = [1.0, 0.0, 1.0]
1176
1177
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
1182 self.assertAlmostEqual(a, euler[0]+euler[2])
1183 self.assertAlmostEqual(b, euler[1])
1184 self.assertAlmostEqual(g, 0.0)
1185
1186
1188 """Test the reverse Euler angle conversion for {1, 1, 1}."""
1189
1190
1191 euler = [1.0, 1.0, 1.0]
1192
1193
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
1198 self.assertAlmostEqual(a, euler[0])
1199 self.assertAlmostEqual(b, euler[1])
1200 self.assertAlmostEqual(g, euler[2])
1201
1202
1204 """Test the reverse Euler angle conversion for {1, 0.5, 3}."""
1205
1206
1207 euler = [1.0, 0.5, 3.0]
1208
1209
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
1214 self.assertAlmostEqual(a, euler[0])
1215 self.assertAlmostEqual(b, euler[1])
1216 self.assertAlmostEqual(g, euler[2])
1217
1218
1220 """Test the quaternion to rotation matrix conversion for a zero angle rotation."""
1221
1222
1223 quat = array([1, 0, 0, 0], float64)
1224
1225
1226 axis, angle = quaternion_to_axis_angle(quat)
1227 print("Axis: %s" % axis)
1228 print("Angle: %s" % angle)
1229
1230
1231 self.assertEqual(angle, 0.0)
1232 for i in range(3):
1233 self.assertEqual(axis[i], 0.0)
1234
1235
1237 """Test the quaternion to axis-angle conversion for a 180 degree rotation about [1, 1, 1]."""
1238
1239
1240 quat = array([0, 1, 1, 1], float64) / sqrt(3)
1241
1242
1243 axis, angle = quaternion_to_axis_angle(quat)
1244 print("Axis: %s" % axis)
1245 print("Angle: %s" % angle)
1246
1247
1248 self.assertEqual(angle, pi)
1249 for i in range(3):
1250 self.assertEqual(axis[i], 1.0 / sqrt(3))
1251
1252
1253
1255 """Test the quaternion to rotation matrix conversion for a zero angle rotation."""
1256
1257
1258 quat = array([1, 0, 0, 0], float64)
1259
1260
1261 quaternion_to_R(quat, R)
1262 print("Rotation matrix:\n%s" % R)
1263
1264
1265 R_true = eye(3)
1266
1267
1268 for i in range(3):
1269 for j in range(3):
1270 self.assertEqual(R[i, j], R_true[i, j])
1271
1272
1274 """Test the quaternion to rotation matrix conversion for a 30 degree rotation about z."""
1275
1276
1277 axis = array([0, 0, 1], float64)
1278 angle = pi / 6
1279
1280
1281 w = cos(angle / 2)
1282
1283
1284 factor = sqrt(1 - w**2)
1285 axis = axis * factor
1286
1287
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
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
1300 quaternion_to_R(quat, R)
1301 print("Rotation matrix:\n%s" % R)
1302
1303
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
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
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
1318 """Test the quaternion to rotation matrix conversion for a 180 degree rotation about [1, 1, 1]."""
1319
1320
1321 axis = array([1, 1, 1], float64) / sqrt(3)
1322 angle = 2 * pi / 3
1323
1324
1325 w = cos(angle / 2)
1326
1327
1328 factor = sqrt(1 - w**2)
1329 axis = axis * factor
1330
1331
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
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
1344 quaternion_to_R(quat, R)
1345 print("Rotation matrix:\n%s" % R)
1346
1347
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
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
1358 self.check_rotation(R, x_real_pos, y_real_pos, z_real_pos, x_real_neg, y_real_neg, z_real_neg)
1359