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:
190 * e.g. euler_order_xyz means compute the column-basis rotation matrix
191 * equivalent to R_x * R_y * R_z, where R_i is the rotation matrix above
192 * axis i (the row-basis matrix would be R_z * R_y * R_x).
194 template < typename E
, class A
, class B
, class L
> void
195 matrix_rotation_euler(matrix
<E
,A
,B
,L
>& m
, E angle_0
, E angle_1
, E angle_2
,
198 typedef matrix
<E
,A
,B
,L
> matrix_type
;
199 typedef typename
matrix_type::value_type value_type
;
202 detail::CheckMatLinear3D(m
);
204 identity_transform(m
);
208 detail::unpack_euler_order(order
, i
, j
, k
, odd
, repeat
);
216 value_type s0
= std::sin(angle_0
);
217 value_type c0
= std::cos(angle_0
);
218 value_type s1
= std::sin(angle_1
);
219 value_type c1
= std::cos(angle_1
);
220 value_type s2
= std::sin(angle_2
);
221 value_type c2
= std::cos(angle_2
);
223 value_type s0s2
= s0
* s2
;
224 value_type s0c2
= s0
* c2
;
225 value_type c0s2
= c0
* s2
;
226 value_type c0c2
= c0
* c2
;
229 m
.set_basis_element(i
,i
, c1
);
230 m
.set_basis_element(i
,j
, s1
* s2
);
231 m
.set_basis_element(i
,k
,-s1
* c2
);
232 m
.set_basis_element(j
,i
, s0
* s1
);
233 m
.set_basis_element(j
,j
,-c1
* s0s2
+ c0c2
);
234 m
.set_basis_element(j
,k
, c1
* s0c2
+ c0s2
);
235 m
.set_basis_element(k
,i
, c0
* s1
);
236 m
.set_basis_element(k
,j
,-c1
* c0s2
- s0c2
);
237 m
.set_basis_element(k
,k
, c1
* c0c2
- s0s2
);
239 m
.set_basis_element(i
,i
, c1
* c2
);
240 m
.set_basis_element(i
,j
, c1
* s2
);
241 m
.set_basis_element(i
,k
,-s1
);
242 m
.set_basis_element(j
,i
, s1
* s0c2
- c0s2
);
243 m
.set_basis_element(j
,j
, s1
* s0s2
+ c0c2
);
244 m
.set_basis_element(j
,k
, s0
* c1
);
245 m
.set_basis_element(k
,i
, s1
* c0c2
+ s0s2
);
246 m
.set_basis_element(k
,j
, s1
* c0s2
- s0c2
);
247 m
.set_basis_element(k
,k
, c0
* c1
);
251 /** Build a matrix of derivatives of Euler angles about the specified axis.
253 * The rotation derivatives are applied about the cardinal axes in the
254 * order specified by the 'order' argument, where 'order' is one of the
255 * following enumerants:
264 * e.g. euler_order_xyz means compute the column-basis rotation matrix
265 * equivalent to R_x * R_y * R_z, where R_i is the rotation matrix above
266 * axis i (the row-basis matrix would be R_z * R_y * R_x).
268 * The derivative is taken with respect to the specified 'axis', which is
269 * the position of the axis in the triple; e.g. if order = euler_order_xyz,
270 * then axis = 0 would mean take the derivative with respect to x. Note
271 * that repeated axes are not currently supported.
273 template < typename E
, class A
, class B
, class L
> void
274 matrix_rotation_euler_derivatives(
275 matrix
<E
,A
,B
,L
>& m
, int axis
, E angle_0
, E angle_1
, E angle_2
,
278 typedef matrix
<E
,A
,B
,L
> matrix_type
;
279 typedef typename
matrix_type::value_type value_type
;
282 detail::CheckMatLinear3D(m
);
284 identity_transform(m
);
288 detail::unpack_euler_order(order
, i
, j
, k
, odd
, repeat
);
289 if(repeat
) throw std::invalid_argument(
290 "matrix_rotation_euler_derivatives does not support repeated axes");
298 value_type s0
= std::sin(angle_0
);
299 value_type c0
= std::cos(angle_0
);
300 value_type s1
= std::sin(angle_1
);
301 value_type c1
= std::cos(angle_1
);
302 value_type s2
= std::sin(angle_2
);
303 value_type c2
= std::cos(angle_2
);
305 value_type s0s2
= s0
* s2
;
306 value_type s0c2
= s0
* c2
;
307 value_type c0s2
= c0
* s2
;
308 value_type c0c2
= c0
* c2
;
311 m
.set_basis_element(i
,i
, 0. );
312 m
.set_basis_element(i
,j
, 0. );
313 m
.set_basis_element(i
,k
, 0. );
314 m
.set_basis_element(j
,i
, s1
* c0
*c2
+ s0
*s2
);
315 m
.set_basis_element(j
,j
, s1
* c0
*s2
- s0
*c2
);
316 m
.set_basis_element(j
,k
, c0
* c1
);
317 m
.set_basis_element(k
,i
,-s1
* s0
*c2
+ c0
*s2
);
318 m
.set_basis_element(k
,j
,-s1
* s0
*s2
- c0
*c2
);
319 m
.set_basis_element(k
,k
,-s0
* c1
);
320 } else if(axis
== 1) {
321 m
.set_basis_element(i
,i
,-s1
* c2
);
322 m
.set_basis_element(i
,j
,-s1
* s2
);
323 m
.set_basis_element(i
,k
,-c1
);
324 m
.set_basis_element(j
,i
, c1
* s0
*c2
);
325 m
.set_basis_element(j
,j
, c1
* s0
*s2
);
326 m
.set_basis_element(j
,k
,-s0
* s1
);
327 m
.set_basis_element(k
,i
, c1
* c0
*c2
);
328 m
.set_basis_element(k
,j
, c1
* c0
*s2
);
329 m
.set_basis_element(k
,k
,-c0
* s1
);
330 } else if(axis
== 2) {
331 m
.set_basis_element(i
,i
,-c1
* s2
);
332 m
.set_basis_element(i
,j
, c1
* c2
);
333 m
.set_basis_element(i
,k
, 0. );
334 m
.set_basis_element(j
,i
,-s1
* s0
*s2
- c0
*c2
);
335 m
.set_basis_element(j
,j
, s1
* s0
*c2
- c0
*s2
);
336 m
.set_basis_element(j
,k
, 0. );
337 m
.set_basis_element(k
,i
,-s1
* c0
*s2
+ s0
*c2
);
338 m
.set_basis_element(k
,j
, s1
* c0
*c2
+ s0
*s2
);
339 m
.set_basis_element(k
,k
, 0. );
343 //////////////////////////////////////////////////////////////////////////////
344 // 3D rotation to align with a vector, multiple vectors, or the view plane
345 //////////////////////////////////////////////////////////////////////////////
347 /** See vector_ortho.h for details */
348 template < typename E
,class A
,class B
,class L
,class VecT_1
,class VecT_2
> void
349 matrix_rotation_align(
352 const VecT_2
& reference
,
353 bool normalize
= true,
354 AxisOrder order
= axis_order_zyx
)
356 typedef vector
< E
,fixed
<3> > vector_type
;
358 identity_transform(m
);
362 orthonormal_basis(align
, reference
, x
, y
, z
, normalize
, order
);
363 matrix_set_basis_vectors(m
, x
, y
, z
);
366 /** See vector_ortho.h for details */
367 template < typename E
, class A
, class B
, class L
, class VecT
> void
368 matrix_rotation_align(matrix
<E
,A
,B
,L
>& m
, const VecT
& align
,
369 bool normalize
= true, AxisOrder order
= axis_order_zyx
)
371 typedef vector
< E
,fixed
<3> > vector_type
;
373 identity_transform(m
);
377 orthonormal_basis(align
, x
, y
, z
, normalize
, order
);
378 matrix_set_basis_vectors(m
, x
, y
, z
);
381 /** See vector_ortho.h for details */
382 template < typename E
,class A
,class B
,class L
,class VecT_1
,class VecT_2
> void
383 matrix_rotation_align_axial(matrix
<E
,A
,B
,L
>& m
, const VecT_1
& align
,
384 const VecT_2
& axis
, bool normalize
= true,
385 AxisOrder order
= axis_order_zyx
)
387 typedef vector
< E
,fixed
<3> > vector_type
;
389 identity_transform(m
);
393 orthonormal_basis_axial(align
, axis
, x
, y
, z
, normalize
, order
);
394 matrix_set_basis_vectors(m
, x
, y
, z
);
397 /** See vector_ortho.h for details */
398 template < typename E
, class A
, class B
, class L
, class MatT
> void
399 matrix_rotation_align_viewplane(
401 const MatT
& view_matrix
,
402 Handedness handedness
,
403 AxisOrder order
= axis_order_zyx
)
405 typedef vector
< E
, fixed
<3> > vector_type
;
407 identity_transform(m
);
411 orthonormal_basis_viewplane(view_matrix
, x
, y
, z
, handedness
, order
);
412 matrix_set_basis_vectors(m
, x
, y
, z
);
415 /** See vector_ortho.h for details */
416 template < typename E
, class A
, class B
, class L
, class MatT
> void
417 matrix_rotation_align_viewplane_LH(
419 const MatT
& view_matrix
,
420 AxisOrder order
= axis_order_zyx
)
422 matrix_rotation_align_viewplane(
423 m
,view_matrix
,left_handed
,order
);
426 /** See vector_ortho.h for details */
427 template < typename E
, class A
, class B
, class L
, class MatT
> void
428 matrix_rotation_align_viewplane_RH(
430 const MatT
& view_matrix
,
431 AxisOrder order
= axis_order_zyx
)
433 matrix_rotation_align_viewplane(
434 m
,view_matrix
,right_handed
,order
);
437 //////////////////////////////////////////////////////////////////////////////
438 // 3D rotation to aim at a target
439 //////////////////////////////////////////////////////////////////////////////
441 /** See vector_ortho.h for details */
442 template < typename E
, class A
, class B
, class L
,
443 class VecT_1
, class VecT_2
, class VecT_3
> void
444 matrix_rotation_aim_at(
447 const VecT_2
& target
,
448 const VecT_3
& reference
,
449 AxisOrder order
= axis_order_zyx
)
451 matrix_rotation_align(m
, target
- pos
, reference
, true, order
);
454 /** See vector_ortho.h for details */
455 template < typename E
, class A
, class B
, class L
,
456 class VecT_1
, class VecT_2
> void
457 matrix_rotation_aim_at(
460 const VecT_2
& target
,
461 AxisOrder order
= axis_order_zyx
)
463 matrix_rotation_align(m
, target
- pos
, true, order
);
466 /** See vector_ortho.h for details */
467 template < typename E
, class A
, class B
, class L
,
468 class VecT_1
, class VecT_2
, class VecT_3
> void
469 matrix_rotation_aim_at_axial(
472 const VecT_2
& target
,
474 AxisOrder order
= axis_order_zyx
)
476 matrix_rotation_align_axial(m
, target
- pos
, axis
, true, order
);
479 //////////////////////////////////////////////////////////////////////////////
481 //////////////////////////////////////////////////////////////////////////////
483 /** Build a matrix representing a 2D rotation */
484 template < typename E
, class A
, class B
, class L
> void
485 matrix_rotation_2D( matrix
<E
,A
,B
,L
>& m
, E angle
)
487 typedef matrix
<E
,A
,B
,L
> matrix_type
;
488 typedef typename
matrix_type::value_type value_type
;
491 detail::CheckMatLinear2D(m
);
493 value_type s
= value_type(std::sin(angle
));
494 value_type c
= value_type(std::cos(angle
));
496 identity_transform(m
);
498 m
.set_basis_element(0,0, c
);
499 m
.set_basis_element(0,1, s
);
500 m
.set_basis_element(1,0,-s
);
501 m
.set_basis_element(1,1, c
);
504 //////////////////////////////////////////////////////////////////////////////
505 // 2D rotation to align with a vector
506 //////////////////////////////////////////////////////////////////////////////
508 /** See vector_ortho.h for details */
509 template < typename E
, class A
, class B
, class L
, class VecT
> void
510 matrix_rotation_align_2D(matrix
<E
,A
,B
,L
>& m
, const VecT
& align
,
511 bool normalize
= true, AxisOrder2D order
= axis_order_xy
)
513 typedef vector
< E
, fixed
<2> > vector_type
;
515 identity_transform(m
);
519 orthonormal_basis_2D(align
, x
, y
, normalize
, order
);
520 matrix_set_basis_vectors_2D(m
, x
, y
);
523 //////////////////////////////////////////////////////////////////////////////
524 // 3D relative rotation about world axes
525 //////////////////////////////////////////////////////////////////////////////
527 /** Rotate a rotation matrix about the given world axis */
528 template < typename E
, class A
, class B
, class L
> void
529 matrix_rotate_about_world_axis(matrix
<E
,A
,B
,L
>& m
, size_t axis
, E angle
)
531 typedef matrix
<E
,A
,B
,L
> matrix_type
;
532 typedef typename
matrix_type::value_type value_type
;
535 detail::CheckMatLinear3D(m
);
536 detail::CheckIndex3(axis
);
539 cyclic_permutation(axis
, i
, j
, k
);
541 value_type s
= value_type(std::sin(angle
));
542 value_type c
= value_type(std::cos(angle
));
544 value_type ij
= c
* m
.basis_element(i
,j
) - s
* m
.basis_element(i
,k
);
545 value_type jj
= c
* m
.basis_element(j
,j
) - s
* m
.basis_element(j
,k
);
546 value_type kj
= c
* m
.basis_element(k
,j
) - s
* m
.basis_element(k
,k
);
548 m
.set_basis_element(i
,k
, s
*m
.basis_element(i
,j
) + c
*m
.basis_element(i
,k
));
549 m
.set_basis_element(j
,k
, s
*m
.basis_element(j
,j
) + c
*m
.basis_element(j
,k
));
550 m
.set_basis_element(k
,k
, s
*m
.basis_element(k
,j
) + c
*m
.basis_element(k
,k
));
552 m
.set_basis_element(i
,j
,ij
);
553 m
.set_basis_element(j
,j
,jj
);
554 m
.set_basis_element(k
,j
,kj
);
557 /** Rotate a rotation matrix about the world x axis */
558 template < typename E
, class A
, class B
, class L
> void
559 matrix_rotate_about_world_x(matrix
<E
,A
,B
,L
>& m
, E angle
) {
560 matrix_rotate_about_world_axis(m
,0,angle
);
563 /** Rotate a rotation matrix about the world y axis */
564 template < typename E
, class A
, class B
, class L
> void
565 matrix_rotate_about_world_y(matrix
<E
,A
,B
,L
>& m
, E angle
) {
566 matrix_rotate_about_world_axis(m
,1,angle
);
569 /** Rotate a rotation matrix about the world z axis */
570 template < typename E
, class A
, class B
, class L
> void
571 matrix_rotate_about_world_z(matrix
<E
,A
,B
,L
>& m
, E angle
) {
572 matrix_rotate_about_world_axis(m
,2,angle
);
575 //////////////////////////////////////////////////////////////////////////////
576 // 3D relative rotation about local axes
577 //////////////////////////////////////////////////////////////////////////////
579 /** Rotate a rotation matrix about the given local axis */
580 template < typename E
, class A
, class B
, class L
> void
581 matrix_rotate_about_local_axis(matrix
<E
,A
,B
,L
>& m
, size_t axis
, E angle
)
583 typedef matrix
<E
,A
,B
,L
> matrix_type
;
584 typedef typename
matrix_type::value_type value_type
;
587 detail::CheckMatLinear3D(m
);
588 detail::CheckIndex3(axis
);
591 cyclic_permutation(axis
, i
, j
, k
);
593 value_type s
= value_type(std::sin(angle
));
594 value_type c
= value_type(std::cos(angle
));
596 value_type j0
= c
* m
.basis_element(j
,0) + s
* m
.basis_element(k
,0);
597 value_type j1
= c
* m
.basis_element(j
,1) + s
* m
.basis_element(k
,1);
598 value_type j2
= c
* m
.basis_element(j
,2) + s
* m
.basis_element(k
,2);
600 m
.set_basis_element(k
,0, c
*m
.basis_element(k
,0) - s
*m
.basis_element(j
,0));
601 m
.set_basis_element(k
,1, c
*m
.basis_element(k
,1) - s
*m
.basis_element(j
,1));
602 m
.set_basis_element(k
,2, c
*m
.basis_element(k
,2) - s
*m
.basis_element(j
,2));
604 m
.set_basis_element(j
,0,j0
);
605 m
.set_basis_element(j
,1,j1
);
606 m
.set_basis_element(j
,2,j2
);
609 /** Rotate a rotation matrix about its local x axis */
610 template < typename E
, class A
, class B
, class L
> void
611 matrix_rotate_about_local_x(matrix
<E
,A
,B
,L
>& m
, E angle
) {
612 matrix_rotate_about_local_axis(m
,0,angle
);
615 /** Rotate a rotation matrix about its local y axis */
616 template < typename E
, class A
, class B
, class L
> void
617 matrix_rotate_about_local_y(matrix
<E
,A
,B
,L
>& m
, E angle
) {
618 matrix_rotate_about_local_axis(m
,1,angle
);
621 /** Rotate a rotation matrix about its local z axis */
622 template < typename E
, class A
, class B
, class L
> void
623 matrix_rotate_about_local_z(matrix
<E
,A
,B
,L
>& m
, E angle
) {
624 matrix_rotate_about_local_axis(m
,2,angle
);
627 //////////////////////////////////////////////////////////////////////////////
628 // 2D relative rotation
629 //////////////////////////////////////////////////////////////////////////////
631 template < typename E
, class A
, class B
, class L
> void
632 matrix_rotate_2D(matrix
<E
,A
,B
,L
>& m
, E angle
)
634 typedef matrix
<E
,A
,B
,L
> matrix_type
;
635 typedef typename
matrix_type::value_type value_type
;
638 detail::CheckMatLinear2D(m
);
640 value_type s
= value_type(std::sin(angle
));
641 value_type c
= value_type(std::cos(angle
));
643 value_type m00
= c
* m
.basis_element(0,0) - s
* m
.basis_element(0,1);
644 value_type m10
= c
* m
.basis_element(1,0) - s
* m
.basis_element(1,1);
646 m
.set_basis_element(0,1, s
*m
.basis_element(0,0) + c
*m
.basis_element(0,1));
647 m
.set_basis_element(1,1, s
*m
.basis_element(1,0) + c
*m
.basis_element(1,1));
649 m
.set_basis_element(0,0,m00
);
650 m
.set_basis_element(1,0,m10
);
653 //////////////////////////////////////////////////////////////////////////////
654 // Rotation from vector to vector
655 //////////////////////////////////////////////////////////////////////////////
657 /** Build a rotation matrix to rotate from one vector to another
659 * Note: The quaternion algorithm is more stable than the matrix algorithm, so
660 * we simply pass off to the quaternion function here.
662 template < class E
,class A
,class B
,class L
,class VecT_1
,class VecT_2
> void
663 matrix_rotation_vec_to_vec(
667 bool unit_length_vectors
= false)
669 typedef quaternion
< E
,fixed
<>,vector_first
,positive_cross
>
673 quaternion_rotation_vec_to_vec(q
,v1
,v2
,unit_length_vectors
);
674 matrix_rotation_quaternion(m
,q
);
677 //////////////////////////////////////////////////////////////////////////////
678 // Scale the angle of a rotation matrix
679 //////////////////////////////////////////////////////////////////////////////
681 /** Scale the angle of a 3D rotation matrix */
682 template < typename E
, class A
, class B
, class L
> void
683 matrix_scale_rotation_angle(matrix
<E
,A
,B
,L
>& m
, E t
,
684 E tolerance
= epsilon
<E
>::placeholder())
686 typedef vector
< E
,fixed
<3> > vector_type
;
687 typedef typename
vector_type::value_type value_type
;
691 matrix_to_axis_angle(m
, axis
, angle
, tolerance
);
692 matrix_rotation_axis_angle(m
, axis
, angle
* t
);
695 /** Scale the angle of a 2D rotation matrix */
696 template < typename E
, class A
, class B
, class L
> void
697 matrix_scale_rotation_angle_2D(
698 matrix
<E
,A
,B
,L
>& m
, E t
, E tolerance
= epsilon
<E
>::placeholder())
700 typedef vector
< E
,fixed
<2> > vector_type
;
701 typedef typename
vector_type::value_type value_type
;
703 value_type angle
= matrix_to_rotation_2D(m
);
704 matrix_rotation_2D(m
, angle
* t
);
707 //////////////////////////////////////////////////////////////////////////////
708 // Support functions for uniform handling of row- and column-basis matrices
709 //////////////////////////////////////////////////////////////////////////////
711 /* Note: The matrix rotation slerp, difference and concatenation functions do
712 * not use et::MatrixPromote<M1,M2>::temporary_type as the return type, even
713 * though that is the return type of the underlying matrix multiplication.
714 * This is because the sizes of these matrices are known at compile time (3x3
715 * and 2x2), and using fixed<> obviates the need for resizing of intermediate
718 * Also, no size- or type-checking is done on the arguments to these
719 * functions, as any such errors will be caught by the matrix multiplication
720 * and assignment to the 3x3 temporary.
723 /** A fixed-size temporary 3x3 matrix */
724 #define MAT_TEMP_3X3 matrix< \
725 typename et::ScalarPromote< \
726 typename MatT_1::value_type, \
727 typename MatT_2::value_type \
730 typename MatT_1::basis_orient, \
734 /** A fixed-size temporary 2x2 matrix */
735 #define MAT_TEMP_2X2 matrix< \
736 typename et::ScalarPromote< \
737 typename MatT_1::value_type, \
738 typename MatT_2::value_type \
741 typename MatT_1::basis_orient, \
747 /** Concatenate two 3D row-basis rotation matrices in the order m1->m2 */
748 template < class MatT_1
, class MatT_2
> MAT_TEMP_3X3
749 matrix_concat_rotations(const MatT_1
& m1
, const MatT_2
& m2
, row_basis
) {
753 /** Concatenate two 3D col-basis rotation matrices in the order m1->m2 */
754 template < class MatT_1
, class MatT_2
> MAT_TEMP_3X3
755 matrix_concat_rotations(const MatT_1
& m1
, const MatT_2
& m2
, col_basis
) {
759 /** Concatenate two 3D rotation matrices in the order m1->m2 */
760 template < class MatT_1
, class MatT_2
> MAT_TEMP_3X3
761 matrix_concat_rotations(const MatT_1
& m1
, const MatT_2
& m2
) {
762 return matrix_concat_rotations(m1
,m2
,typename
MatT_1::basis_orient());
765 /** Concatenate two 2D row-basis rotation matrices in the order m1->m2 */
766 template < class MatT_1
, class MatT_2
> MAT_TEMP_2X2
767 matrix_concat_rotations_2D(const MatT_1
& m1
, const MatT_2
& m2
, row_basis
) {
771 /** Concatenate two 2D col-basis rotation matrices in the order m1->m2 */
772 template < class MatT_1
, class MatT_2
> MAT_TEMP_2X2
773 matrix_concat_rotations_2D(const MatT_1
& m1
, const MatT_2
& m2
, col_basis
) {
777 /** Concatenate two 2D rotation matrices in the order m1->m2 */
778 template < class MatT_1
, class MatT_2
> MAT_TEMP_2X2
779 matrix_concat_rotations_2D(const MatT_1
& m1
, const MatT_2
& m2
) {
780 return matrix_concat_rotations_2D(m1
,m2
,typename
MatT_1::basis_orient());
783 } // namespace detail
785 //////////////////////////////////////////////////////////////////////////////
786 // Matrix rotation difference
787 //////////////////////////////////////////////////////////////////////////////
789 /** Return the rotational 'difference' between two 3D rotation matrices */
790 template < class MatT_1
, class MatT_2
> MAT_TEMP_3X3
791 matrix_rotation_difference(const MatT_1
& m1
, const MatT_2
& m2
) {
792 return detail::matrix_concat_rotations(transpose(m1
),m2
);
795 /** Return the rotational 'difference' between two 2D rotation matrices */
796 template < class MatT_1
, class MatT_2
> MAT_TEMP_2X2
797 matrix_rotation_difference_2D(const MatT_1
& m1
, const MatT_2
& m2
) {
798 return detail::matrix_concat_rotations_2D(transpose(m1
),m2
);
801 //////////////////////////////////////////////////////////////////////////////
802 // Spherical linear interpolation of rotation matrices
803 //////////////////////////////////////////////////////////////////////////////
805 /* @todo: It might be as fast or faster to simply convert the matrices to
806 * quaternions, interpolate, and convert back.
808 * @todo: The behavior of matrix slerp is currently a little different than
809 * for quaternions: in the matrix function, when the two matrices are close
810 * to identical the first is returned, while in the quaternion function the
811 * quaternions are nlerp()'d in this case.
813 * I still need to do the equivalent of nlerp() for matrices, in which case
814 * these functions could be revised to pass off to nlerp() when the matrices
815 * are nearly aligned.
818 /** Spherical linear interpolation of two 3D rotation matrices */
819 template < class MatT_1
, class MatT_2
, typename E
> MAT_TEMP_3X3
820 matrix_slerp(const MatT_1
& m1
, const MatT_2
& m2
, E t
,
821 E tolerance
= epsilon
<E
>::placeholder())
823 typedef MAT_TEMP_3X3 temporary_type
;
825 temporary_type m
= matrix_rotation_difference(m1
,m2
);
826 matrix_scale_rotation_angle(m
,t
,tolerance
);
827 return detail::matrix_concat_rotations(m1
,m
);
830 /** Spherical linear interpolation of two 2D rotation matrices */
831 template < class MatT_1
, class MatT_2
, typename E
> MAT_TEMP_2X2
832 matrix_slerp_2D(const MatT_1
& m1
, const MatT_2
& m2
, E t
,
833 E tolerance
= epsilon
<E
>::placeholder())
835 typedef MAT_TEMP_2X2 temporary_type
;
837 temporary_type m
= matrix_rotation_difference_2D(m1
,m2
);
838 matrix_scale_rotation_angle_2D(m
,t
,tolerance
);
839 return detail::matrix_concat_rotations_2D(m1
,m
);
845 //////////////////////////////////////////////////////////////////////////////
847 //////////////////////////////////////////////////////////////////////////////
849 /** Convert a 3D rotation matrix to an axis-angle pair */
850 template < class MatT
, typename E
, class A
> void
851 matrix_to_axis_angle(
855 E tolerance
= epsilon
<E
>::placeholder())
857 typedef MatT matrix_type
;
858 typedef typename
matrix_type::value_type value_type
;
861 detail::CheckMatLinear3D(m
);
864 m
.basis_element(1,2) - m
.basis_element(2,1),
865 m
.basis_element(2,0) - m
.basis_element(0,2),
866 m
.basis_element(0,1) - m
.basis_element(1,0)
868 value_type l
= length(axis
);
869 value_type tmo
= trace_3x3(m
) - value_type(1);
873 angle
= std::atan2(l
, tmo
); // l=2sin(theta),tmo=2cos(theta)
874 } else if (tmo
> value_type(0)) {
876 angle
= value_type(0);
878 size_t largest_diagonal_element
=
880 m
.basis_element(0,0),
881 m
.basis_element(1,1),
885 cyclic_permutation(largest_diagonal_element
, i
, j
, k
);
888 m
.basis_element(i
,i
) -
889 m
.basis_element(j
,j
) -
890 m
.basis_element(k
,k
) +
893 value_type s
= value_type(.5) / axis
[i
];
894 axis
[j
] = m
.basis_element(i
,j
) * s
;
895 axis
[k
] = m
.basis_element(i
,k
) * s
;
896 angle
= constants
<value_type
>::pi();
900 /** Convert a 3D rotation matrix to an Euler-angle triple */
901 template < class MatT
, typename Real
>
902 void matrix_to_euler(
908 Real tolerance
= epsilon
<Real
>::placeholder())
910 typedef MatT matrix_type
;
911 typedef typename
matrix_type::value_type value_type
;
914 detail::CheckMatLinear3D(m
);
918 detail::unpack_euler_order(order
, i
, j
, k
, odd
, repeat
);
921 value_type s1
= length(m
.basis_element(j
,i
),m
.basis_element(k
,i
));
922 value_type c1
= m
.basis_element(i
,i
);
924 angle_1
= std::atan2(s1
, c1
);
925 if (s1
> tolerance
) {
926 angle_0
= std::atan2(m
.basis_element(j
,i
),m
.basis_element(k
,i
));
927 angle_2
= std::atan2(m
.basis_element(i
,j
),-m
.basis_element(i
,k
));
929 angle_0
= value_type(0);
931 std::atan2(-m
.basis_element(k
,j
),m
.basis_element(j
,j
));
934 value_type s1
= -m
.basis_element(i
,k
);
935 value_type c1
= length(m
.basis_element(i
,i
),m
.basis_element(i
,j
));
937 angle_1
= std::atan2(s1
, c1
);
938 if (c1
> tolerance
) {
939 angle_0
= std::atan2(m
.basis_element(j
,k
),m
.basis_element(k
,k
));
940 angle_2
= std::atan2(m
.basis_element(i
,j
),m
.basis_element(i
,i
));
942 angle_0
= value_type(0);
943 angle_2
= -sign(s1
) *
944 std::atan2(-m
.basis_element(k
,j
),m
.basis_element(j
,j
));
955 /** Convert a 2D rotation matrix to a rotation angle */
956 template < class MatT
> typename
MatT::value_type
957 matrix_to_rotation_2D(const MatT
& m
)
960 detail::CheckMatLinear2D(m
);
962 return std::atan2(m
.basis_element(0,1),m
.basis_element(0,0));