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