1 /* -*- C++ -*- ------------------------------------------------------------
3 Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
5 The Configurable Math Library (CML) is distributed under the terms of the
6 Boost Software License, v1.0 (see cml/LICENSE for details).
8 *-----------------------------------------------------------------------*/
13 #ifndef matrix_rotation_h
14 #define matrix_rotation_h
16 #include <cml/mathlib/matrix_misc.h>
17 #include <cml/mathlib/vector_ortho.h>
19 /* Functions related to matrix rotations in 3D and 2D. */
23 //////////////////////////////////////////////////////////////////////////////
24 // 3D rotation about world axes
25 //////////////////////////////////////////////////////////////////////////////
27 /** Build a matrix representing a 3D rotation about the given world axis */
28 template < typename E
, class A
, class B
, class L
> void
29 matrix_rotation_world_axis( matrix
<E
,A
,B
,L
>& m
, size_t axis
, E angle
)
31 typedef matrix
<E
,A
,B
,L
> matrix_type
;
32 typedef typename
matrix_type::value_type value_type
;
35 detail::CheckMatLinear3D(m
);
36 detail::CheckIndex3(axis
);
39 cyclic_permutation(axis
, i
, j
, k
);
41 value_type s
= value_type(std::sin(angle
));
42 value_type c
= value_type(std::cos(angle
));
44 identity_transform(m
);
46 m
.set_basis_element(j
,j
, c
);
47 m
.set_basis_element(j
,k
, s
);
48 m
.set_basis_element(k
,j
,-s
);
49 m
.set_basis_element(k
,k
, c
);
52 /** Build a matrix representing a 3D rotation about the world x axis */
53 template < typename E
, class A
, class B
, class L
> void
54 matrix_rotation_world_x(matrix
<E
,A
,B
,L
>& m
, E angle
) {
55 matrix_rotation_world_axis(m
,0,angle
);
58 /** Build a matrix representing a 3D rotation about the world y axis */
59 template < typename E
, class A
, class B
, class L
> void
60 matrix_rotation_world_y(matrix
<E
,A
,B
,L
>& m
, E angle
) {
61 matrix_rotation_world_axis(m
,1,angle
);
64 /** Build a matrix representing a 3D rotation about the world z axis */
65 template < typename E
, class A
, class B
, class L
> void
66 matrix_rotation_world_z(matrix
<E
,A
,B
,L
>& m
, E angle
) {
67 matrix_rotation_world_axis(m
,2,angle
);
70 //////////////////////////////////////////////////////////////////////////////
71 // 3D rotation from an axis-angle pair
72 //////////////////////////////////////////////////////////////////////////////
74 /** Build a rotation matrix from an axis-angle pair */
75 template < typename E
, class A
, class B
, class L
, class VecT
> void
76 matrix_rotation_axis_angle(matrix
<E
,A
,B
,L
>& m
, const VecT
& axis
, E angle
)
78 typedef matrix
<E
,A
,B
,L
> matrix_type
;
79 typedef typename
matrix_type::value_type value_type
;
82 detail::CheckMatLinear3D(m
);
83 detail::CheckVec3(axis
);
85 identity_transform(m
);
87 value_type s
= std::sin(angle
);
88 value_type c
= std::cos(angle
);
89 value_type omc
= value_type(1) - c
;
91 value_type xomc
= axis
[0] * omc
;
92 value_type yomc
= axis
[1] * omc
;
93 value_type zomc
= axis
[2] * omc
;
95 value_type xxomc
= axis
[0] * xomc
;
96 value_type yyomc
= axis
[1] * yomc
;
97 value_type zzomc
= axis
[2] * zomc
;
98 value_type xyomc
= axis
[0] * yomc
;
99 value_type yzomc
= axis
[1] * zomc
;
100 value_type zxomc
= axis
[2] * xomc
;
102 value_type xs
= axis
[0] * s
;
103 value_type ys
= axis
[1] * s
;
104 value_type zs
= axis
[2] * s
;
106 m
.set_basis_element(0,0, xxomc
+ c
);
107 m
.set_basis_element(0,1, xyomc
+ zs
);
108 m
.set_basis_element(0,2, zxomc
- ys
);
109 m
.set_basis_element(1,0, xyomc
- zs
);
110 m
.set_basis_element(1,1, yyomc
+ c
);
111 m
.set_basis_element(1,2, yzomc
+ xs
);
112 m
.set_basis_element(2,0, zxomc
+ ys
);
113 m
.set_basis_element(2,1, yzomc
- xs
);
114 m
.set_basis_element(2,2, zzomc
+ c
);
117 //////////////////////////////////////////////////////////////////////////////
118 // 3D rotation from a quaternion
119 //////////////////////////////////////////////////////////////////////////////
121 /** Build a rotation matrix from a quaternion */
122 template < typename E
, class A
, class B
, class L
, class QuatT
> void
123 matrix_rotation_quaternion(matrix
<E
,A
,B
,L
>& m
, const QuatT
& q
)
125 typedef matrix
<E
,A
,B
,L
> matrix_type
;
126 typedef QuatT quaternion_type
;
127 typedef typename
quaternion_type::order_type order_type
;
128 typedef typename
matrix_type::value_type value_type
;
138 detail::CheckMatLinear3D(m
);
139 detail::CheckQuat(q
);
141 identity_transform(m
);
143 value_type x2
= q
[X
] + q
[X
];
144 value_type y2
= q
[Y
] + q
[Y
];
145 value_type z2
= q
[Z
] + q
[Z
];
147 value_type xx2
= q
[X
] * x2
;
148 value_type yy2
= q
[Y
] * y2
;
149 value_type zz2
= q
[Z
] * z2
;
150 value_type xy2
= q
[X
] * y2
;
151 value_type yz2
= q
[Y
] * z2
;
152 value_type zx2
= q
[Z
] * x2
;
153 value_type xw2
= q
[W
] * x2
;
154 value_type yw2
= q
[W
] * y2
;
155 value_type zw2
= q
[W
] * z2
;
157 m
.set_basis_element(0,0, value_type(1) - yy2
- zz2
);
158 m
.set_basis_element(0,1, xy2
+ zw2
);
159 m
.set_basis_element(0,2, zx2
- yw2
);
160 m
.set_basis_element(1,0, xy2
- zw2
);
161 m
.set_basis_element(1,1, value_type(1) - zz2
- xx2
);
162 m
.set_basis_element(1,2, yz2
+ xw2
);
163 m
.set_basis_element(2,0, zx2
+ yw2
);
164 m
.set_basis_element(2,1, yz2
- xw2
);
165 m
.set_basis_element(2,2, value_type(1) - xx2
- yy2
);
168 //////////////////////////////////////////////////////////////////////////////
169 // 3D rotation from Euler angles
170 //////////////////////////////////////////////////////////////////////////////
172 /** Build a rotation matrix from an Euler-angle triple
174 * The rotations are applied about the cardinal axes in the order specified by
175 * the 'order' argument, where 'order' is one of the following enumerants:
191 template < typename E
, class A
, class B
, class L
> void
192 matrix_rotation_euler(matrix
<E
,A
,B
,L
>& m
, E angle_0
, E angle_1
, E angle_2
,
195 typedef matrix
<E
,A
,B
,L
> matrix_type
;
196 typedef typename
matrix_type::value_type value_type
;
199 detail::CheckMatLinear3D(m
);
201 identity_transform(m
);
205 detail::unpack_euler_order(order
, i
, j
, k
, odd
, repeat
);
213 value_type s0
= std::sin(angle_0
);
214 value_type c0
= std::cos(angle_0
);
215 value_type s1
= std::sin(angle_1
);
216 value_type c1
= std::cos(angle_1
);
217 value_type s2
= std::sin(angle_2
);
218 value_type c2
= std::cos(angle_2
);
220 value_type s0s2
= s0
* s2
;
221 value_type s0c2
= s0
* c2
;
222 value_type c0s2
= c0
* s2
;
223 value_type c0c2
= c0
* c2
;
226 m
.set_basis_element(i
,i
, c1
);
227 m
.set_basis_element(i
,j
, s1
* s2
);
228 m
.set_basis_element(i
,k
,-s1
* c2
);
229 m
.set_basis_element(j
,i
, s0
* s1
);
230 m
.set_basis_element(j
,j
,-c1
* s0s2
+ c0c2
);
231 m
.set_basis_element(j
,k
, c1
* s0c2
+ c0s2
);
232 m
.set_basis_element(k
,i
, c0
* s1
);
233 m
.set_basis_element(k
,j
,-c1
* c0s2
- s0c2
);
234 m
.set_basis_element(k
,k
, c1
* c0c2
- s0s2
);
236 m
.set_basis_element(i
,i
, c1
* c2
);
237 m
.set_basis_element(i
,j
, c1
* s2
);
238 m
.set_basis_element(i
,k
,-s1
);
239 m
.set_basis_element(j
,i
, s1
* s0c2
- c0s2
);
240 m
.set_basis_element(j
,j
, s1
* s0s2
+ c0c2
);
241 m
.set_basis_element(j
,k
, s0
* c1
);
242 m
.set_basis_element(k
,i
, s1
* c0c2
+ s0s2
);
243 m
.set_basis_element(k
,j
, s1
* c0s2
- s0c2
);
244 m
.set_basis_element(k
,k
, c0
* c1
);
248 //////////////////////////////////////////////////////////////////////////////
249 // 3D rotation to align with a vector, multiple vectors, or the view plane
250 //////////////////////////////////////////////////////////////////////////////
252 /** See vector_ortho.h for details */
253 template < typename E
,class A
,class B
,class L
,class VecT_1
,class VecT_2
> void
254 matrix_rotation_align(
257 const VecT_2
& reference
,
258 bool normalize
= true,
259 AxisOrder order
= axis_order_zyx
)
261 typedef vector
< E
,fixed
<3> > vector_type
;
263 identity_transform(m
);
267 orthonormal_basis(align
, reference
, x
, y
, z
, normalize
, order
);
268 matrix_set_basis_vectors(m
, x
, y
, z
);
271 /** See vector_ortho.h for details */
272 template < typename E
, class A
, class B
, class L
, class VecT
> void
273 matrix_rotation_align(matrix
<E
,A
,B
,L
>& m
, const VecT
& align
,
274 bool normalize
= true, AxisOrder order
= axis_order_zyx
)
276 typedef vector
< E
,fixed
<3> > vector_type
;
278 identity_transform(m
);
282 orthonormal_basis(align
, x
, y
, z
, normalize
, order
);
283 matrix_set_basis_vectors(m
, x
, y
, z
);
286 /** See vector_ortho.h for details */
287 template < typename E
,class A
,class B
,class L
,class VecT_1
,class VecT_2
> void
288 matrix_rotation_align_axial(matrix
<E
,A
,B
,L
>& m
, const VecT_1
& align
,
289 const VecT_2
& axis
, bool normalize
= true,
290 AxisOrder order
= axis_order_zyx
)
292 typedef vector
< E
,fixed
<3> > vector_type
;
294 identity_transform(m
);
298 orthonormal_basis_axial(align
, axis
, x
, y
, z
, normalize
, order
);
299 matrix_set_basis_vectors(m
, x
, y
, z
);
302 /** See vector_ortho.h for details */
303 template < typename E
, class A
, class B
, class L
, class MatT
> void
304 matrix_rotation_align_viewplane(
306 const MatT
& view_matrix
,
307 Handedness handedness
,
308 AxisOrder order
= axis_order_zyx
)
310 typedef vector
< E
, fixed
<3> > vector_type
;
312 identity_transform(m
);
316 orthonormal_basis_viewplane(view_matrix
, x
, y
, z
, handedness
, order
);
317 matrix_set_basis_vectors(m
, x
, y
, z
);
320 /** See vector_ortho.h for details */
321 template < typename E
, class A
, class B
, class L
, class MatT
> void
322 matrix_rotation_align_viewplane_LH(
324 const MatT
& view_matrix
,
325 AxisOrder order
= axis_order_zyx
)
327 matrix_rotation_align_viewplane(
328 m
,view_matrix
,left_handed
,order
);
331 /** See vector_ortho.h for details */
332 template < typename E
, class A
, class B
, class L
, class MatT
> void
333 matrix_rotation_align_viewplane_RH(
335 const MatT
& view_matrix
,
336 AxisOrder order
= axis_order_zyx
)
338 matrix_rotation_align_viewplane(
339 m
,view_matrix
,right_handed
,order
);
342 //////////////////////////////////////////////////////////////////////////////
343 // 3D rotation to aim at a target
344 //////////////////////////////////////////////////////////////////////////////
346 /** See vector_ortho.h for details */
347 template < typename E
, class A
, class B
, class L
,
348 class VecT_1
, class VecT_2
, class VecT_3
> void
349 matrix_rotation_aim_at(
352 const VecT_2
& target
,
353 const VecT_3
& reference
,
354 AxisOrder order
= axis_order_zyx
)
356 matrix_rotation_align(m
, target
- pos
, reference
, true, order
);
359 /** See vector_ortho.h for details */
360 template < typename E
, class A
, class B
, class L
,
361 class VecT_1
, class VecT_2
> void
362 matrix_rotation_aim_at(
365 const VecT_2
& target
,
366 AxisOrder order
= axis_order_zyx
)
368 matrix_rotation_align(m
, target
- pos
, true, order
);
371 /** See vector_ortho.h for details */
372 template < typename E
, class A
, class B
, class L
,
373 class VecT_1
, class VecT_2
, class VecT_3
> void
374 matrix_rotation_aim_at_axial(
377 const VecT_2
& target
,
379 AxisOrder order
= axis_order_zyx
)
381 matrix_rotation_align_axial(m
, target
- pos
, axis
, true, order
);
384 //////////////////////////////////////////////////////////////////////////////
386 //////////////////////////////////////////////////////////////////////////////
388 /** Build a matrix representing a 2D rotation */
389 template < typename E
, class A
, class B
, class L
> void
390 matrix_rotation_2D( matrix
<E
,A
,B
,L
>& m
, E angle
)
392 typedef matrix
<E
,A
,B
,L
> matrix_type
;
393 typedef typename
matrix_type::value_type value_type
;
396 detail::CheckMatLinear2D(m
);
398 value_type s
= value_type(std::sin(angle
));
399 value_type c
= value_type(std::cos(angle
));
401 identity_transform(m
);
403 m
.set_basis_element(0,0, c
);
404 m
.set_basis_element(0,1, s
);
405 m
.set_basis_element(1,0,-s
);
406 m
.set_basis_element(1,1, c
);
409 //////////////////////////////////////////////////////////////////////////////
410 // 2D rotation to align with a vector
411 //////////////////////////////////////////////////////////////////////////////
413 /** See vector_ortho.h for details */
414 template < typename E
, class A
, class B
, class L
, class VecT
> void
415 matrix_rotation_align_2D(matrix
<E
,A
,B
,L
>& m
, const VecT
& align
,
416 bool normalize
= true, AxisOrder2D order
= axis_order_xy
)
418 typedef vector
< E
, fixed
<2> > vector_type
;
420 identity_transform(m
);
424 orthonormal_basis_2D(align
, x
, y
, normalize
, order
);
425 matrix_set_basis_vectors_2D(m
, x
, y
);
428 //////////////////////////////////////////////////////////////////////////////
429 // 3D relative rotation about world axes
430 //////////////////////////////////////////////////////////////////////////////
432 /** Rotate a rotation matrix about the given world axis */
433 template < typename E
, class A
, class B
, class L
> void
434 matrix_rotate_about_world_axis(matrix
<E
,A
,B
,L
>& m
, size_t axis
, E angle
)
436 typedef matrix
<E
,A
,B
,L
> matrix_type
;
437 typedef typename
matrix_type::value_type value_type
;
440 detail::CheckMatLinear3D(m
);
441 detail::CheckIndex3(axis
);
444 cyclic_permutation(axis
, i
, j
, k
);
446 value_type s
= value_type(std::sin(angle
));
447 value_type c
= value_type(std::cos(angle
));
449 value_type ij
= c
* m
.basis_element(i
,j
) - s
* m
.basis_element(i
,k
);
450 value_type jj
= c
* m
.basis_element(j
,j
) - s
* m
.basis_element(j
,k
);
451 value_type kj
= c
* m
.basis_element(k
,j
) - s
* m
.basis_element(k
,k
);
453 m
.set_basis_element(i
,k
, s
*m
.basis_element(i
,j
) + c
*m
.basis_element(i
,k
));
454 m
.set_basis_element(j
,k
, s
*m
.basis_element(j
,j
) + c
*m
.basis_element(j
,k
));
455 m
.set_basis_element(k
,k
, s
*m
.basis_element(k
,j
) + c
*m
.basis_element(k
,k
));
457 m
.set_basis_element(i
,j
,ij
);
458 m
.set_basis_element(j
,j
,jj
);
459 m
.set_basis_element(k
,j
,kj
);
462 /** Rotate a rotation matrix about the world x axis */
463 template < typename E
, class A
, class B
, class L
> void
464 matrix_rotate_about_world_x(matrix
<E
,A
,B
,L
>& m
, E angle
) {
465 matrix_rotate_about_world_axis(m
,0,angle
);
468 /** Rotate a rotation matrix about the world y axis */
469 template < typename E
, class A
, class B
, class L
> void
470 matrix_rotate_about_world_y(matrix
<E
,A
,B
,L
>& m
, E angle
) {
471 matrix_rotate_about_world_axis(m
,1,angle
);
474 /** Rotate a rotation matrix about the world z axis */
475 template < typename E
, class A
, class B
, class L
> void
476 matrix_rotate_about_world_z(matrix
<E
,A
,B
,L
>& m
, E angle
) {
477 matrix_rotate_about_world_axis(m
,2,angle
);
480 //////////////////////////////////////////////////////////////////////////////
481 // 3D relative rotation about local axes
482 //////////////////////////////////////////////////////////////////////////////
484 /** Rotate a rotation matrix about the given local axis */
485 template < typename E
, class A
, class B
, class L
> void
486 matrix_rotate_about_local_axis(matrix
<E
,A
,B
,L
>& m
, size_t axis
, E angle
)
488 typedef matrix
<E
,A
,B
,L
> matrix_type
;
489 typedef typename
matrix_type::value_type value_type
;
492 detail::CheckMatLinear3D(m
);
493 detail::CheckIndex3(axis
);
496 cyclic_permutation(axis
, i
, j
, k
);
498 value_type s
= value_type(std::sin(angle
));
499 value_type c
= value_type(std::cos(angle
));
501 value_type j0
= c
* m
.basis_element(j
,0) + s
* m
.basis_element(k
,0);
502 value_type j1
= c
* m
.basis_element(j
,1) + s
* m
.basis_element(k
,1);
503 value_type j2
= c
* m
.basis_element(j
,2) + s
* m
.basis_element(k
,2);
505 m
.set_basis_element(k
,0, c
*m
.basis_element(k
,0) - s
*m
.basis_element(j
,0));
506 m
.set_basis_element(k
,1, c
*m
.basis_element(k
,1) - s
*m
.basis_element(j
,1));
507 m
.set_basis_element(k
,2, c
*m
.basis_element(k
,2) - s
*m
.basis_element(j
,2));
509 m
.set_basis_element(j
,0,j0
);
510 m
.set_basis_element(j
,1,j1
);
511 m
.set_basis_element(j
,2,j2
);
514 /** Rotate a rotation matrix about its local x axis */
515 template < typename E
, class A
, class B
, class L
> void
516 matrix_rotate_about_local_x(matrix
<E
,A
,B
,L
>& m
, E angle
) {
517 matrix_rotate_about_local_axis(m
,0,angle
);
520 /** Rotate a rotation matrix about its local y axis */
521 template < typename E
, class A
, class B
, class L
> void
522 matrix_rotate_about_local_y(matrix
<E
,A
,B
,L
>& m
, E angle
) {
523 matrix_rotate_about_local_axis(m
,1,angle
);
526 /** Rotate a rotation matrix about its local z axis */
527 template < typename E
, class A
, class B
, class L
> void
528 matrix_rotate_about_local_z(matrix
<E
,A
,B
,L
>& m
, E angle
) {
529 matrix_rotate_about_local_axis(m
,2,angle
);
532 //////////////////////////////////////////////////////////////////////////////
533 // 2D relative rotation
534 //////////////////////////////////////////////////////////////////////////////
536 template < typename E
, class A
, class B
, class L
> void
537 matrix_rotate_2D(matrix
<E
,A
,B
,L
>& m
, E angle
)
539 typedef matrix
<E
,A
,B
,L
> matrix_type
;
540 typedef typename
matrix_type::value_type value_type
;
543 detail::CheckMatLinear2D(m
);
545 value_type s
= value_type(std::sin(angle
));
546 value_type c
= value_type(std::cos(angle
));
548 value_type m00
= c
* m
.basis_element(0,0) - s
* m
.basis_element(0,1);
549 value_type m10
= c
* m
.basis_element(1,0) - s
* m
.basis_element(1,1);
551 m
.set_basis_element(0,1, s
*m
.basis_element(0,0) + c
*m
.basis_element(0,1));
552 m
.set_basis_element(1,1, s
*m
.basis_element(1,0) + c
*m
.basis_element(1,1));
554 m
.set_basis_element(0,0,m00
);
555 m
.set_basis_element(1,0,m10
);
558 //////////////////////////////////////////////////////////////////////////////
559 // Rotation from vector to vector
560 //////////////////////////////////////////////////////////////////////////////
562 /** Build a rotation matrix to rotate from one vector to another
564 * Note: The quaternion algorithm is more stable than the matrix algorithm, so
565 * we simply pass off to the quaternion function here.
567 template < class E
,class A
,class B
,class L
,class VecT_1
,class VecT_2
> void
568 matrix_rotation_vec_to_vec(
572 bool unit_length_vectors
= false)
574 typedef quaternion
< E
,fixed
<>,vector_first
,positive_cross
>
578 quaternion_rotation_vec_to_vec(q
,v1
,v2
,unit_length_vectors
);
579 matrix_rotation_quaternion(m
,q
);
582 //////////////////////////////////////////////////////////////////////////////
583 // Scale the angle of a rotation matrix
584 //////////////////////////////////////////////////////////////////////////////
586 /** Scale the angle of a 3D rotation matrix */
587 template < typename E
, class A
, class B
, class L
> void
588 matrix_scale_rotation_angle(matrix
<E
,A
,B
,L
>& m
, E t
,
589 E tolerance
= epsilon
<E
>::placeholder())
591 typedef vector
< E
,fixed
<3> > vector_type
;
592 typedef typename
vector_type::value_type value_type
;
596 matrix_to_axis_angle(m
, axis
, angle
, tolerance
);
597 matrix_rotation_axis_angle(m
, axis
, angle
* t
);
600 /** Scale the angle of a 2D rotation matrix */
601 template < typename E
, class A
, class B
, class L
> void
602 matrix_scale_rotation_angle_2D(
603 matrix
<E
,A
,B
,L
>& m
, E t
, E tolerance
= epsilon
<E
>::placeholder())
605 typedef vector
< E
,fixed
<2> > vector_type
;
606 typedef typename
vector_type::value_type value_type
;
608 value_type angle
= matrix_to_rotation_2D(m
);
609 matrix_rotation_2D(m
, angle
* t
);
612 //////////////////////////////////////////////////////////////////////////////
613 // Support functions for uniform handling of row- and column-basis matrices
614 //////////////////////////////////////////////////////////////////////////////
616 /* Note: The matrix rotation slerp, difference and concatenation functions do
617 * not use et::MatrixPromote<M1,M2>::temporary_type as the return type, even
618 * though that is the return type of the underlying matrix multiplication.
619 * This is because the sizes of these matrices are known at compile time (3x3
620 * and 2x2), and using fixed<> obviates the need for resizing of intermediate
623 * Also, no size- or type-checking is done on the arguments to these
624 * functions, as any such errors will be caught by the matrix multiplication
625 * and assignment to the 3x3 temporary.
628 /** A fixed-size temporary 3x3 matrix */
629 #define MAT_TEMP_3X3 matrix< \
630 typename et::ScalarPromote< \
631 typename MatT_1::value_type, \
632 typename MatT_2::value_type \
635 typename MatT_1::basis_orient, \
639 /** A fixed-size temporary 2x2 matrix */
640 #define MAT_TEMP_2X2 matrix< \
641 typename et::ScalarPromote< \
642 typename MatT_1::value_type, \
643 typename MatT_2::value_type \
646 typename MatT_1::basis_orient, \
652 /** Concatenate two 3D row-basis rotation matrices in the order m1->m2 */
653 template < class MatT_1
, class MatT_2
> MAT_TEMP_3X3
654 matrix_concat_rotations(const MatT_1
& m1
, const MatT_2
& m2
, row_basis
) {
658 /** Concatenate two 3D col-basis rotation matrices in the order m1->m2 */
659 template < class MatT_1
, class MatT_2
> MAT_TEMP_3X3
660 matrix_concat_rotations(const MatT_1
& m1
, const MatT_2
& m2
, col_basis
) {
664 /** Concatenate two 3D rotation matrices in the order m1->m2 */
665 template < class MatT_1
, class MatT_2
> MAT_TEMP_3X3
666 matrix_concat_rotations(const MatT_1
& m1
, const MatT_2
& m2
) {
667 return matrix_concat_rotations(m1
,m2
,typename
MatT_1::basis_orient());
670 /** Concatenate two 2D row-basis rotation matrices in the order m1->m2 */
671 template < class MatT_1
, class MatT_2
> MAT_TEMP_2X2
672 matrix_concat_rotations_2D(const MatT_1
& m1
, const MatT_2
& m2
, row_basis
) {
676 /** Concatenate two 2D col-basis rotation matrices in the order m1->m2 */
677 template < class MatT_1
, class MatT_2
> MAT_TEMP_2X2
678 matrix_concat_rotations_2D(const MatT_1
& m1
, const MatT_2
& m2
, col_basis
) {
682 /** Concatenate two 2D rotation matrices in the order m1->m2 */
683 template < class MatT_1
, class MatT_2
> MAT_TEMP_2X2
684 matrix_concat_rotations_2D(const MatT_1
& m1
, const MatT_2
& m2
) {
685 return matrix_concat_rotations_2D(m1
,m2
,typename
MatT_1::basis_orient());
688 } // namespace detail
690 //////////////////////////////////////////////////////////////////////////////
691 // Matrix rotation difference
692 //////////////////////////////////////////////////////////////////////////////
694 /** Return the rotational 'difference' between two 3D rotation matrices */
695 template < class MatT_1
, class MatT_2
> MAT_TEMP_3X3
696 matrix_rotation_difference(const MatT_1
& m1
, const MatT_2
& m2
) {
697 return detail::matrix_concat_rotations(transpose(m1
),m2
);
700 /** Return the rotational 'difference' between two 2D rotation matrices */
701 template < class MatT_1
, class MatT_2
> MAT_TEMP_2X2
702 matrix_rotation_difference_2D(const MatT_1
& m1
, const MatT_2
& m2
) {
703 return detail::matrix_concat_rotations_2D(transpose(m1
),m2
);
706 //////////////////////////////////////////////////////////////////////////////
707 // Spherical linear interpolation of rotation matrices
708 //////////////////////////////////////////////////////////////////////////////
710 /* @todo: It might be as fast or faster to simply convert the matrices to
711 * quaternions, interpolate, and convert back.
713 * @todo: The behavior of matrix slerp is currently a little different than
714 * for quaternions: in the matrix function, when the two matrices are close
715 * to identical the first is returned, while in the quaternion function the
716 * quaternions are nlerp()'d in this case.
718 * I still need to do the equivalent of nlerp() for matrices, in which case
719 * these functions could be revised to pass off to nlerp() when the matrices
720 * are nearly aligned.
723 /** Spherical linear interpolation of two 3D rotation matrices */
724 template < class MatT_1
, class MatT_2
, typename E
> MAT_TEMP_3X3
725 matrix_slerp(const MatT_1
& m1
, const MatT_2
& m2
, E t
,
726 E tolerance
= epsilon
<E
>::placeholder())
728 typedef MAT_TEMP_3X3 temporary_type
;
730 temporary_type m
= matrix_rotation_difference(m1
,m2
);
731 matrix_scale_rotation_angle(m
,t
,tolerance
);
732 return detail::matrix_concat_rotations(m1
,m
);
735 /** Spherical linear interpolation of two 2D rotation matrices */
736 template < class MatT_1
, class MatT_2
, typename E
> MAT_TEMP_2X2
737 matrix_slerp_2D(const MatT_1
& m1
, const MatT_2
& m2
, E t
,
738 E tolerance
= epsilon
<E
>::placeholder())
740 typedef MAT_TEMP_2X2 temporary_type
;
742 temporary_type m
= matrix_rotation_difference_2D(m1
,m2
);
743 matrix_scale_rotation_angle_2D(m
,t
,tolerance
);
744 return detail::matrix_concat_rotations_2D(m1
,m
);
750 //////////////////////////////////////////////////////////////////////////////
752 //////////////////////////////////////////////////////////////////////////////
754 /** Convert a 3D rotation matrix to an axis-angle pair */
755 template < class MatT
, typename E
, class A
> void
756 matrix_to_axis_angle(
760 E tolerance
= epsilon
<E
>::placeholder())
762 typedef MatT matrix_type
;
763 typedef typename
matrix_type::value_type value_type
;
766 detail::CheckMatLinear3D(m
);
769 m
.basis_element(1,2) - m
.basis_element(2,1),
770 m
.basis_element(2,0) - m
.basis_element(0,2),
771 m
.basis_element(0,1) - m
.basis_element(1,0)
773 value_type l
= length(axis
);
774 value_type tmo
= trace_3x3(m
) - value_type(1);
778 angle
= std::atan2(l
, tmo
); // l=2sin(theta),tmo=2cos(theta)
779 } else if (tmo
> value_type(0)) {
781 angle
= value_type(0);
783 size_t largest_diagonal_element
=
785 m
.basis_element(0,0),
786 m
.basis_element(1,1),
790 cyclic_permutation(largest_diagonal_element
, i
, j
, k
);
793 m
.basis_element(i
,i
) -
794 m
.basis_element(j
,j
) -
795 m
.basis_element(k
,k
) +
798 value_type s
= value_type(.5) / axis
[i
];
799 axis
[j
] = m
.basis_element(i
,j
) * s
;
800 axis
[k
] = m
.basis_element(i
,k
) * s
;
801 angle
= constants
<value_type
>::pi();
805 /** Convert a 3D rotation matrix to an Euler-angle triple */
806 template < class MatT
, typename Real
>
807 void matrix_to_euler(
813 Real tolerance
= epsilon
<Real
>::placeholder())
815 typedef MatT matrix_type
;
816 typedef typename
matrix_type::value_type value_type
;
819 detail::CheckMatLinear3D(m
);
823 detail::unpack_euler_order(order
, i
, j
, k
, odd
, repeat
);
826 value_type s1
= length(m
.basis_element(j
,i
),m
.basis_element(k
,i
));
827 value_type c1
= m
.basis_element(i
,i
);
829 angle_1
= std::atan2(s1
, c1
);
830 if (s1
> tolerance
) {
831 angle_0
= std::atan2(m
.basis_element(j
,i
),m
.basis_element(k
,i
));
832 angle_2
= std::atan2(m
.basis_element(i
,j
),-m
.basis_element(i
,k
));
834 angle_0
= value_type(0);
836 std::atan2(-m
.basis_element(k
,j
),m
.basis_element(j
,j
));
839 value_type s1
= -m
.basis_element(i
,k
);
840 value_type c1
= length(m
.basis_element(i
,i
),m
.basis_element(i
,j
));
842 angle_1
= std::atan2(s1
, c1
);
843 if (c1
> tolerance
) {
844 angle_0
= std::atan2(m
.basis_element(j
,k
),m
.basis_element(k
,k
));
845 angle_2
= std::atan2(m
.basis_element(i
,j
),m
.basis_element(i
,i
));
847 angle_0
= value_type(0);
848 angle_2
= -sign(s1
) *
849 std::atan2(-m
.basis_element(k
,j
),m
.basis_element(j
,j
));
860 /** Convert a 2D rotation matrix to a rotation angle */
861 template < class MatT
> typename
MatT::value_type
862 matrix_to_rotation_2D(const MatT
& m
)
865 detail::CheckMatLinear2D(m
);
867 return std::atan2(m
.basis_element(0,1),m
.basis_element(0,0));