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 acos, asin, atan2, cos, pi, sin, sqrt
25 from numpy import array, cross, dot, float64, hypot, sign, transpose, zeros
26 from numpy.linalg import norm
27 from random import gauss, uniform
28
29
30 import generic_fns
31
32
33
34 EULER_NEXT = [1, 2, 0, 1]
35 EULER_TRANS_TABLE = {
36 'xzx': [0, 1, 1],
37 'yxy': [1, 1, 1],
38 'zyz': [2, 1, 1],
39
40 'xzy': [0, 1, 0],
41 'yxz': [1, 1, 0],
42 'zyx': [2, 1, 0],
43
44 'xyx': [0, 0, 1],
45 'yzy': [1, 0, 1],
46 'zxz': [2, 0, 1],
47
48 'xyz': [0, 0, 0],
49 'yzx': [1, 0, 0],
50 'zxy': [2, 0, 0]
51 }
52 EULER_EPSILON = 1e-5
53
54
55
57 """Convert the axis-angle notation to xyx Euler angles.
58
59 This first generates a rotation matrix via axis_angle_to_R() and then used this together with R_to_euler_xyx() to obtain the Euler angles.
60
61 @param axis: The 3D rotation axis.
62 @type axis: numpy array, len 3
63 @param angle: The rotation angle.
64 @type angle: float
65 @return: The alpha, beta, and gamma Euler angles in the xyx convention.
66 @rtype: float, float, float
67 """
68
69
70 R = zeros((3, 3), float64)
71
72
73 axis_angle_to_R(axis, angle, R)
74
75
76 return R_to_euler_xyx(R)
77
78
80 """Convert the axis-angle notation to xyz Euler angles.
81
82 This first generates a rotation matrix via axis_angle_to_R() and then used this together with R_to_euler_xyz() to obtain the Euler angles.
83
84 @param axis: The 3D rotation axis.
85 @type axis: numpy array, len 3
86 @param angle: The rotation angle.
87 @type angle: float
88 @return: The alpha, beta, and gamma Euler angles in the xyz convention.
89 @rtype: float, float, float
90 """
91
92
93 R = zeros((3, 3), float64)
94
95
96 axis_angle_to_R(axis, angle, R)
97
98
99 return R_to_euler_xyz(R)
100
101
103 """Convert the axis-angle notation to xzx Euler angles.
104
105 This first generates a rotation matrix via axis_angle_to_R() and then used this together with R_to_euler_xzx() to obtain the Euler angles.
106
107 @param axis: The 3D rotation axis.
108 @type axis: numpy array, len 3
109 @param angle: The rotation angle.
110 @type angle: float
111 @return: The alpha, beta, and gamma Euler angles in the xzx convention.
112 @rtype: float, float, float
113 """
114
115
116 R = zeros((3, 3), float64)
117
118
119 axis_angle_to_R(axis, angle, R)
120
121
122 return R_to_euler_xzx(R)
123
124
126 """Convert the axis-angle notation to xzy Euler angles.
127
128 This first generates a rotation matrix via axis_angle_to_R() and then used this together with R_to_euler_xzy() to obtain the Euler angles.
129
130 @param axis: The 3D rotation axis.
131 @type axis: numpy array, len 3
132 @param angle: The rotation angle.
133 @type angle: float
134 @return: The alpha, beta, and gamma Euler angles in the xzy convention.
135 @rtype: float, float, float
136 """
137
138
139 R = zeros((3, 3), float64)
140
141
142 axis_angle_to_R(axis, angle, R)
143
144
145 return R_to_euler_xzy(R)
146
147
149 """Convert the axis-angle notation to yxy Euler angles.
150
151 This first generates a rotation matrix via axis_angle_to_R() and then used this together with R_to_euler_yxy() to obtain the Euler angles.
152
153 @param axis: The 3D rotation axis.
154 @type axis: numpy array, len 3
155 @param angle: The rotation angle.
156 @type angle: float
157 @return: The alpha, beta, and gamma Euler angles in the yxy convention.
158 @rtype: float, float, float
159 """
160
161
162 R = zeros((3, 3), float64)
163
164
165 axis_angle_to_R(axis, angle, R)
166
167
168 return R_to_euler_yxy(R)
169
170
172 """Convert the axis-angle notation to yxz Euler angles.
173
174 This first generates a rotation matrix via axis_angle_to_R() and then used this together with R_to_euler_yxz() to obtain the Euler angles.
175
176 @param axis: The 3D rotation axis.
177 @type axis: numpy array, len 3
178 @param angle: The rotation angle.
179 @type angle: float
180 @return: The alpha, beta, and gamma Euler angles in the yxz convention.
181 @rtype: float, float, float
182 """
183
184
185 R = zeros((3, 3), float64)
186
187
188 axis_angle_to_R(axis, angle, R)
189
190
191 return R_to_euler_yxz(R)
192
193
195 """Convert the axis-angle notation to yzx Euler angles.
196
197 This first generates a rotation matrix via axis_angle_to_R() and then used this together with R_to_euler_yzx() to obtain the Euler angles.
198
199 @param axis: The 3D rotation axis.
200 @type axis: numpy array, len 3
201 @param angle: The rotation angle.
202 @type angle: float
203 @return: The alpha, beta, and gamma Euler angles in the yzx convention.
204 @rtype: float, float, float
205 """
206
207
208 R = zeros((3, 3), float64)
209
210
211 axis_angle_to_R(axis, angle, R)
212
213
214 return R_to_euler_yzx(R)
215
216
218 """Convert the axis-angle notation to yzy Euler angles.
219
220 This first generates a rotation matrix via axis_angle_to_R() and then used this together with R_to_euler_yzy() to obtain the Euler angles.
221
222 @param axis: The 3D rotation axis.
223 @type axis: numpy array, len 3
224 @param angle: The rotation angle.
225 @type angle: float
226 @return: The alpha, beta, and gamma Euler angles in the yzy convention.
227 @rtype: float, float, float
228 """
229
230
231 R = zeros((3, 3), float64)
232
233
234 axis_angle_to_R(axis, angle, R)
235
236
237 return R_to_euler_yzy(R)
238
239
241 """Convert the axis-angle notation to zxy Euler angles.
242
243 This first generates a rotation matrix via axis_angle_to_R() and then used this together with R_to_euler_zxy() to obtain the Euler angles.
244
245 @param axis: The 3D rotation axis.
246 @type axis: numpy array, len 3
247 @param angle: The rotation angle.
248 @type angle: float
249 @return: The alpha, beta, and gamma Euler angles in the zxy convention.
250 @rtype: float, float, float
251 """
252
253
254 R = zeros((3, 3), float64)
255
256
257 axis_angle_to_R(axis, angle, R)
258
259
260 return R_to_euler_zxy(R)
261
262
264 """Convert the axis-angle notation to zxz Euler angles.
265
266 This first generates a rotation matrix via axis_angle_to_R() and then used this together with R_to_euler_zxz() to obtain the Euler angles.
267
268 @param axis: The 3D rotation axis.
269 @type axis: numpy array, len 3
270 @param angle: The rotation angle.
271 @type angle: float
272 @return: The alpha, beta, and gamma Euler angles in the zxz convention.
273 @rtype: float, float, float
274 """
275
276
277 R = zeros((3, 3), float64)
278
279
280 axis_angle_to_R(axis, angle, R)
281
282
283 return R_to_euler_zxz(R)
284
285
287 """Convert the axis-angle notation to zyx Euler angles.
288
289 This first generates a rotation matrix via axis_angle_to_R() and then used this together with R_to_euler_zyx() to obtain the Euler angles.
290
291 @param axis: The 3D rotation axis.
292 @type axis: numpy array, len 3
293 @param angle: The rotation angle.
294 @type angle: float
295 @return: The alpha, beta, and gamma Euler angles in the zyx convention.
296 @rtype: float, float, float
297 """
298
299
300 R = zeros((3, 3), float64)
301
302
303 axis_angle_to_R(axis, angle, R)
304
305
306 return R_to_euler_zyx(R)
307
308
310 """Convert the axis-angle notation to zyz Euler angles.
311
312 This first generates a rotation matrix via axis_angle_to_R() and then used this together with R_to_euler_zyz() to obtain the Euler angles.
313
314 @param axis: The 3D rotation axis.
315 @type axis: numpy array, len 3
316 @param angle: The rotation angle.
317 @type angle: float
318 @return: The alpha, beta, and gamma Euler angles in the zyz convention.
319 @rtype: float, float, float
320 """
321
322
323 R = zeros((3, 3), float64)
324
325
326 axis_angle_to_R(axis, angle, R)
327
328
329 return R_to_euler_zyz(R)
330
331
333 """Generate the rotation matrix from the axis-angle notation.
334
335 Conversion equations
336 ====================
337
338 From Wikipedia (http://en.wikipedia.org/wiki/Rotation_matrix), the conversion is given by::
339
340 c = cos(angle); s = sin(angle); C = 1-c
341 xs = x*s; ys = y*s; zs = z*s
342 xC = x*C; yC = y*C; zC = z*C
343 xyC = x*yC; yzC = y*zC; zxC = z*xC
344 [ x*xC+c xyC-zs zxC+ys ]
345 [ xyC+zs y*yC+c yzC-xs ]
346 [ zxC-ys yzC+xs z*zC+c ]
347
348
349 @param axis: The 3D rotation axis.
350 @type axis: numpy array, len 3
351 @param angle: The rotation angle.
352 @type angle: float
353 @param R: The 3x3 rotation matrix to update.
354 @type R: 3x3 numpy array
355 """
356
357
358 ca = cos(angle)
359 sa = sin(angle)
360 C = 1 - ca
361
362
363 x, y, z = axis
364
365
366 xs = x*sa
367 ys = y*sa
368 zs = z*sa
369 xC = x*C
370 yC = y*C
371 zC = z*C
372 xyC = x*yC
373 yzC = y*zC
374 zxC = z*xC
375
376
377 R[0, 0] = x*xC + ca
378 R[0, 1] = xyC - zs
379 R[0, 2] = zxC + ys
380 R[1, 0] = xyC + zs
381 R[1, 1] = y*yC + ca
382 R[1, 2] = yzC - xs
383 R[2, 0] = zxC - ys
384 R[2, 1] = yzC + xs
385 R[2, 2] = z*zC + ca
386
387
389 """Generate the quaternion from the axis-angle notation.
390
391 Conversion equations
392 ====================
393
394 From Wolfram MathWorld (http://mathworld.wolfram.com/Quaternion.html), the conversion is given by::
395
396 q = (cos(angle/2), n * sin(angle/2)),
397
398 where q is the quaternion and n is the unit vector representing the rotation axis.
399
400
401 @param axis: The 3D rotation axis.
402 @type axis: numpy array, len 3
403 @param angle: The rotation angle.
404 @type angle: float
405 @param quat: The quaternion structure.
406 @type quat: numpy 4D, rank-1 array
407 @keyword norm_flag: A flag which if True forces the axis to be converted to a unit vector.
408 @type norm_flag: bool
409 """
410
411
412 if norm_flag:
413 axis = axis / norm(axis)
414
415
416 quat[0] = cos(angle/2)
417
418
419 quat[1:] = axis * sin(angle/2)
420
421
423 """Return x with the sign of y.
424
425 This is defined as::
426
427 copysign(x, y) = abs(x) / abs(y) * y
428
429
430 @param x: The value.
431 @type x: float
432 @param y: The value.
433 @type y: float
434 @return: x with the sign of y.
435 @rtype: float
436 """
437
438
439 return abs(x) / abs(y) * y
440
441
443 """Convert the xyx Euler angles to axis-angle notation.
444
445 This function first generates a rotation matrix via euler_*_to_R() and then uses R_to_axis_angle() to convert to the axis and angle notation.
446
447 @param alpha: The alpha Euler angle in rad.
448 @type alpha: float
449 @param beta: The beta Euler angle in rad.
450 @type beta: float
451 @param gamma: The gamma Euler angle in rad.
452 @type gamma: float
453 @return: The 3D rotation axis and angle.
454 @rtype: numpy 3D rank-1 array, float
455 """
456
457
458 R = zeros((3, 3), float64)
459
460
461 euler_to_R_xyx(alpha, beta, gamma, R)
462
463
464 return R_to_axis_angle(R)
465
466
468 """Convert the xyz Euler angles to axis-angle notation.
469
470 This function first generates a rotation matrix via euler_*_to_R() and then uses R_to_axis_angle() to convert to the axis and angle notation.
471
472 @param alpha: The alpha Euler angle in rad.
473 @type alpha: float
474 @param beta: The beta Euler angle in rad.
475 @type beta: float
476 @param gamma: The gamma Euler angle in rad.
477 @type gamma: float
478 @return: The 3D rotation axis and angle.
479 @rtype: numpy 3D rank-1 array, float
480 """
481
482
483 R = zeros((3, 3), float64)
484
485
486 euler_to_R_xyz(alpha, beta, gamma, R)
487
488
489 return R_to_axis_angle(R)
490
491
493 """Convert the xzx Euler angles to axis-angle notation.
494
495 This function first generates a rotation matrix via euler_*_to_R() and then uses R_to_axis_angle() to convert to the axis and angle notation.
496
497 @param alpha: The alpha Euler angle in rad.
498 @type alpha: float
499 @param beta: The beta Euler angle in rad.
500 @type beta: float
501 @param gamma: The gamma Euler angle in rad.
502 @type gamma: float
503 @return: The 3D rotation axis and angle.
504 @rtype: numpy 3D rank-1 array, float
505 """
506
507
508 R = zeros((3, 3), float64)
509
510
511 euler_to_R_xzx(alpha, beta, gamma, R)
512
513
514 return R_to_axis_angle(R)
515
516
518 """Convert the xzy Euler angles to axis-angle notation.
519
520 This function first generates a rotation matrix via euler_*_to_R() and then uses R_to_axis_angle() to convert to the axis and angle notation.
521
522 @param alpha: The alpha Euler angle in rad.
523 @type alpha: float
524 @param beta: The beta Euler angle in rad.
525 @type beta: float
526 @param gamma: The gamma Euler angle in rad.
527 @type gamma: float
528 @return: The 3D rotation axis and angle.
529 @rtype: numpy 3D rank-1 array, float
530 """
531
532
533 R = zeros((3, 3), float64)
534
535
536 euler_to_R_xzy(alpha, beta, gamma, R)
537
538
539 return R_to_axis_angle(R)
540
541
543 """Convert the yxy Euler angles to axis-angle notation.
544
545 This function first generates a rotation matrix via euler_*_to_R() and then uses R_to_axis_angle() to convert to the axis and angle notation.
546
547 @param alpha: The alpha Euler angle in rad.
548 @type alpha: float
549 @param beta: The beta Euler angle in rad.
550 @type beta: float
551 @param gamma: The gamma Euler angle in rad.
552 @type gamma: float
553 @return: The 3D rotation axis and angle.
554 @rtype: numpy 3D rank-1 array, float
555 """
556
557
558 R = zeros((3, 3), float64)
559
560
561 euler_to_R_yxy(alpha, beta, gamma, R)
562
563
564 return R_to_axis_angle(R)
565
566
568 """Convert the yxz Euler angles to axis-angle notation.
569
570 This function first generates a rotation matrix via euler_*_to_R() and then uses R_to_axis_angle() to convert to the axis and angle notation.
571
572 @param alpha: The alpha Euler angle in rad.
573 @type alpha: float
574 @param beta: The beta Euler angle in rad.
575 @type beta: float
576 @param gamma: The gamma Euler angle in rad.
577 @type gamma: float
578 @return: The 3D rotation axis and angle.
579 @rtype: numpy 3D rank-1 array, float
580 """
581
582
583 R = zeros((3, 3), float64)
584
585
586 euler_to_R_yxz(alpha, beta, gamma, R)
587
588
589 return R_to_axis_angle(R)
590
591
593 """Convert the yzx Euler angles to axis-angle notation.
594
595 This function first generates a rotation matrix via euler_*_to_R() and then uses R_to_axis_angle() to convert to the axis and angle notation.
596
597 @param alpha: The alpha Euler angle in rad.
598 @type alpha: float
599 @param beta: The beta Euler angle in rad.
600 @type beta: float
601 @param gamma: The gamma Euler angle in rad.
602 @type gamma: float
603 @return: The 3D rotation axis and angle.
604 @rtype: numpy 3D rank-1 array, float
605 """
606
607
608 R = zeros((3, 3), float64)
609
610
611 euler_to_R_yzx(alpha, beta, gamma, R)
612
613
614 return R_to_axis_angle(R)
615
616
618 """Convert the yzy Euler angles to axis-angle notation.
619
620 This function first generates a rotation matrix via euler_*_to_R() and then uses R_to_axis_angle() to convert to the axis and angle notation.
621
622 @param alpha: The alpha Euler angle in rad.
623 @type alpha: float
624 @param beta: The beta Euler angle in rad.
625 @type beta: float
626 @param gamma: The gamma Euler angle in rad.
627 @type gamma: float
628 @return: The 3D rotation axis and angle.
629 @rtype: numpy 3D rank-1 array, float
630 """
631
632
633 R = zeros((3, 3), float64)
634
635
636 euler_to_R_yzy(alpha, beta, gamma, R)
637
638
639 return R_to_axis_angle(R)
640
641
643 """Convert the zxy Euler angles to axis-angle notation.
644
645 This function first generates a rotation matrix via euler_*_to_R() and then uses R_to_axis_angle() to convert to the axis and angle notation.
646
647 @param alpha: The alpha Euler angle in rad.
648 @type alpha: float
649 @param beta: The beta Euler angle in rad.
650 @type beta: float
651 @param gamma: The gamma Euler angle in rad.
652 @type gamma: float
653 @return: The 3D rotation axis and angle.
654 @rtype: numpy 3D rank-1 array, float
655 """
656
657
658 R = zeros((3, 3), float64)
659
660
661 euler_to_R_zxy(alpha, beta, gamma, R)
662
663
664 return R_to_axis_angle(R)
665
666
668 """Convert the zxz Euler angles to axis-angle notation.
669
670 This function first generates a rotation matrix via euler_*_to_R() and then uses R_to_axis_angle() to convert to the axis and angle notation.
671
672 @param alpha: The alpha Euler angle in rad.
673 @type alpha: float
674 @param beta: The beta Euler angle in rad.
675 @type beta: float
676 @param gamma: The gamma Euler angle in rad.
677 @type gamma: float
678 @return: The 3D rotation axis and angle.
679 @rtype: numpy 3D rank-1 array, float
680 """
681
682
683 R = zeros((3, 3), float64)
684
685
686 euler_to_R_zxz(alpha, beta, gamma, R)
687
688
689 return R_to_axis_angle(R)
690
691
693 """Convert the zyx Euler angles to axis-angle notation.
694
695 This function first generates a rotation matrix via euler_*_to_R() and then uses R_to_axis_angle() to convert to the axis and angle notation.
696
697 @param alpha: The alpha Euler angle in rad.
698 @type alpha: float
699 @param beta: The beta Euler angle in rad.
700 @type beta: float
701 @param gamma: The gamma Euler angle in rad.
702 @type gamma: float
703 @return: The 3D rotation axis and angle.
704 @rtype: numpy 3D rank-1 array, float
705 """
706
707
708 R = zeros((3, 3), float64)
709
710
711 euler_to_R_zyx(alpha, beta, gamma, R)
712
713
714 return R_to_axis_angle(R)
715
716
718 """Convert the zyz Euler angles to axis-angle notation.
719
720 This function first generates a rotation matrix via euler_*_to_R() and then uses R_to_axis_angle() to convert to the axis and angle notation.
721
722 @param alpha: The alpha Euler angle in rad.
723 @type alpha: float
724 @param beta: The beta Euler angle in rad.
725 @type beta: float
726 @param gamma: The gamma Euler angle in rad.
727 @type gamma: float
728 @return: The 3D rotation axis and angle.
729 @rtype: numpy 3D rank-1 array, float
730 """
731
732
733 R = zeros((3, 3), float64)
734
735
736 euler_to_R_zyz(alpha, beta, gamma, R)
737
738
739 return R_to_axis_angle(R)
740
741
743 """Generate the x-y-x Euler angle convention rotation matrix.
744
745 Rotation matrix
746 ===============
747
748 The rotation matrix is defined as the vector of unit vectors::
749
750 R = [mux, muy, muz].
751
752 According to wikipedia (http://en.wikipedia.org/wiki/Euler_angles#Table_of_matrices), the rotation matrix for the xyx convention is::
753
754 | cb sa*sb ca*sb |
755 R = | sb*sg ca*cg - sa*cb*sg -sa*cg - ca*cb*sg |,
756 | -sb*cg ca*sg + sa*cb*cg -sa*sg + ca*cb*cg |
757
758 where::
759
760 ca = cos(alpha),
761 sa = sin(alpha),
762 cb = cos(beta),
763 sb = sin(beta),
764 cg = cos(gamma),
765 sg = sin(gamma).
766
767
768 @param alpha: The alpha Euler angle in rad for the x-rotation.
769 @type alpha: float
770 @param beta: The beta Euler angle in rad for the y-rotation.
771 @type beta: float
772 @param gamma: The gamma Euler angle in rad for the second x-rotation.
773 @type gamma: float
774 @param R: The 3x3 rotation matrix to update.
775 @type R: 3x3 numpy array
776 """
777
778
779 sin_a = sin(alpha)
780 cos_a = cos(alpha)
781 sin_b = sin(beta)
782 cos_b = cos(beta)
783 sin_g = sin(gamma)
784 cos_g = cos(gamma)
785
786
787 R[0, 0] = cos_b
788 R[1, 0] = sin_b * sin_g
789 R[2, 0] = -sin_b * cos_g
790
791
792 R[0, 1] = sin_a * sin_b
793 R[1, 1] = cos_a * cos_g - sin_a * cos_b * sin_g
794 R[2, 1] = cos_a * sin_g + sin_a * cos_b * cos_g
795
796
797 R[0, 2] = cos_a * sin_b
798 R[1, 2] = -sin_a * cos_g - cos_a * cos_b * sin_g
799 R[2, 2] = -sin_a * sin_g + cos_a * cos_b * cos_g
800
801
803 """Generate the x-y-z Euler angle convention rotation matrix.
804
805 Rotation matrix
806 ===============
807
808 The rotation matrix is defined as the vector of unit vectors::
809
810 R = [mux, muy, muz].
811
812 According to wikipedia (http://en.wikipedia.org/wiki/Euler_angles#Table_of_matrices), the rotation matrix for the xyz convention is::
813
814 | cb*cg -ca*sg + sa*sb*cg sa*sg + ca*sb*cg |
815 R = | cb*sg ca*cg + sa*sb*sg -sa*cg + ca*sb*sg |,
816 | -sb sa*cb ca*cb |
817
818 where::
819
820 ca = cos(alpha),
821 sa = sin(alpha),
822 cb = cos(beta),
823 sb = sin(beta),
824 cg = cos(gamma),
825 sg = sin(gamma).
826
827
828 @param alpha: The alpha Euler angle in rad for the x-rotation.
829 @type alpha: float
830 @param beta: The beta Euler angle in rad for the y-rotation.
831 @type beta: float
832 @param gamma: The gamma Euler angle in rad for the z-rotation.
833 @type gamma: float
834 @param R: The 3x3 rotation matrix to update.
835 @type R: 3x3 numpy array
836 """
837
838
839 sin_a = sin(alpha)
840 cos_a = cos(alpha)
841 sin_b = sin(beta)
842 cos_b = cos(beta)
843 sin_g = sin(gamma)
844 cos_g = cos(gamma)
845
846
847 R[0, 0] = cos_b * cos_g
848 R[1, 0] = cos_b * sin_g
849 R[2, 0] = -sin_b
850
851
852 R[0, 1] = -cos_a * sin_g + sin_a * sin_b * cos_g
853 R[1, 1] = cos_a * cos_g + sin_a * sin_b * sin_g
854 R[2, 1] = sin_a * cos_b
855
856
857 R[0, 2] = sin_a * sin_g + cos_a * sin_b * cos_g
858 R[1, 2] = -sin_a * cos_g + cos_a * sin_b * sin_g
859 R[2, 2] = cos_a * cos_b
860
861
863 """Generate the x-z-x Euler angle convention rotation matrix.
864
865 Rotation matrix
866 ===============
867
868 The rotation matrix is defined as the vector of unit vectors::
869
870 R = [mux, muy, muz].
871
872 According to wikipedia (http://en.wikipedia.org/wiki/Euler_angles#Table_of_matrices), the rotation matrix for the xzx convention is::
873
874 | cb -ca*sb sa*sb |
875 R = | sb*cg -sa*sg + ca*cb*cg -ca*sg - sa*cb*cg |,
876 | sb*sg sa*cg + ca*cb*sg ca*cg - sa*cb*sg |
877
878 where::
879
880 ca = cos(alpha),
881 sa = sin(alpha),
882 cb = cos(beta),
883 sb = sin(beta),
884 cg = cos(gamma),
885 sg = sin(gamma).
886
887
888 @param alpha: The alpha Euler angle in rad for the x-rotation.
889 @type alpha: float
890 @param beta: The beta Euler angle in rad for the z-rotation.
891 @type beta: float
892 @param gamma: The gamma Euler angle in rad for the second x-rotation.
893 @type gamma: float
894 @param R: The 3x3 rotation matrix to update.
895 @type R: 3x3 numpy array
896 """
897
898
899 sin_a = sin(alpha)
900 cos_a = cos(alpha)
901 sin_b = sin(beta)
902 cos_b = cos(beta)
903 sin_g = sin(gamma)
904 cos_g = cos(gamma)
905
906
907 R[0, 0] = cos_b
908 R[1, 0] = sin_b * cos_g
909 R[2, 0] = sin_b * sin_g
910
911
912 R[0, 1] = -cos_a * sin_b
913 R[1, 1] = -sin_a * sin_g + cos_a * cos_b * cos_g
914 R[2, 1] = sin_a * cos_g + cos_a * cos_b * sin_g
915
916
917 R[0, 2] = sin_a * sin_b
918 R[1, 2] = -cos_a * sin_g - sin_a * cos_b * cos_g
919 R[2, 2] = cos_a * cos_g - sin_a * cos_b * sin_g
920
921
923 """Generate the x-z-y Euler angle convention rotation matrix.
924
925 Rotation matrix
926 ===============
927
928 The rotation matrix is defined as the vector of unit vectors::
929
930 R = [mux, muy, muz].
931
932 According to wikipedia (http://en.wikipedia.org/wiki/Euler_angles#Table_of_matrices), the rotation matrix for the xzy convention is::
933
934 | cb*cg sa*sg - ca*sb*cg ca*sg + sa*sb*cg |
935 R = | sb ca*cb -sa*cb |,
936 | -cb*sg sa*cg + ca*sb*sg ca*cg - sa*sb*sg |
937
938 where::
939
940 ca = cos(alpha),
941 sa = sin(alpha),
942 cb = cos(beta),
943 sb = sin(beta),
944 cg = cos(gamma),
945 sg = sin(gamma).
946
947
948 @param alpha: The alpha Euler angle in rad for the x-rotation.
949 @type alpha: float
950 @param beta: The beta Euler angle in rad for the z-rotation.
951 @type beta: float
952 @param gamma: The gamma Euler angle in rad for the y-rotation.
953 @type gamma: float
954 @param R: The 3x3 rotation matrix to update.
955 @type R: 3x3 numpy array
956 """
957
958
959 sin_a = sin(alpha)
960 cos_a = cos(alpha)
961 sin_b = sin(beta)
962 cos_b = cos(beta)
963 sin_g = sin(gamma)
964 cos_g = cos(gamma)
965
966
967 R[0, 0] = cos_b * cos_g
968 R[1, 0] = sin_b
969 R[2, 0] = -cos_b * sin_g
970
971
972 R[0, 1] = sin_a * sin_g - cos_a * sin_b * cos_g
973 R[1, 1] = cos_a * cos_b
974 R[2, 1] = sin_a * cos_g + cos_a * sin_b * sin_g
975
976
977 R[0, 2] = cos_a * sin_g + sin_a * sin_b * cos_g
978 R[1, 2] = -sin_a * cos_b
979 R[2, 2] = cos_a * cos_g - sin_a * sin_b * sin_g
980
981
983 """Generate the y-x-y Euler angle convention rotation matrix.
984
985 Rotation matrix
986 ===============
987
988 The rotation matrix is defined as the vector of unit vectors::
989
990 R = [mux, muy, muz].
991
992 According to wikipedia (http://en.wikipedia.org/wiki/Euler_angles#Table_of_matrices), the rotation matrix for the yxy convention is::
993
994 | ca*cg - sa*cb*sg sb*sg sa*cg + ca*cb*sg |
995 R = | sa*sb cb -ca*sb |,
996 | -ca*sg - sa*cb*cg sb*cg -sa*sg + ca*cb*cg |
997
998 where::
999
1000 ca = cos(alpha),
1001 sa = sin(alpha),
1002 cb = cos(beta),
1003 sb = sin(beta),
1004 cg = cos(gamma),
1005 sg = sin(gamma).
1006
1007
1008 @param alpha: The alpha Euler angle in rad for the y-rotation.
1009 @type alpha: float
1010 @param beta: The beta Euler angle in rad for the x-rotation.
1011 @type beta: float
1012 @param gamma: The gamma Euler angle in rad for the second y-rotation.
1013 @type gamma: float
1014 @param R: The 3x3 rotation matrix to update.
1015 @type R: 3x3 numpy array
1016 """
1017
1018
1019 sin_a = sin(alpha)
1020 cos_a = cos(alpha)
1021 sin_b = sin(beta)
1022 cos_b = cos(beta)
1023 sin_g = sin(gamma)
1024 cos_g = cos(gamma)
1025
1026
1027 R[0, 0] = cos_a * cos_g - sin_a * cos_b * sin_g
1028 R[1, 0] = sin_a * sin_b
1029 R[2, 0] = -cos_a * sin_g - sin_a * cos_b * cos_g
1030
1031
1032 R[0, 1] = sin_b * sin_g
1033 R[1, 1] = cos_b
1034 R[2, 1] = sin_b * cos_g
1035
1036
1037 R[0, 2] = sin_a * cos_g + cos_a * cos_b * sin_g
1038 R[1, 2] = -cos_a * sin_b
1039 R[2, 2] = -sin_a * sin_g + cos_a * cos_b * cos_g
1040
1041
1043 """Generate the y-x-z Euler angle convention rotation matrix.
1044
1045 Rotation matrix
1046 ===============
1047
1048 The rotation matrix is defined as the vector of unit vectors::
1049
1050 R = [mux, muy, muz].
1051
1052 According to wikipedia (http://en.wikipedia.org/wiki/Euler_angles#Table_of_matrices), the rotation matrix for the yxz convention is::
1053
1054 | ca*cg - sa*sb*sg -cb*sg sa*cg + ca*sb*sg |
1055 R = | ca*sg + sa*sb*cg cb*cg sa*sg - ca*sb*cg |,
1056 | -sa*cb sb ca*cb |
1057
1058 where::
1059
1060 ca = cos(alpha),
1061 sa = sin(alpha),
1062 cb = cos(beta),
1063 sb = sin(beta),
1064 cg = cos(gamma),
1065 sg = sin(gamma).
1066
1067
1068 @param alpha: The alpha Euler angle in rad for the y-rotation.
1069 @type alpha: float
1070 @param beta: The beta Euler angle in rad for the x-rotation.
1071 @type beta: float
1072 @param gamma: The gamma Euler angle in rad for the z-rotation.
1073 @type gamma: float
1074 @param R: The 3x3 rotation matrix to update.
1075 @type R: 3x3 numpy array
1076 """
1077
1078
1079 sin_a = sin(alpha)
1080 cos_a = cos(alpha)
1081 sin_b = sin(beta)
1082 cos_b = cos(beta)
1083 sin_g = sin(gamma)
1084 cos_g = cos(gamma)
1085
1086
1087 R[0, 0] = cos_a * cos_g - sin_a * sin_b * sin_g
1088 R[1, 0] = cos_a * sin_g + sin_a * sin_b * cos_g
1089 R[2, 0] = -sin_a * cos_b
1090
1091
1092 R[0, 1] = -cos_b * sin_g
1093 R[1, 1] = cos_b * cos_g
1094 R[2, 1] = sin_b
1095
1096
1097 R[0, 2] = sin_a * cos_g + cos_a * sin_b * sin_g
1098 R[1, 2] = sin_a * sin_g - cos_a * sin_b * cos_g
1099 R[2, 2] = cos_a * cos_b
1100
1101
1103 """Generate the y-z-x Euler angle convention rotation matrix.
1104
1105 Rotation matrix
1106 ===============
1107
1108 The rotation matrix is defined as the vector of unit vectors::
1109
1110 R = [mux, muy, muz].
1111
1112 According to wikipedia (http://en.wikipedia.org/wiki/Euler_angles#Table_of_matrices), the rotation matrix for the yzx convention is::
1113
1114 | ca*cb -sb sa*cb |
1115 R = | sa*sg + ca*sb*cg cb*cg -ca*sg + sa*sb*cg |,
1116 | -sa*cg + ca*sb*sg cb*sg ca*cg + sa*sb*sg |
1117
1118 where::
1119
1120 ca = cos(alpha),
1121 sa = sin(alpha),
1122 cb = cos(beta),
1123 sb = sin(beta),
1124 cg = cos(gamma),
1125 sg = sin(gamma).
1126
1127
1128 @param alpha: The alpha Euler angle in rad for the y-rotation.
1129 @type alpha: float
1130 @param beta: The beta Euler angle in rad for the z-rotation.
1131 @type beta: float
1132 @param gamma: The gamma Euler angle in rad for the x-rotation.
1133 @type gamma: float
1134 @param R: The 3x3 rotation matrix to update.
1135 @type R: 3x3 numpy array
1136 """
1137
1138
1139 sin_a = sin(alpha)
1140 cos_a = cos(alpha)
1141 sin_b = sin(beta)
1142 cos_b = cos(beta)
1143 sin_g = sin(gamma)
1144 cos_g = cos(gamma)
1145
1146
1147 R[0, 0] = cos_a * cos_b
1148 R[1, 0] = sin_a * sin_g + cos_a * sin_b * cos_g
1149 R[2, 0] = -sin_a * cos_g + cos_a * sin_b * sin_g
1150
1151
1152 R[0, 1] = -sin_b
1153 R[1, 1] = cos_b * cos_g
1154 R[2, 1] = cos_b * sin_g
1155
1156
1157 R[0, 2] = sin_a * cos_b
1158 R[1, 2] = -cos_a * sin_g + sin_a * sin_b * cos_g
1159 R[2, 2] = cos_a * cos_g + sin_a * sin_b * sin_g
1160
1161
1163 """Generate the y-z-y Euler angle convention rotation matrix.
1164
1165 Rotation matrix
1166 ===============
1167
1168 The rotation matrix is defined as the vector of unit vectors::
1169
1170 R = [mux, muy, muz].
1171
1172 According to wikipedia (http://en.wikipedia.org/wiki/Euler_angles#Table_of_matrices), the rotation matrix for the yzy convention is::
1173
1174 | -sa*sg + ca*cb*cg -sb*cg ca*sg + sa*cb*cg |
1175 R = | ca*sb cb sa*sb |,
1176 | -sa*cg - ca*cb*sg sb*sg ca*cg - sa*cb*sg |
1177
1178 where::
1179
1180 ca = cos(alpha),
1181 sa = sin(alpha),
1182 cb = cos(beta),
1183 sb = sin(beta),
1184 cg = cos(gamma),
1185 sg = sin(gamma).
1186
1187
1188 @param alpha: The alpha Euler angle in rad for the y-rotation.
1189 @type alpha: float
1190 @param beta: The beta Euler angle in rad for the z-rotation.
1191 @type beta: float
1192 @param gamma: The gamma Euler angle in rad for the second y-rotation.
1193 @type gamma: float
1194 @param R: The 3x3 rotation matrix to update.
1195 @type R: 3x3 numpy array
1196 """
1197
1198
1199 sin_a = sin(alpha)
1200 cos_a = cos(alpha)
1201 sin_b = sin(beta)
1202 cos_b = cos(beta)
1203 sin_g = sin(gamma)
1204 cos_g = cos(gamma)
1205
1206
1207 R[0, 0] = -sin_a * sin_g + cos_a * cos_b * cos_g
1208 R[1, 0] = cos_a * sin_b
1209 R[2, 0] = -sin_a * cos_g - cos_a * cos_b * sin_g
1210
1211
1212 R[0, 1] = -sin_b * cos_g
1213 R[1, 1] = cos_b
1214 R[2, 1] = sin_b * sin_g
1215
1216
1217 R[0, 2] = cos_a * sin_g + sin_a * cos_b * cos_g
1218 R[1, 2] = sin_a * sin_b
1219 R[2, 2] = cos_a * cos_g - sin_a * cos_b * sin_g
1220
1221
1223 """Generate the z-x-y Euler angle convention rotation matrix.
1224
1225 Rotation matrix
1226 ===============
1227
1228 The rotation matrix is defined as the vector of unit vectors::
1229
1230 R = [mux, muy, muz].
1231
1232 According to wikipedia (http://en.wikipedia.org/wiki/Euler_angles#Table_of_matrices), the rotation matrix for the zxy convention is::
1233
1234 | ca*cg + sa*sb*sg -sa*cg + ca*sb*sg cb*sg |
1235 R = | sa*cb ca*cb -sb |,
1236 | -ca*sg + sa*sb*cg sa*sg + ca*sb*cg cb*cg |
1237
1238 where::
1239
1240 ca = cos(alpha),
1241 sa = sin(alpha),
1242 cb = cos(beta),
1243 sb = sin(beta),
1244 cg = cos(gamma),
1245 sg = sin(gamma).
1246
1247
1248 @param alpha: The alpha Euler angle in rad for the z-rotation.
1249 @type alpha: float
1250 @param beta: The beta Euler angle in rad for the x-rotation.
1251 @type beta: float
1252 @param gamma: The gamma Euler angle in rad for the y-rotation.
1253 @type gamma: float
1254 @param R: The 3x3 rotation matrix to update.
1255 @type R: 3x3 numpy array
1256 """
1257
1258
1259 sin_a = sin(alpha)
1260 cos_a = cos(alpha)
1261 sin_b = sin(beta)
1262 cos_b = cos(beta)
1263 sin_g = sin(gamma)
1264 cos_g = cos(gamma)
1265
1266
1267 R[0, 0] = cos_a * cos_g + sin_a * sin_b * sin_g
1268 R[1, 0] = sin_a * cos_b
1269 R[2, 0] = -cos_a * sin_g + sin_a * sin_b * cos_g
1270
1271
1272 R[0, 1] = -sin_a * cos_g + cos_a * sin_b * sin_g
1273 R[1, 1] = cos_a * cos_b
1274 R[2, 1] = sin_a * sin_g + cos_a * sin_b * cos_g
1275
1276
1277 R[0, 2] = cos_b * sin_g
1278 R[1, 2] = -sin_b
1279 R[2, 2] = cos_b * cos_g
1280
1281
1283 """Generate the z-x-z Euler angle convention rotation matrix.
1284
1285 Rotation matrix
1286 ===============
1287
1288 The rotation matrix is defined as the vector of unit vectors::
1289
1290 R = [mux, muy, muz].
1291
1292 According to wikipedia (http://en.wikipedia.org/wiki/Euler_angles#Table_of_matrices), the rotation matrix for the zxz convention is::
1293
1294 | ca*cg - sa*cb*sg -sa*cg - ca*cb*sg sb*sg |
1295 R = | ca*sg + sa*cb*cg -sa*sg + ca*cb*cg -sb*cg |,
1296 | sa*sb ca*sb cb |
1297
1298 where::
1299
1300 ca = cos(alpha),
1301 sa = sin(alpha),
1302 cb = cos(beta),
1303 sb = sin(beta),
1304 cg = cos(gamma),
1305 sg = sin(gamma).
1306
1307
1308 @param alpha: The alpha Euler angle in rad for the z-rotation.
1309 @type alpha: float
1310 @param beta: The beta Euler angle in rad for the y-rotation.
1311 @type beta: float
1312 @param gamma: The gamma Euler angle in rad for the second z-rotation.
1313 @type gamma: float
1314 @param R: The 3x3 rotation matrix to update.
1315 @type R: 3x3 numpy array
1316 """
1317
1318
1319 sin_a = sin(alpha)
1320 cos_a = cos(alpha)
1321 sin_b = sin(beta)
1322 cos_b = cos(beta)
1323 sin_g = sin(gamma)
1324 cos_g = cos(gamma)
1325
1326
1327 R[0, 0] = cos_a * cos_g - sin_a * cos_b * sin_g
1328 R[1, 0] = cos_a * sin_g + sin_a * cos_b * cos_g
1329 R[2, 0] = sin_a * sin_b
1330
1331
1332 R[0, 1] = -sin_a * cos_g - cos_a * cos_b * sin_g
1333 R[1, 1] = -sin_a * sin_g + cos_a * cos_b * cos_g
1334 R[2, 1] = cos_a * sin_b
1335
1336
1337 R[0, 2] = sin_b * sin_g
1338 R[1, 2] = -sin_b * cos_g
1339 R[2, 2] = cos_b
1340
1341
1343 """Generate the z-y-x Euler angle convention rotation matrix.
1344
1345 Rotation matrix
1346 ===============
1347
1348 The rotation matrix is defined as the vector of unit vectors::
1349
1350 R = [mux, muy, muz].
1351
1352 According to wikipedia (http://en.wikipedia.org/wiki/Euler_angles#Table_of_matrices), the rotation matrix for the zyx convention is::
1353
1354 | ca*cb -sa*cb sb |
1355 R = | sa*cg + ca*sb*sg ca*cg - sa*sb*sg -cb*sg |,
1356 | sa*sg - ca*sb*cg ca*sg + sa*sb*cg cb*cg |
1357
1358 where::
1359
1360 ca = cos(alpha),
1361 sa = sin(alpha),
1362 cb = cos(beta),
1363 sb = sin(beta),
1364 cg = cos(gamma),
1365 sg = sin(gamma).
1366
1367
1368 @param alpha: The alpha Euler angle in rad for the z-rotation.
1369 @type alpha: float
1370 @param beta: The beta Euler angle in rad for the y-rotation.
1371 @type beta: float
1372 @param gamma: The gamma Euler angle in rad for the x-rotation.
1373 @type gamma: float
1374 @param R: The 3x3 rotation matrix to update.
1375 @type R: 3x3 numpy array
1376 """
1377
1378
1379 sin_a = sin(alpha)
1380 cos_a = cos(alpha)
1381 sin_b = sin(beta)
1382 cos_b = cos(beta)
1383 sin_g = sin(gamma)
1384 cos_g = cos(gamma)
1385
1386
1387 R[0, 0] = cos_a * cos_b
1388 R[1, 0] = sin_a * cos_g + cos_a * sin_b * sin_g
1389 R[2, 0] = sin_a * sin_g - cos_a * sin_b * cos_g
1390
1391
1392 R[0, 1] = -sin_a * cos_b
1393 R[1, 1] = cos_a * cos_g - sin_a * sin_b * sin_g
1394 R[2, 1] = cos_a * sin_g + sin_a * sin_b * cos_g
1395
1396
1397 R[0, 2] = sin_b
1398 R[1, 2] = -cos_b * sin_g
1399 R[2, 2] = cos_b * cos_g
1400
1401
1403 """Generate the z-y-z Euler angle convention rotation matrix.
1404
1405 Rotation matrix
1406 ===============
1407
1408 The rotation matrix is defined as the vector of unit vectors::
1409
1410 R = [mux, muy, muz].
1411
1412 According to wikipedia (http://en.wikipedia.org/wiki/Euler_angles#Table_of_matrices), the rotation matrix for the zyz convention is::
1413
1414 | -sa*sg + ca*cb*cg -ca*sg - sa*cb*cg sb*cg |
1415 R = | sa*cg + ca*cb*sg ca*cg - sa*cb*sg sb*sg |,
1416 | -ca*sb sa*sb cb |
1417
1418 where::
1419
1420 ca = cos(alpha),
1421 sa = sin(alpha),
1422 cb = cos(beta),
1423 sb = sin(beta),
1424 cg = cos(gamma),
1425 sg = sin(gamma).
1426
1427
1428 @param alpha: The alpha Euler angle in rad for the z-rotation.
1429 @type alpha: float
1430 @param beta: The beta Euler angle in rad for the y-rotation.
1431 @type beta: float
1432 @param gamma: The gamma Euler angle in rad for the second z-rotation.
1433 @type gamma: float
1434 @param R: The 3x3 rotation matrix to update.
1435 @type R: 3x3 numpy array
1436 """
1437
1438
1439 sin_a = sin(alpha)
1440 cos_a = cos(alpha)
1441 sin_b = sin(beta)
1442 cos_b = cos(beta)
1443 sin_g = sin(gamma)
1444 cos_g = cos(gamma)
1445
1446
1447 R[0, 0] = -sin_a * sin_g + cos_a * cos_b * cos_g
1448 R[1, 0] = sin_a * cos_g + cos_a * cos_b * sin_g
1449 R[2, 0] = -cos_a * sin_b
1450
1451
1452 R[0, 1] = -cos_a * sin_g - sin_a * cos_b * cos_g
1453 R[1, 1] = cos_a * cos_g - sin_a * cos_b * sin_g
1454 R[2, 1] = sin_a * sin_b
1455
1456
1457 R[0, 2] = sin_b * cos_g
1458 R[1, 2] = sin_b * sin_g
1459 R[2, 2] = cos_b
1460
1461
1463 """Calculate the parameteric indices i, j, k, and h.
1464
1465 This is one of the algorithms of Ken Shoemake in "Euler Angle Conversion. Graphics Gems IV. Paul Heckbert (ed.). Academic Press, 1994, ISBN: 0123361567. pp. 222-229." (http://www.graphicsgems.org/).
1466
1467 The indices (i, j, k) are a permutation of (x, y, z), and the index h corresponds to the row containing the Givens argument a.
1468
1469
1470 @param i: The index i.
1471 @type i: int
1472 @param neg: Zero if (i, j, k) is an even permutation of (x, y, z) or one if odd.
1473 @type neg: int
1474 @param alt: Zero if the first and last system axes are the same, or one if they are different.
1475 @type alt: int
1476 @return: The values of j, k, and h.
1477 @rtype: tuple of int
1478 """
1479
1480
1481 j = EULER_NEXT[i + neg]
1482 k = EULER_NEXT[i+1 - neg]
1483
1484
1485 if alt:
1486 h = k
1487 else:
1488 h = i
1489
1490
1491 return j, k, h
1492
1493
1495 """Generate a random rotation matrix of fixed angle via the axis-angle notation.
1496
1497 Uniform point sampling on a unit sphere is used to generate a random axis orientation. This,
1498 together with the fixed rotation angle, is used to generate the random rotation matrix.
1499
1500 @param R: A 3D matrix to convert to the rotation matrix.
1501 @type R: numpy 3D, rank-2 array
1502 @keyword angle: The fixed rotation angle.
1503 @type angle: float
1504 """
1505
1506
1507 rot_axis = zeros(3, float64)
1508 random_rot_axis(rot_axis)
1509
1510
1511 axis_angle_to_R(rot_axis, angle, R)
1512
1513
1515 """Generate a random rotation matrix using 4D hypersphere point picking.
1516
1517 A quaternion is generated by creating a 4D vector with each value randomly selected from a
1518 Gaussian distribution, and then normalising.
1519
1520 @param R: A 3D matrix to convert to the rotation matrix.
1521 @type R: numpy 3D, rank-2 array
1522 """
1523
1524
1525 quat = array([gauss(0, 1), gauss(0, 1), gauss(0, 1), gauss(0, 1)], float64)
1526 quat = quat / norm(quat)
1527
1528
1529 quaternion_to_R(quat, R)
1530
1531
1533 """Convert the rotation matrix into the axis-angle notation.
1534
1535 Conversion equations
1536 ====================
1537
1538 From Wikipedia (http://en.wikipedia.org/wiki/Rotation_matrix), the conversion is given by::
1539
1540 x = Qzy-Qyz
1541 y = Qxz-Qzx
1542 z = Qyx-Qxy
1543 r = hypot(x,hypot(y,z))
1544 t = Qxx+Qyy+Qzz
1545 theta = atan2(r,t-1)
1546
1547 @param R: The 3x3 rotation matrix to update.
1548 @type R: 3x3 numpy array
1549 @return: The 3D rotation axis and angle.
1550 @rtype: numpy 3D rank-1 array, float
1551 """
1552
1553
1554 axis = zeros(3, float64)
1555 axis[0] = R[2, 1] - R[1, 2]
1556 axis[1] = R[0, 2] - R[2, 0]
1557 axis[2] = R[1, 0] - R[0, 1]
1558
1559
1560 r = hypot(axis[0], hypot(axis[1], axis[2]))
1561 t = R[0, 0] + R[1, 1] + R[2, 2]
1562 theta = atan2(r, t-1)
1563
1564
1565 if r != 0.0:
1566 axis = axis / r
1567
1568
1569 return axis, theta
1570
1571
1572 -def R_to_euler(R, notation, axes_rot='static', second_sol=False):
1573 """Convert the rotation matrix to the given Euler angles.
1574
1575 This uses the algorithms of Ken Shoemake in "Euler Angle Conversion. Graphics Gems IV. Paul Heckbert (ed.). Academic Press, 1994, ISBN: 0123361567. pp. 222-229." (http://www.graphicsgems.org/).
1576
1577
1578 The Euler angle notation can be one of:
1579 - xyx
1580 - xyz
1581 - xzx
1582 - xzy
1583 - yxy
1584 - yxz
1585 - yzx
1586 - yzy
1587 - zxy
1588 - zxz
1589 - zyx
1590 - zyz
1591
1592
1593 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1594 @type R: 3D, rank-2 numpy array
1595 @param notation: The Euler angle notation to use.
1596 @type notation: str
1597 @keyword axes_rot: The axes rotation - either 'static', the static axes or 'rotating', the rotating axes.
1598 @type axes_rot: str
1599 @keyword second_sol: Return the second solution instead (currently unused).
1600 @type second_sol: bool
1601 @return: The alpha, beta, and gamma Euler angles in the given convention.
1602 @rtype: tuple of float
1603 """
1604
1605
1606 R = deepcopy(R)
1607
1608
1609 i, neg, alt = EULER_TRANS_TABLE[notation]
1610
1611
1612 rev = 0
1613 if axes_rot != 'static':
1614 rev = 1
1615
1616
1617 j, k, h = matrix_indices(i, neg, alt)
1618
1619
1620 if alt:
1621
1622 sin_beta = sqrt(R[i, j]**2 + R[i, k]**2)
1623
1624
1625 if sin_beta > EULER_EPSILON:
1626 alpha = atan2( R[i, j], R[i, k])
1627 beta = atan2( sin_beta, R[i, i])
1628 gamma = atan2( R[j, i], -R[k, i])
1629
1630
1631 else:
1632 alpha = atan2(-R[j, k], R[j, j])
1633 beta = atan2( sin_beta, R[i, i])
1634 gamma = 0.0
1635
1636
1637 else:
1638
1639 cos_beta = sqrt(R[i, i]**2 + R[j, i]**2)
1640
1641
1642 if cos_beta > EULER_EPSILON:
1643 alpha = atan2( R[k, j], R[k, k])
1644 beta = atan2(-R[k, i], cos_beta)
1645 gamma = atan2( R[j, i], R[i, i])
1646
1647
1648 else:
1649 alpha = atan2(-R[j, k], R[j, j])
1650 beta = atan2(-R[k, i], cos_beta)
1651 gamma = 0.0
1652
1653
1654 if neg:
1655 alpha, beta, gamma = -alpha, -beta, -gamma
1656 if rev:
1657 alpha_old = alpha
1658 alpha = gamma
1659 gamma = alpha_old
1660
1661
1662 if alt and -pi < beta < 0.0:
1663 alpha = alpha + pi
1664 beta = -beta
1665 gamma = gamma + pi
1666
1667 alpha = generic_fns.angles.wrap_angles(alpha, 0.0, 2.0*pi)
1668 beta = generic_fns.angles.wrap_angles(beta, 0.0, 2.0*pi)
1669 gamma = generic_fns.angles.wrap_angles(gamma, 0.0, 2.0*pi)
1670
1671
1672 return alpha, beta, gamma
1673
1674
1676 """Convert the rotation matrix to the xyx Euler angles.
1677
1678 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1679 @type R: 3D, rank-2 numpy array
1680 @return: The alpha, beta, and gamma Euler angles in the xyx convention.
1681 @rtype: tuple of float
1682 """
1683
1684
1685 return R_to_euler(R, 'xyx')
1686
1687
1689 """Convert the rotation matrix to the xyz Euler angles.
1690
1691 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1692 @type R: 3D, rank-2 numpy array
1693 @return: The alpha, beta, and gamma Euler angles in the xyz convention.
1694 @rtype: tuple of float
1695 """
1696
1697
1698 return R_to_euler(R, 'xyz')
1699
1700
1702 """Convert the rotation matrix to the xzx Euler angles.
1703
1704 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1705 @type R: 3D, rank-2 numpy array
1706 @return: The alpha, beta, and gamma Euler angles in the xzx convention.
1707 @rtype: tuple of float
1708 """
1709
1710
1711 return R_to_euler(R, 'xzx')
1712
1713
1715 """Convert the rotation matrix to the xzy Euler angles.
1716
1717 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1718 @type R: 3D, rank-2 numpy array
1719 @return: The alpha, beta, and gamma Euler angles in the xzy convention.
1720 @rtype: tuple of float
1721 """
1722
1723
1724 return R_to_euler(R, 'xzy')
1725
1726
1728 """Convert the rotation matrix to the yxy Euler angles.
1729
1730 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1731 @type R: 3D, rank-2 numpy array
1732 @return: The alpha, beta, and gamma Euler angles in the yxy convention.
1733 @rtype: tuple of float
1734 """
1735
1736
1737 return R_to_euler(R, 'yxy')
1738
1739
1741 """Convert the rotation matrix to the yxz Euler angles.
1742
1743 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1744 @type R: 3D, rank-2 numpy array
1745 @return: The alpha, beta, and gamma Euler angles in the yxz convention.
1746 @rtype: tuple of float
1747 """
1748
1749
1750 return R_to_euler(R, 'yxz')
1751
1752
1754 """Convert the rotation matrix to the yzx Euler angles.
1755
1756 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1757 @type R: 3D, rank-2 numpy array
1758 @return: The alpha, beta, and gamma Euler angles in the yzx convention.
1759 @rtype: tuple of float
1760 """
1761
1762
1763 return R_to_euler(R, 'yzx')
1764
1765
1767 """Convert the rotation matrix to the yzy Euler angles.
1768
1769 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1770 @type R: 3D, rank-2 numpy array
1771 @return: The alpha, beta, and gamma Euler angles in the yzy convention.
1772 @rtype: tuple of float
1773 """
1774
1775
1776 return R_to_euler(R, 'yzy')
1777
1778
1780 """Convert the rotation matrix to the zxy Euler angles.
1781
1782 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1783 @type R: 3D, rank-2 numpy array
1784 @return: The alpha, beta, and gamma Euler angles in the zxy convention.
1785 @rtype: tuple of float
1786 """
1787
1788
1789 return R_to_euler(R, 'zxy')
1790
1791
1793 """Convert the rotation matrix to the zxz Euler angles.
1794
1795 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1796 @type R: 3D, rank-2 numpy array
1797 @return: The alpha, beta, and gamma Euler angles in the zxz convention.
1798 @rtype: tuple of float
1799 """
1800
1801
1802 return R_to_euler(R, 'zxz')
1803
1804
1806 """Convert the rotation matrix to the zyx Euler angles.
1807
1808 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1809 @type R: 3D, rank-2 numpy array
1810 @return: The alpha, beta, and gamma Euler angles in the zyx convention.
1811 @rtype: tuple of float
1812 """
1813
1814
1815 return R_to_euler(R, 'zyx')
1816
1817
1819 """Convert the rotation matrix to the zyz Euler angles.
1820
1821 @param R: The 3x3 rotation matrix to extract the Euler angles from.
1822 @type R: 3D, rank-2 numpy array
1823 @return: The alpha, beta, and gamma Euler angles in the zyz convention.
1824 @rtype: tuple of float
1825 """
1826
1827
1828 return R_to_euler(R, 'zyz')
1829
1830
1832 """Convert the rotation matrix to the tilt and torsion rotation angles.
1833
1834 This notation is taken from "Bonev, I. A. and Gosselin, C. M. (2006) Analytical determination of the workspace of symmetrical spherical parallel mechanisms. IEEE Transactions on Robotics, 22(5), 1011-1017".
1835
1836
1837 @param R: The 3x3 rotation matrix to extract the tilt and torsion angles from.
1838 @type R: 3D, rank-2 numpy array
1839 @return: The phi, theta, and sigma tilt and torsion angles.
1840 @rtype: tuple of float
1841 """
1842
1843
1844 alpha, beta, gamma = R_to_euler(R, 'zyz')
1845
1846
1847 phi = gamma
1848 theta = beta
1849 sigma = alpha + gamma
1850
1851
1852 return phi, theta, sigma
1853
1854
1856 """Convert a rotation matrix into quaternion form.
1857
1858 This is from Wikipedia (http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion), where::
1859
1860 w = 0.5*sqrt(1+Qxx+Qyy+Qzz),
1861 x = copysign(0.5*sqrt(1+Qxx-Qyy-Qzz),Qzy-Qyz),
1862 y = copysign(0.5*sqrt(1-Qxx+Qyy-Qzz),Qxz-Qzx),
1863 z = copysign(0.5*sqrt(1-Qxx-Qyy+Qzz),Qyx-Qxy),
1864
1865 where the quaternion is defined as q = (w, x, y, z), and the copysign function is x with the
1866 sign of y::
1867
1868 copysign(x, y) = abs(x) / abs(y) * y
1869
1870
1871 @param R: The 3D rotation matrix.
1872 @type R: numpy 3D, rank-2 array
1873 @param quat: The quaternion.
1874 @type quat: numpy 4D, rank-1 array
1875 """
1876
1877
1878 quat[0] = 0.5 * sqrt(1.0 + R[0, 0] + R[1, 1] + R[2, 2])
1879 quat[1] = R[2, 1] - R[1, 2]
1880 if quat[1]:
1881 quat[1] = copysign(0.5*sqrt(1 + R[0, 0] - R[1, 1] - R[2, 2]), quat[1])
1882 quat[2] = R[0, 2] - R[2, 0]
1883 if quat[2]:
1884 quat[2] = copysign(0.5*sqrt(1 - R[0, 0] + R[1, 1] - R[2, 2]), quat[2])
1885 quat[3] = R[1, 0] - R[0, 1]
1886 if quat[3]:
1887 quat[3] = copysign(0.5*sqrt(1 - R[0, 0] - R[1, 1] + R[2, 2]), quat[3])
1888
1889
1891 """Generate a random rotation axis.
1892
1893 Uniform point sampling on a unit sphere is used to generate a random axis orientation.
1894
1895 @param axis: The 3D rotation axis.
1896 @type axis: numpy array, len 3
1897 """
1898
1899
1900 u = uniform(0, 1)
1901 theta = 2*pi*u
1902
1903
1904 v = uniform(0, 1)
1905 phi = acos(2.0*v - 1)
1906
1907
1908 axis[0] = cos(theta) * sin(phi)
1909 axis[1] = sin(theta) * sin(phi)
1910 axis[2] = cos(phi)
1911
1912
1914 """Convert the given forward rotation Euler angles into the equivalent reverse rotation Euler angles.
1915
1916 This if for the xyx notation.
1917
1918
1919 @param alpha: The alpha Euler angle in rad.
1920 @type alpha: float
1921 @param beta: The beta Euler angle in rad.
1922 @type beta: float
1923 @param gamma: The gamma Euler angle in rad.
1924 @type gamma: float
1925 @return: The alpha, beta, and gamma Euler angles for the reverse rotation.
1926 @rtype: tuple of float
1927 """
1928
1929
1930 R = zeros((3, 3), float64)
1931
1932
1933 euler_xyx_to_R(alpha, beta, gamma, R)
1934
1935
1936 R = transpose(R)
1937
1938
1939 return R_to_euler_xyx(R)
1940
1941
1943 """Convert the given forward rotation Euler angles into the equivalent reverse rotation Euler angles.
1944
1945 This if for the xyz notation.
1946
1947
1948 @param alpha: The alpha Euler angle in rad.
1949 @type alpha: float
1950 @param beta: The beta Euler angle in rad.
1951 @type beta: float
1952 @param gamma: The gamma Euler angle in rad.
1953 @type gamma: float
1954 @return: The alpha, beta, and gamma Euler angles for the reverse rotation.
1955 @rtype: tuple of float
1956 """
1957
1958
1959 R = zeros((3, 3), float64)
1960
1961
1962 euler_xyz_to_R(alpha, beta, gamma, R)
1963
1964
1965 R = transpose(R)
1966
1967
1968 return R_to_euler_xyz(R)
1969
1970
1972 """Convert the given forward rotation Euler angles into the equivalent reverse rotation Euler angles.
1973
1974 This if for the xzx notation.
1975
1976
1977 @param alpha: The alpha Euler angle in rad.
1978 @type alpha: float
1979 @param beta: The beta Euler angle in rad.
1980 @type beta: float
1981 @param gamma: The gamma Euler angle in rad.
1982 @type gamma: float
1983 @return: The alpha, beta, and gamma Euler angles for the reverse rotation.
1984 @rtype: tuple of float
1985 """
1986
1987
1988 R = zeros((3, 3), float64)
1989
1990
1991 euler_xzx_to_R(alpha, beta, gamma, R)
1992
1993
1994 R = transpose(R)
1995
1996
1997 return R_to_euler_xzx(R)
1998
1999
2001 """Convert the given forward rotation Euler angles into the equivalent reverse rotation Euler angles.
2002
2003 This if for the xzy notation.
2004
2005
2006 @param alpha: The alpha Euler angle in rad.
2007 @type alpha: float
2008 @param beta: The beta Euler angle in rad.
2009 @type beta: float
2010 @param gamma: The gamma Euler angle in rad.
2011 @type gamma: float
2012 @return: The alpha, beta, and gamma Euler angles for the reverse rotation.
2013 @rtype: tuple of float
2014 """
2015
2016
2017 R = zeros((3, 3), float64)
2018
2019
2020 euler_xzy_to_R(alpha, beta, gamma, R)
2021
2022
2023 R = transpose(R)
2024
2025
2026 return R_to_euler_xzy(R)
2027
2028
2030 """Convert the given forward rotation Euler angles into the equivalent reverse rotation Euler angles.
2031
2032 This if for the yxy notation.
2033
2034
2035 @param alpha: The alpha Euler angle in rad.
2036 @type alpha: float
2037 @param beta: The beta Euler angle in rad.
2038 @type beta: float
2039 @param gamma: The gamma Euler angle in rad.
2040 @type gamma: float
2041 @return: The alpha, beta, and gamma Euler angles for the reverse rotation.
2042 @rtype: tuple of float
2043 """
2044
2045
2046 R = zeros((3, 3), float64)
2047
2048
2049 euler_yxy_to_R(alpha, beta, gamma, R)
2050
2051
2052 R = transpose(R)
2053
2054
2055 return R_to_euler_yxy(R)
2056
2057
2059 """Convert the given forward rotation Euler angles into the equivalent reverse rotation Euler angles.
2060
2061 This if for the yxz notation.
2062
2063
2064 @param alpha: The alpha Euler angle in rad.
2065 @type alpha: float
2066 @param beta: The beta Euler angle in rad.
2067 @type beta: float
2068 @param gamma: The gamma Euler angle in rad.
2069 @type gamma: float
2070 @return: The alpha, beta, and gamma Euler angles for the reverse rotation.
2071 @rtype: tuple of float
2072 """
2073
2074
2075 R = zeros((3, 3), float64)
2076
2077
2078 euler_yxz_to_R(alpha, beta, gamma, R)
2079
2080
2081 R = transpose(R)
2082
2083
2084 return R_to_euler_yxz(R)
2085
2086
2088 """Convert the given forward rotation Euler angles into the equivalent reverse rotation Euler angles.
2089
2090 This if for the yzx notation.
2091
2092
2093 @param alpha: The alpha Euler angle in rad.
2094 @type alpha: float
2095 @param beta: The beta Euler angle in rad.
2096 @type beta: float
2097 @param gamma: The gamma Euler angle in rad.
2098 @type gamma: float
2099 @return: The alpha, beta, and gamma Euler angles for the reverse rotation.
2100 @rtype: tuple of float
2101 """
2102
2103
2104 R = zeros((3, 3), float64)
2105
2106
2107 euler_yzx_to_R(alpha, beta, gamma, R)
2108
2109
2110 R = transpose(R)
2111
2112
2113 return R_to_euler_yzx(R)
2114
2115
2117 """Convert the given forward rotation Euler angles into the equivalent reverse rotation Euler angles.
2118
2119 This if for the yzy notation.
2120
2121
2122 @param alpha: The alpha Euler angle in rad.
2123 @type alpha: float
2124 @param beta: The beta Euler angle in rad.
2125 @type beta: float
2126 @param gamma: The gamma Euler angle in rad.
2127 @type gamma: float
2128 @return: The alpha, beta, and gamma Euler angles for the reverse rotation.
2129 @rtype: tuple of float
2130 """
2131
2132
2133 R = zeros((3, 3), float64)
2134
2135
2136 euler_yzy_to_R(alpha, beta, gamma, R)
2137
2138
2139 R = transpose(R)
2140
2141
2142 return R_to_euler_yzy(R)
2143
2144
2146 """Convert the given forward rotation Euler angles into the equivalent reverse rotation Euler angles.
2147
2148 This if for the zxy notation.
2149
2150
2151 @param alpha: The alpha Euler angle in rad.
2152 @type alpha: float
2153 @param beta: The beta Euler angle in rad.
2154 @type beta: float
2155 @param gamma: The gamma Euler angle in rad.
2156 @type gamma: float
2157 @return: The alpha, beta, and gamma Euler angles for the reverse rotation.
2158 @rtype: tuple of float
2159 """
2160
2161
2162 R = zeros((3, 3), float64)
2163
2164
2165 euler_zxy_to_R(alpha, beta, gamma, R)
2166
2167
2168 R = transpose(R)
2169
2170
2171 return R_to_euler_zxy(R)
2172
2173
2175 """Convert the given forward rotation Euler angles into the equivalent reverse rotation Euler angles.
2176
2177 This if for the zxz notation.
2178
2179
2180 @param alpha: The alpha Euler angle in rad.
2181 @type alpha: float
2182 @param beta: The beta Euler angle in rad.
2183 @type beta: float
2184 @param gamma: The gamma Euler angle in rad.
2185 @type gamma: float
2186 @return: The alpha, beta, and gamma Euler angles for the reverse rotation.
2187 @rtype: tuple of float
2188 """
2189
2190
2191 R = zeros((3, 3), float64)
2192
2193
2194 euler_zxz_to_R(alpha, beta, gamma, R)
2195
2196
2197 R = transpose(R)
2198
2199
2200 return R_to_euler_zxz(R)
2201
2202
2204 """Convert the given forward rotation Euler angles into the equivalent reverse rotation Euler angles.
2205
2206 This if for the zyx notation.
2207
2208
2209 @param alpha: The alpha Euler angle in rad.
2210 @type alpha: float
2211 @param beta: The beta Euler angle in rad.
2212 @type beta: float
2213 @param gamma: The gamma Euler angle in rad.
2214 @type gamma: float
2215 @return: The alpha, beta, and gamma Euler angles for the reverse rotation.
2216 @rtype: tuple of float
2217 """
2218
2219
2220 R = zeros((3, 3), float64)
2221
2222
2223 euler_zyx_to_R(alpha, beta, gamma, R)
2224
2225
2226 R = transpose(R)
2227
2228
2229 return R_to_euler_zyx(R)
2230
2231
2233 """Convert the given forward rotation Euler angles into the equivalent reverse rotation Euler angles.
2234
2235 This if for the zyz notation.
2236
2237
2238 @param alpha: The alpha Euler angle in rad.
2239 @type alpha: float
2240 @param beta: The beta Euler angle in rad.
2241 @type beta: float
2242 @param gamma: The gamma Euler angle in rad.
2243 @type gamma: float
2244 @return: The alpha, beta, and gamma Euler angles for the reverse rotation.
2245 @rtype: tuple of float
2246 """
2247
2248
2249 R = zeros((3, 3), float64)
2250
2251
2252 euler_to_R_zyz(alpha, beta, gamma, R)
2253
2254
2255 R = transpose(R)
2256
2257
2258 return R_to_euler_zyz(R)
2259
2260
2262 """Convert a quaternion into the axis-angle notation.
2263
2264 Conversion equations
2265 ====================
2266
2267 From Wolfram MathWorld (http://mathworld.wolfram.com/Quaternion.html), the conversion is given by::
2268
2269 q = (cos(angle/2), n * sin(angle/2)),
2270
2271 where q is the quaternion and n is the unit vector representing the rotation axis. Therfore::
2272
2273 angle = 2*acos(w),
2274
2275 axis = 2*asin([x, y, z])
2276
2277 @param quat: The quaternion.
2278 @type quat: numpy 4D, rank-1 array
2279 @return: The 3D rotation axis and angle.
2280 @rtype: numpy 3D rank-1 array, float
2281 """
2282
2283
2284 angle = 2 * acos(quat[0])
2285
2286
2287 if angle:
2288 axis = quat[1:] / sin(angle/2)
2289 else:
2290 axis = quat[1:] * 0.0
2291
2292
2293 return axis, angle
2294
2295
2297 """Convert a quaternion into rotation matrix form.
2298
2299 This is from Wikipedia (http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion), where::
2300
2301 | 1 - 2y**2 - 2z**2 2xy - 2zw 2xz + 2yw |
2302 Q = | 2xy + 2zw 1 - 2x**2 - 2z**2 2yz - 2xw |,
2303 | 2xz - 2yw 2yz + 2xw 1 - 2x**2 - 2y**2 |
2304
2305 and where the quaternion is defined as q = (w, x, y, z). This has been verified using Simo
2306 Saerkkae's "Notes on Quaternions" at http://www.lce.hut.fi/~ssarkka/.
2307
2308
2309 @param quat: The quaternion.
2310 @type quat: numpy 4D, rank-1 array
2311 @param R: A 3D matrix to convert to the rotation matrix.
2312 @type R: numpy 3D, rank-2 array
2313 """
2314
2315
2316 (w, x, y, z) = quat
2317
2318
2319 x2 = 2.0 * x**2
2320 y2 = 2.0 * y**2
2321 z2 = 2.0 * z**2
2322 xw = 2.0 * x*w
2323 xy = 2.0 * x*y
2324 xz = 2.0 * x*z
2325 yw = 2.0 * y*w
2326 yz = 2.0 * y*z
2327 zw = 2.0 * z*w
2328
2329
2330 R[0, 0] = 1.0 - y2 - z2
2331 R[1, 1] = 1.0 - x2 - z2
2332 R[2, 2] = 1.0 - x2 - y2
2333
2334
2335 R[0, 1] = xy - zw
2336 R[0, 2] = xz + yw
2337 R[1, 2] = yz - xw
2338
2339 R[1, 0] = xy + zw
2340 R[2, 0] = xz - yw
2341 R[2, 1] = yz + xw
2342
2343
2345 """Generate a rotation matrix from the tilt and torsion rotation angles.
2346
2347 This notation is taken from "Bonev, I. A. and Gosselin, C. M. (2006) Analytical determination of the workspace of symmetrical spherical parallel mechanisms. IEEE Transactions on Robotics, 22(5), 1011-1017".
2348
2349
2350 @param phi: The angle defining the x-y plane rotation axis.
2351 @type phi: float
2352 @param theta: The tilt angle - the angle of rotation about the x-y plane rotation axis.
2353 @type theta: float
2354 @param sigma: The torsion angle - the angle of rotation about the z' axis.
2355 @type sigma: float
2356 @param R: The 3x3 rotation matrix to update.
2357 @type R: 3D, rank-2 numpy array
2358 """
2359
2360
2361 alpha = sigma - phi
2362 beta = theta
2363 gamma = phi
2364
2365
2366 euler_to_R_zyz(alpha, beta, gamma, R)
2367
2368
2370 """Calculate the rotation matrix required to rotate from one vector to another.
2371
2372 For the rotation of one vector to another, there are an infinit series of rotation matrices
2373 possible. Due to axially symmetry, the rotation axis can be any vector lying in the symmetry
2374 plane between the two vectors. Hence the axis-angle convention will be used to construct the
2375 matrix with the rotation axis defined as the cross product of the two vectors. The rotation
2376 angle is the arccosine of the dot product of the two unit vectors.
2377
2378 Given a unit vector parallel to the rotation axis, w = [x, y, z] and the rotation angle a,
2379 the rotation matrix R is::
2380
2381 | 1 + (1-cos(a))*(x*x-1) -z*sin(a)+(1-cos(a))*x*y y*sin(a)+(1-cos(a))*x*z |
2382 R = | z*sin(a)+(1-cos(a))*x*y 1 + (1-cos(a))*(y*y-1) -x*sin(a)+(1-cos(a))*y*z |
2383 | -y*sin(a)+(1-cos(a))*x*z x*sin(a)+(1-cos(a))*y*z 1 + (1-cos(a))*(z*z-1) |
2384
2385
2386 @param vector_orig: The unrotated vector defined in the reference frame.
2387 @type vector_orig: numpy array, len 3
2388 @param vector_fin: The rotated vector defined in the reference frame.
2389 @type vector_fin: numpy array, len 3
2390 @param R: The 3x3 rotation matrix to update.
2391 @type R: 3x3 numpy array
2392 """
2393
2394
2395 vector_orig = vector_orig / norm(vector_orig)
2396 vector_fin = vector_fin / norm(vector_fin)
2397
2398
2399 axis = cross(vector_orig, vector_fin)
2400 axis_len = norm(axis)
2401 if axis_len != 0.0:
2402 axis = axis / axis_len
2403
2404
2405 x = axis[0]
2406 y = axis[1]
2407 z = axis[2]
2408
2409
2410 angle = acos(dot(vector_orig, vector_fin))
2411
2412
2413 ca = cos(angle)
2414 sa = sin(angle)
2415
2416
2417 R[0, 0] = 1.0 + (1.0 - ca)*(x**2 - 1.0)
2418 R[0, 1] = -z*sa + (1.0 - ca)*x*y
2419 R[0, 2] = y*sa + (1.0 - ca)*x*z
2420 R[1, 0] = z*sa+(1.0 - ca)*x*y
2421 R[1, 1] = 1.0 + (1.0 - ca)*(y**2 - 1.0)
2422 R[1, 2] = -x*sa+(1.0 - ca)*y*z
2423 R[2, 0] = -y*sa+(1.0 - ca)*x*z
2424 R[2, 1] = x*sa+(1.0 - ca)*y*z
2425 R[2, 2] = 1.0 + (1.0 - ca)*(z**2 - 1.0)
2426