]>
Dogcows Code - chaz/yoink/blob - src/cml/mathlib/quaternion_rotation.h
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 quaternion_rotation_h
14 #define quaternion_rotation_h
16 #include <cml/mathlib/checking.h>
18 /* Functions related to quaternion rotations.
20 * Note: A number of these functions simply wrap calls to the corresponding
21 * matrix functions. Some of them (the 'aim-at' and 'align' functions in
22 * particular) might be considered a bit superfluous, since the resulting
23 * quaternion will most likely be converted to a matrix at some point anyway.
24 * However, they're included here for completeness, and for convenience in
25 * cases where a quaternion is being used as the primary rotation
31 //////////////////////////////////////////////////////////////////////////////
32 // Rotation about world axes
33 //////////////////////////////////////////////////////////////////////////////
35 /** Build a quaternion representing a rotation about the given world axis */
36 template < class E
, class A
, class O
, class C
> void
37 quaternion_rotation_world_axis(quaternion
<E
,A
,O
,C
>& q
, size_t axis
, E angle
)
39 typedef quaternion
<E
,A
,O
,C
> quaternion_type
;
40 typedef typename
quaternion_type::value_type value_type
;
41 typedef typename
quaternion_type::order_type order_type
;
44 detail::CheckIndex3(axis
);
48 const size_t W
= order_type::W
;
49 const size_t I
= order_type::X
+ axis
;
51 angle
*= value_type(.5);
52 q
[I
] = std::sin(angle
);
53 q
[W
] = std::cos(angle
);
56 /** Build a quaternion representing a rotation about the world x axis */
57 template < class E
, class A
, class O
, class C
> void
58 quaternion_rotation_world_x(quaternion
<E
,A
,O
,C
>& q
, E angle
) {
59 quaternion_rotation_world_axis(q
,0,angle
);
62 /** Build a quaternion representing a rotation about the world y axis */
63 template < class E
, class A
, class O
, class C
> void
64 quaternion_rotation_world_y(quaternion
<E
,A
,O
,C
>& q
, E angle
) {
65 quaternion_rotation_world_axis(q
,1,angle
);
68 /** Build a quaternion representing a rotation about the world z axis */
69 template < class E
, class A
, class O
, class C
> void
70 quaternion_rotation_world_z(quaternion
<E
,A
,O
,C
>& q
, E angle
) {
71 quaternion_rotation_world_axis(q
,2,angle
);
74 //////////////////////////////////////////////////////////////////////////////
75 // Rotation from an axis-angle pair
76 //////////////////////////////////////////////////////////////////////////////
78 /** Build a quaternion from an axis-angle pair */
79 template < class E
, class A
, class O
, class C
, class VecT
> void
80 quaternion_rotation_axis_angle(
81 quaternion
<E
,A
,O
,C
>& q
, const VecT
& axis
, E angle
)
83 typedef quaternion
<E
,A
,O
,C
> quaternion_type
;
84 typedef typename
quaternion_type::value_type value_type
;
85 typedef typename
quaternion_type::order_type order_type
;
88 detail::CheckVec3(axis
);
97 angle
*= value_type(.5);
99 /* @todo: If and when we have a set() function that takes a vector and a
100 * scalar, this can be written as:
102 * q.set(std::cos(angle), axis * std::sin(angle));
104 * In which case the enum will also not be necessary.
107 q
[W
] = std::cos(angle
);
108 value_type s
= std::sin(angle
);
114 //////////////////////////////////////////////////////////////////////////////
115 // Rotation from a matrix
116 //////////////////////////////////////////////////////////////////////////////
118 /** Build a quaternion from a rotation matrix */
119 template < class E
, class A
, class O
, class C
, class MatT
> void
120 quaternion_rotation_matrix(quaternion
<E
,A
,O
,C
>& q
, const MatT
& m
)
122 typedef quaternion
<E
,A
,O
,C
> quaternion_type
;
123 typedef typename
quaternion_type::value_type value_type
;
124 typedef typename
quaternion_type::order_type order_type
;
127 detail::CheckMatLinear3D(m
);
136 value_type tr
= trace_3x3(m
);
137 if (tr
>= value_type(0)) {
138 q
[W
] = std::sqrt(tr
+ value_type(1)) * value_type(.5);
139 value_type s
= value_type(.25) / q
[W
];
140 q
[X
] = (m
.basis_element(1,2) - m
.basis_element(2,1)) * s
;
141 q
[Y
] = (m
.basis_element(2,0) - m
.basis_element(0,2)) * s
;
142 q
[Z
] = (m
.basis_element(0,1) - m
.basis_element(1,0)) * s
;
144 size_t largest_diagonal_element
=
146 m
.basis_element(0,0),
147 m
.basis_element(1,1),
151 cyclic_permutation(largest_diagonal_element
, i
, j
, k
);
152 const size_t I
= X
+ i
;
153 const size_t J
= X
+ j
;
154 const size_t K
= X
+ k
;
157 m
.basis_element(i
,i
) -
158 m
.basis_element(j
,j
) -
159 m
.basis_element(k
,k
) +
162 value_type s
= value_type(.25) / q
[I
];
163 q
[J
] = (m
.basis_element(i
,j
) + m
.basis_element(j
,i
)) * s
;
164 q
[K
] = (m
.basis_element(i
,k
) + m
.basis_element(k
,i
)) * s
;
165 q
[W
] = (m
.basis_element(j
,k
) - m
.basis_element(k
,j
)) * s
;
169 //////////////////////////////////////////////////////////////////////////////
170 // Rotation from Euler angles
171 //////////////////////////////////////////////////////////////////////////////
173 /** Build a quaternion from an Euler-angle triple */
174 template < class E
, class A
, class O
, class C
> void
175 quaternion_rotation_euler(
176 quaternion
<E
,A
,O
,C
>& q
, E angle_0
, E angle_1
, E angle_2
,
179 typedef quaternion
<E
,A
,O
,C
> quaternion_type
;
180 typedef typename
quaternion_type::value_type value_type
;
181 typedef typename
quaternion_type::order_type order_type
;
185 detail::unpack_euler_order(order
, i
, j
, k
, odd
, repeat
);
187 const size_t W
= order_type::W
;
188 const size_t I
= order_type::X
+ i
;
189 const size_t J
= order_type::X
+ j
;
190 const size_t K
= order_type::X
+ k
;
196 angle_0
*= value_type(.5);
197 angle_1
*= value_type(.5);
198 angle_2
*= value_type(.5);
200 value_type s0
= std::sin(angle_0
);
201 value_type c0
= std::cos(angle_0
);
202 value_type s1
= std::sin(angle_1
);
203 value_type c1
= std::cos(angle_1
);
204 value_type s2
= std::sin(angle_2
);
205 value_type c2
= std::cos(angle_2
);
207 value_type s0s2
= s0
* s2
;
208 value_type s0c2
= s0
* c2
;
209 value_type c0s2
= c0
* s2
;
210 value_type c0c2
= c0
* c2
;
213 q
[I
] = c1
* (c0s2
+ s0c2
);
214 q
[J
] = s1
* (c0c2
+ s0s2
);
215 q
[K
] = s1
* (c0s2
- s0c2
);
216 q
[W
] = c1
* (c0c2
- s0s2
);
218 q
[I
] = c1
* s0c2
- s1
* c0s2
;
219 q
[J
] = c1
* s0s2
+ s1
* c0c2
;
220 q
[K
] = c1
* c0s2
- s1
* s0c2
;
221 q
[W
] = c1
* c0c2
+ s1
* s0s2
;
228 //////////////////////////////////////////////////////////////////////////////
229 // Rotation to align with a vector, multiple vectors, or the view plane
230 //////////////////////////////////////////////////////////////////////////////
232 /** See vector_ortho.h for details */
233 template < typename E
,class A
,class O
,class C
,class VecT_1
,class VecT_2
> void
234 quaternion_rotation_align(
235 quaternion
<E
,A
,O
,C
>& q
,
237 const VecT_2
& reference
,
238 bool normalize
= true,
239 AxisOrder order
= axis_order_zyx
)
241 typedef matrix
< E
,fixed
<3,3>,row_basis
,row_major
> matrix_type
;
244 matrix_rotation_align(m
,align
,reference
,normalize
,order
);
245 quaternion_rotation_matrix(q
,m
);
248 /** See vector_ortho.h for details */
249 template < typename E
, class A
, class O
, class C
, class VecT
> void
250 quaternion_rotation_align(quaternion
<E
,A
,O
,C
>& q
, const VecT
& align
,
251 bool normalize
= true, AxisOrder order
= axis_order_zyx
)
253 typedef matrix
< E
,fixed
<3,3>,row_basis
,row_major
> matrix_type
;
256 matrix_rotation_align(m
,align
,normalize
,order
);
257 quaternion_rotation_matrix(q
,m
);
260 /** See vector_ortho.h for details */
261 template < typename E
,class A
,class O
,class C
,class VecT_1
,class VecT_2
> void
262 quaternion_rotation_align_axial(quaternion
<E
,A
,O
,C
>& q
, const VecT_1
& align
,
263 const VecT_2
& axis
, bool normalize
= true,
264 AxisOrder order
= axis_order_zyx
)
266 typedef matrix
< E
,fixed
<3,3>,row_basis
,row_major
> matrix_type
;
269 matrix_rotation_align_axial(m
,align
,axis
,normalize
,order
);
270 quaternion_rotation_matrix(q
,m
);
273 /** See vector_ortho.h for details */
274 template < typename E
, class A
, class O
, class C
, class MatT
> void
275 quaternion_rotation_align_viewplane(
276 quaternion
<E
,A
,O
,C
>& q
,
277 const MatT
& view_matrix
,
278 Handedness handedness
,
279 AxisOrder order
= axis_order_zyx
)
281 typedef matrix
< E
,fixed
<3,3>,row_basis
,row_major
> matrix_type
;
284 matrix_rotation_align_viewplane(m
,view_matrix
,handedness
,order
);
285 quaternion_rotation_matrix(q
,m
);
288 /** See vector_ortho.h for details */
289 template < typename E
, class A
, class O
, class C
, class MatT
> void
290 quaternion_rotation_align_viewplane_LH(
291 quaternion
<E
,A
,O
,C
>& q
,
292 const MatT
& view_matrix
,
293 AxisOrder order
= axis_order_zyx
)
295 typedef matrix
< E
,fixed
<3,3>,row_basis
,row_major
> matrix_type
;
298 matrix_rotation_align_viewplane_LH(m
,view_matrix
,order
);
299 quaternion_rotation_matrix(q
,m
);
302 /** See vector_ortho.h for details */
303 template < typename E
, class A
, class O
, class C
, class MatT
> void
304 quaternion_rotation_align_viewplane_RH(
305 quaternion
<E
,A
,O
,C
>& q
,
306 const MatT
& view_matrix
,
307 AxisOrder order
= axis_order_zyx
)
309 typedef matrix
< E
,fixed
<3,3>,row_basis
,row_major
> matrix_type
;
312 matrix_rotation_align_viewplane_RH(m
,view_matrix
,order
);
313 quaternion_rotation_matrix(q
,m
);
316 //////////////////////////////////////////////////////////////////////////////
317 // Rotation to aim at a target
318 //////////////////////////////////////////////////////////////////////////////
320 /** See vector_ortho.h for details */
321 template < typename E
, class A
, class O
, class C
,
322 class VecT_1
, class VecT_2
, class VecT_3
> void
323 quaternion_rotation_aim_at(
324 quaternion
<E
,A
,O
,C
>& q
,
326 const VecT_2
& target
,
327 const VecT_3
& reference
,
328 AxisOrder order
= axis_order_zyx
)
330 typedef matrix
< E
,fixed
<3,3>,row_basis
,row_major
> matrix_type
;
333 matrix_rotation_aim_at(m
,pos
,target
,reference
,order
);
334 quaternion_rotation_matrix(q
,m
);
337 /** See vector_ortho.h for details */
338 template < typename E
, class A
, class O
, class C
,
339 class VecT_1
, class VecT_2
> void
340 quaternion_rotation_aim_at(
341 quaternion
<E
,A
,O
,C
>& q
,
343 const VecT_2
& target
,
344 AxisOrder order
= axis_order_zyx
)
346 typedef matrix
< E
,fixed
<3,3>,row_basis
,row_major
> matrix_type
;
349 matrix_rotation_aim_at(m
,pos
,target
,order
);
350 quaternion_rotation_matrix(q
,m
);
353 /** See vector_ortho.h for details */
354 template < typename E
, class A
, class O
, class C
,
355 class VecT_1
, class VecT_2
, class VecT_3
> void
356 quaternion_rotation_aim_at_axial(
357 quaternion
<E
,A
,O
,C
>& q
,
359 const VecT_2
& target
,
361 AxisOrder order
= axis_order_zyx
)
363 typedef matrix
< E
,fixed
<3,3>,row_basis
,row_major
> matrix_type
;
366 matrix_rotation_aim_at_axial(m
,pos
,target
,axis
,order
);
367 quaternion_rotation_matrix(q
,m
);
370 //////////////////////////////////////////////////////////////////////////////
371 // Relative rotation about world axes
372 //////////////////////////////////////////////////////////////////////////////
374 /* Rotate a quaternion about the given world axis */
375 template < class E
, class A
, class O
, class C
> void
376 quaternion_rotate_about_world_axis(quaternion
<E
,A
,O
,C
>& q
,size_t axis
,E angle
)
378 typedef quaternion
<E
,A
,O
,C
> quaternion_type
;
379 typedef typename
quaternion_type::value_type value_type
;
380 typedef typename
quaternion_type::order_type order_type
;
383 detail::CheckIndex3(axis
);
386 cyclic_permutation(axis
, i
, j
, k
);
388 const size_t W
= order_type::W
;
389 const size_t I
= order_type::X
+ i
;
390 const size_t J
= order_type::X
+ j
;
391 const size_t K
= order_type::X
+ k
;
393 angle
*= value_type(.5);
394 value_type s
= value_type(std::sin(angle
));
395 value_type c
= value_type(std::cos(angle
));
397 quaternion_type result
;
398 result
[I
] = c
* q
[I
] + s
* q
[W
];
399 result
[J
] = c
* q
[J
] - s
* q
[K
];
400 result
[K
] = c
* q
[K
] + s
* q
[J
];
401 result
[W
] = c
* q
[W
] - s
* q
[I
];
405 /* Rotate a quaternion about the world x axis */
406 template < class E
, class A
, class O
, class C
> void
407 quaternion_rotate_about_world_x(quaternion
<E
,A
,O
,C
>& q
, E angle
) {
408 quaternion_rotate_about_world_axis(q
,0,angle
);
411 /* Rotate a quaternion about the world y axis */
412 template < class E
, class A
, class O
, class C
> void
413 quaternion_rotate_about_world_y(quaternion
<E
,A
,O
,C
>& q
, E angle
) {
414 quaternion_rotate_about_world_axis(q
,1,angle
);
417 /* Rotate a quaternion about the world z axis */
418 template < class E
, class A
, class O
, class C
> void
419 quaternion_rotate_about_world_z(quaternion
<E
,A
,O
,C
>& q
, E angle
) {
420 quaternion_rotate_about_world_axis(q
,2,angle
);
423 //////////////////////////////////////////////////////////////////////////////
424 // Relative rotation about local axes
425 //////////////////////////////////////////////////////////////////////////////
427 /* Rotate a quaternion about the given local axis */
428 template < class E
, class A
, class O
, class C
> void
429 quaternion_rotate_about_local_axis(quaternion
<E
,A
,O
,C
>& q
,size_t axis
,E angle
)
431 typedef quaternion
<E
,A
,O
,C
> quaternion_type
;
432 typedef typename
quaternion_type::value_type value_type
;
433 typedef typename
quaternion_type::order_type order_type
;
436 detail::CheckIndex3(axis
);
439 cyclic_permutation(axis
, i
, j
, k
);
441 const size_t W
= order_type::W
;
442 const size_t I
= order_type::X
+ i
;
443 const size_t J
= order_type::X
+ j
;
444 const size_t K
= order_type::X
+ k
;
446 angle
*= value_type(.5);
447 value_type s
= value_type(std::sin(angle
));
448 value_type c
= value_type(std::cos(angle
));
450 quaternion_type result
;
451 result
[I
] = c
* q
[I
] + s
* q
[W
];
452 result
[J
] = c
* q
[J
] + s
* q
[K
];
453 result
[K
] = c
* q
[K
] - s
* q
[J
];
454 result
[W
] = c
* q
[W
] - s
* q
[I
];
458 /* Rotate a quaternion about its local x axis */
459 template < class E
, class A
, class O
, class C
> void
460 quaternion_rotate_about_local_x(quaternion
<E
,A
,O
,C
>& q
, E angle
) {
461 quaternion_rotate_about_local_axis(q
,0,angle
);
464 /* Rotate a quaternion about its local y axis */
465 template < class E
, class A
, class O
, class C
> void
466 quaternion_rotate_about_local_y(quaternion
<E
,A
,O
,C
>& q
, E angle
) {
467 quaternion_rotate_about_local_axis(q
,1,angle
);
470 /* Rotate a quaternion about its local z axis */
471 template < class E
, class A
, class O
, class C
> void
472 quaternion_rotate_about_local_z(quaternion
<E
,A
,O
,C
>& q
, E angle
) {
473 quaternion_rotate_about_local_axis(q
,2,angle
);
476 //////////////////////////////////////////////////////////////////////////////
477 // Rotation from vector to vector
478 //////////////////////////////////////////////////////////////////////////////
480 /* http://www.martinb.com/maths/algebra/vectors/angleBetween/index.htm. */
482 /** Build a quaternion to rotate from one vector to another */
483 template < class E
,class A
,class O
,class C
,class VecT_1
,class VecT_2
> void
484 quaternion_rotation_vec_to_vec(
485 quaternion
<E
,A
,O
,C
>& q
,
488 bool unit_length_vectors
= false)
490 typedef quaternion
<E
,A
,O
,C
> quaternion_type
;
491 typedef typename
quaternion_type::value_type value_type
;
492 typedef vector
< value_type
, fixed
<3> > vector_type
;
494 /* Checking handled by cross() */
496 /* @todo: If at some point quaternion<> has a set() function that takes a
497 * vector and a scalar, this can then be written as:
500 * q.set(value_type(1)+dot(v1,v2), cross(v1,v2));
502 * q.set(std::sqrt(...)+dot(v1,v2), cross(v1,v2));
506 vector_type c
= cross(v1
,v2
);
507 if (unit_length_vectors
) {
508 q
= quaternion_type(value_type(1) + dot(v1
,v2
), c
.data());
511 std::sqrt(v1
.length_squared() * v2
.length_squared()) + dot(v1
,v2
),
518 //////////////////////////////////////////////////////////////////////////////
519 // Scale the angle of a rotation matrix
520 //////////////////////////////////////////////////////////////////////////////
522 template < typename E
, class A
, class O
, class C
> void
523 quaternion_scale_angle(quaternion
<E
,A
,O
,C
>& q
, E t
,
524 E tolerance
= epsilon
<E
>::placeholder())
526 typedef vector
< E
,fixed
<3> > vector_type
;
527 typedef typename
vector_type::value_type value_type
;
531 quaternion_to_axis_angle(q
, axis
, angle
, tolerance
);
532 quaternion_rotation_axis_angle(q
, axis
, angle
* t
);
535 //////////////////////////////////////////////////////////////////////////////
536 // Support functions for uniform handling of pos- and neg-cross quaternions
537 //////////////////////////////////////////////////////////////////////////////
541 /** Concatenate two quaternions in the order q1->q2 */
542 template < class QuatT_1
, class QuatT_2
>
543 typename
et::QuaternionPromote2
<QuatT_1
,QuatT_2
>::temporary_type
544 quaternion_rotation_difference(
545 const QuatT_1
& q1
, const QuatT_2
& q2
, positive_cross
)
547 return q2
* conjugate(q1
);
550 /** Concatenate two quaternions in the order q1->q2 */
551 template < class QuatT_1
, class QuatT_2
>
552 typename
et::QuaternionPromote2
<QuatT_1
,QuatT_2
>::temporary_type
553 quaternion_rotation_difference(
554 const QuatT_1
& q1
, const QuatT_2
& q2
, negative_cross
)
556 return conjugate(q1
) * q2
;
559 } // namespace detail
561 //////////////////////////////////////////////////////////////////////////////
562 // Quaternions rotation difference
563 //////////////////////////////////////////////////////////////////////////////
565 /** Return the rotational 'difference' between two quaternions */
566 template < class QuatT_1
, class QuatT_2
>
567 typename
et::QuaternionPromote2
<QuatT_1
,QuatT_2
>::temporary_type
568 quaternion_rotation_difference(const QuatT_1
& q1
, const QuatT_2
& q2
) {
569 return detail::quaternion_rotation_difference(
570 q1
, q2
, typename
QuatT_1::cross_type());
573 //////////////////////////////////////////////////////////////////////////////
575 //////////////////////////////////////////////////////////////////////////////
577 /** Convert a quaternion to an axis-angle pair */
578 template < class QuatT
, typename E
, class A
> void
579 quaternion_to_axis_angle(
583 E tolerance
= epsilon
<E
>::placeholder())
585 typedef QuatT quaternion_type
;
586 typedef typename
quaternion_type::value_type value_type
;
587 typedef typename
quaternion_type::order_type order_type
;
590 detail::CheckQuat(q
);
592 axis
= q
.imaginary();
593 value_type l
= length(axis
);
596 angle
= value_type(2) * std::atan2(l
,q
.real());
599 angle
= value_type(0);
603 /** Convert a quaternion to an Euler-angle triple
605 * Note: I've implemented direct quaternion-to-Euler conversion, but as far as
606 * I can tell it more or less reduces to converting the quaternion to a matrix
607 * as you go. The direct method is a little more efficient in that it doesn't
608 * require a temporary and only the necessary matrix elements need be
609 * computed. However, the implementation is complex and there's considerable
610 * opportunity for error, so from a development and debugging standpoint I
611 * think it's better to just perform the conversion via matrix_to_euler(),
612 * which is already known to be correct.
615 template < class QuatT
, typename Real
> void
622 Real tolerance
= epsilon
<Real
>::placeholder())
624 typedef QuatT quaternion_type
;
625 typedef typename
quaternion_type::value_type value_type
;
626 typedef matrix
< value_type
,fixed
<3,3>,row_basis
,row_major
> matrix_type
;
629 matrix_rotation_quaternion(m
, q
);
630 matrix_to_euler(m
, angle_0
, angle_1
, angle_2
, order
, tolerance
);
This page took 0.065926 seconds and 5 git commands to generate.