+++ /dev/null
-/* -*- C++ -*- ------------------------------------------------------------
-
-Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
-
-The Configurable Math Library (CML) is distributed under the terms of the
-Boost Software License, v1.0 (see cml/LICENSE for details).
-
- *-----------------------------------------------------------------------*/
-/** @file
- * @brief
- */
-
-#ifndef quaternion_rotation_h
-#define quaternion_rotation_h
-
-#include <cml/mathlib/checking.h>
-
-/* Functions related to quaternion rotations.
- *
- * Note: A number of these functions simply wrap calls to the corresponding
- * matrix functions. Some of them (the 'aim-at' and 'align' functions in
- * particular) might be considered a bit superfluous, since the resulting
- * quaternion will most likely be converted to a matrix at some point anyway.
- * However, they're included here for completeness, and for convenience in
- * cases where a quaternion is being used as the primary rotation
- * representation.
-*/
-
-namespace cml {
-
-//////////////////////////////////////////////////////////////////////////////
-// Rotation about world axes
-//////////////////////////////////////////////////////////////////////////////
-
-/** Build a quaternion representing a rotation about the given world axis */
-template < class E, class A, class O, class C > void
-quaternion_rotation_world_axis(quaternion<E,A,O,C>& q, size_t axis, E angle)
-{
- typedef quaternion<E,A,O,C> quaternion_type;
- typedef typename quaternion_type::value_type value_type;
- typedef typename quaternion_type::order_type order_type;
-
- /* Checking */
- detail::CheckIndex3(axis);
-
- q.identity();
-
- const size_t W = order_type::W;
- const size_t I = order_type::X + axis;
-
- angle *= value_type(.5);
- q[I] = std::sin(angle);
- q[W] = std::cos(angle);
-}
-
-/** Build a quaternion representing a rotation about the world x axis */
-template < class E, class A, class O, class C > void
-quaternion_rotation_world_x(quaternion<E,A,O,C>& q, E angle) {
- quaternion_rotation_world_axis(q,0,angle);
-}
-
-/** Build a quaternion representing a rotation about the world y axis */
-template < class E, class A, class O, class C > void
-quaternion_rotation_world_y(quaternion<E,A,O,C>& q, E angle) {
- quaternion_rotation_world_axis(q,1,angle);
-}
-
-/** Build a quaternion representing a rotation about the world z axis */
-template < class E, class A, class O, class C > void
-quaternion_rotation_world_z(quaternion<E,A,O,C>& q, E angle) {
- quaternion_rotation_world_axis(q,2,angle);
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Rotation from an axis-angle pair
-//////////////////////////////////////////////////////////////////////////////
-
-/** Build a quaternion from an axis-angle pair */
-template < class E, class A, class O, class C, class VecT > void
-quaternion_rotation_axis_angle(
- quaternion<E,A,O,C>& q, const VecT& axis, E angle)
-{
- typedef quaternion<E,A,O,C> quaternion_type;
- typedef typename quaternion_type::value_type value_type;
- typedef typename quaternion_type::order_type order_type;
-
- /* Checking */
- detail::CheckVec3(axis);
-
- enum {
- W = order_type::W,
- X = order_type::X,
- Y = order_type::Y,
- Z = order_type::Z
- };
-
- angle *= value_type(.5);
-
- /* @todo: If and when we have a set() function that takes a vector and a
- * scalar, this can be written as:
- *
- * q.set(std::cos(angle), axis * std::sin(angle));
- *
- * In which case the enum will also not be necessary.
- */
-
- q[W] = std::cos(angle);
- value_type s = std::sin(angle);
- q[X] = axis[0] * s;
- q[Y] = axis[1] * s;
- q[Z] = axis[2] * s;
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Rotation from a matrix
-//////////////////////////////////////////////////////////////////////////////
-
-/** Build a quaternion from a rotation matrix */
-template < class E, class A, class O, class C, class MatT > void
-quaternion_rotation_matrix(quaternion<E,A,O,C>& q, const MatT& m)
-{
- typedef quaternion<E,A,O,C> quaternion_type;
- typedef typename quaternion_type::value_type value_type;
- typedef typename quaternion_type::order_type order_type;
-
- /* Checking */
- detail::CheckMatLinear3D(m);
-
- enum {
- W = order_type::W,
- X = order_type::X,
- Y = order_type::Y,
- Z = order_type::Z
- };
-
- value_type tr = trace_3x3(m);
- if (tr >= value_type(0)) {
- q[W] = std::sqrt(tr + value_type(1)) * value_type(.5);
- value_type s = value_type(.25) / q[W];
- q[X] = (m.basis_element(1,2) - m.basis_element(2,1)) * s;
- q[Y] = (m.basis_element(2,0) - m.basis_element(0,2)) * s;
- q[Z] = (m.basis_element(0,1) - m.basis_element(1,0)) * s;
- } else {
- size_t largest_diagonal_element =
- index_of_max(
- m.basis_element(0,0),
- m.basis_element(1,1),
- m.basis_element(2,2)
- );
- size_t i, j, k;
- cyclic_permutation(largest_diagonal_element, i, j, k);
- const size_t I = X + i;
- const size_t J = X + j;
- const size_t K = X + k;
- q[I] =
- std::sqrt(
- m.basis_element(i,i) -
- m.basis_element(j,j) -
- m.basis_element(k,k) +
- value_type(1)
- ) * value_type(.5);
- value_type s = value_type(.25) / q[I];
- q[J] = (m.basis_element(i,j) + m.basis_element(j,i)) * s;
- q[K] = (m.basis_element(i,k) + m.basis_element(k,i)) * s;
- q[W] = (m.basis_element(j,k) - m.basis_element(k,j)) * s;
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Rotation from Euler angles
-//////////////////////////////////////////////////////////////////////////////
-
-/** Build a quaternion from an Euler-angle triple */
-template < class E, class A, class O, class C > void
-quaternion_rotation_euler(
- quaternion<E,A,O,C>& q, E angle_0, E angle_1, E angle_2,
- EulerOrder order)
-{
- typedef quaternion<E,A,O,C> quaternion_type;
- typedef typename quaternion_type::value_type value_type;
- typedef typename quaternion_type::order_type order_type;
-
- size_t i, j, k;
- bool odd, repeat;
- detail::unpack_euler_order(order, i, j, k, odd, repeat);
-
- const size_t W = order_type::W;
- const size_t I = order_type::X + i;
- const size_t J = order_type::X + j;
- const size_t K = order_type::X + k;
-
- if (odd) {
- angle_1 = -angle_1;
- }
-
- angle_0 *= value_type(.5);
- angle_1 *= value_type(.5);
- angle_2 *= value_type(.5);
-
- value_type s0 = std::sin(angle_0);
- value_type c0 = std::cos(angle_0);
- value_type s1 = std::sin(angle_1);
- value_type c1 = std::cos(angle_1);
- value_type s2 = std::sin(angle_2);
- value_type c2 = std::cos(angle_2);
-
- value_type s0s2 = s0 * s2;
- value_type s0c2 = s0 * c2;
- value_type c0s2 = c0 * s2;
- value_type c0c2 = c0 * c2;
-
- if (repeat) {
- q[I] = c1 * (c0s2 + s0c2);
- q[J] = s1 * (c0c2 + s0s2);
- q[K] = s1 * (c0s2 - s0c2);
- q[W] = c1 * (c0c2 - s0s2);
- } else {
- q[I] = c1 * s0c2 - s1 * c0s2;
- q[J] = c1 * s0s2 + s1 * c0c2;
- q[K] = c1 * c0s2 - s1 * s0c2;
- q[W] = c1 * c0c2 + s1 * s0s2;
- }
- if (odd) {
- q[J] = -q[J];
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Rotation to align with a vector, multiple vectors, or the view plane
-//////////////////////////////////////////////////////////////////////////////
-
-/** See vector_ortho.h for details */
-template < typename E,class A,class O,class C,class VecT_1,class VecT_2 > void
-quaternion_rotation_align(
- quaternion<E,A,O,C>& q,
- const VecT_1& align,
- const VecT_2& reference,
- bool normalize = true,
- AxisOrder order = axis_order_zyx)
-{
- typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
-
- matrix_type m;
- matrix_rotation_align(m,align,reference,normalize,order);
- quaternion_rotation_matrix(q,m);
-}
-
-/** See vector_ortho.h for details */
-template < typename E, class A, class O, class C, class VecT > void
-quaternion_rotation_align(quaternion<E,A,O,C>& q, const VecT& align,
- bool normalize = true, AxisOrder order = axis_order_zyx)
-{
- typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
-
- matrix_type m;
- matrix_rotation_align(m,align,normalize,order);
- quaternion_rotation_matrix(q,m);
-}
-
-/** See vector_ortho.h for details */
-template < typename E,class A,class O,class C,class VecT_1,class VecT_2 > void
-quaternion_rotation_align_axial(quaternion<E,A,O,C>& q, const VecT_1& align,
- const VecT_2& axis, bool normalize = true,
- AxisOrder order = axis_order_zyx)
-{
- typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
-
- matrix_type m;
- matrix_rotation_align_axial(m,align,axis,normalize,order);
- quaternion_rotation_matrix(q,m);
-}
-
-/** See vector_ortho.h for details */
-template < typename E, class A, class O, class C, class MatT > void
-quaternion_rotation_align_viewplane(
- quaternion<E,A,O,C>& q,
- const MatT& view_matrix,
- Handedness handedness,
- AxisOrder order = axis_order_zyx)
-{
- typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
-
- matrix_type m;
- matrix_rotation_align_viewplane(m,view_matrix,handedness,order);
- quaternion_rotation_matrix(q,m);
-}
-
-/** See vector_ortho.h for details */
-template < typename E, class A, class O, class C, class MatT > void
-quaternion_rotation_align_viewplane_LH(
- quaternion<E,A,O,C>& q,
- const MatT& view_matrix,
- AxisOrder order = axis_order_zyx)
-{
- typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
-
- matrix_type m;
- matrix_rotation_align_viewplane_LH(m,view_matrix,order);
- quaternion_rotation_matrix(q,m);
-}
-
-/** See vector_ortho.h for details */
-template < typename E, class A, class O, class C, class MatT > void
-quaternion_rotation_align_viewplane_RH(
- quaternion<E,A,O,C>& q,
- const MatT& view_matrix,
- AxisOrder order = axis_order_zyx)
-{
- typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
-
- matrix_type m;
- matrix_rotation_align_viewplane_RH(m,view_matrix,order);
- quaternion_rotation_matrix(q,m);
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Rotation to aim at a target
-//////////////////////////////////////////////////////////////////////////////
-
-/** See vector_ortho.h for details */
-template < typename E, class A, class O, class C,
- class VecT_1, class VecT_2, class VecT_3 > void
-quaternion_rotation_aim_at(
- quaternion<E,A,O,C>& q,
- const VecT_1& pos,
- const VecT_2& target,
- const VecT_3& reference,
- AxisOrder order = axis_order_zyx)
-{
- typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
-
- matrix_type m;
- matrix_rotation_aim_at(m,pos,target,reference,order);
- quaternion_rotation_matrix(q,m);
-}
-
-/** See vector_ortho.h for details */
-template < typename E, class A, class O, class C,
- class VecT_1, class VecT_2 > void
-quaternion_rotation_aim_at(
- quaternion<E,A,O,C>& q,
- const VecT_1& pos,
- const VecT_2& target,
- AxisOrder order = axis_order_zyx)
-{
- typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
-
- matrix_type m;
- matrix_rotation_aim_at(m,pos,target,order);
- quaternion_rotation_matrix(q,m);
-}
-
-/** See vector_ortho.h for details */
-template < typename E, class A, class O, class C,
- class VecT_1, class VecT_2, class VecT_3 > void
-quaternion_rotation_aim_at_axial(
- quaternion<E,A,O,C>& q,
- const VecT_1& pos,
- const VecT_2& target,
- const VecT_3& axis,
- AxisOrder order = axis_order_zyx)
-{
- typedef matrix< E,fixed<3,3>,row_basis,row_major > matrix_type;
-
- matrix_type m;
- matrix_rotation_aim_at_axial(m,pos,target,axis,order);
- quaternion_rotation_matrix(q,m);
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Relative rotation about world axes
-//////////////////////////////////////////////////////////////////////////////
-
-/* Rotate a quaternion about the given world axis */
-template < class E, class A, class O, class C > void
-quaternion_rotate_about_world_axis(quaternion<E,A,O,C>& q,size_t axis,E angle)
-{
- typedef quaternion<E,A,O,C> quaternion_type;
- typedef typename quaternion_type::value_type value_type;
- typedef typename quaternion_type::order_type order_type;
-
- /* Checking */
- detail::CheckIndex3(axis);
-
- size_t i, j, k;
- cyclic_permutation(axis, i, j, k);
-
- const size_t W = order_type::W;
- const size_t I = order_type::X + i;
- const size_t J = order_type::X + j;
- const size_t K = order_type::X + k;
-
- angle *= value_type(.5);
- value_type s = value_type(std::sin(angle));
- value_type c = value_type(std::cos(angle));
-
- quaternion_type result;
- result[I] = c * q[I] + s * q[W];
- result[J] = c * q[J] - s * q[K];
- result[K] = c * q[K] + s * q[J];
- result[W] = c * q[W] - s * q[I];
- q = result;
-}
-
-/* Rotate a quaternion about the world x axis */
-template < class E, class A, class O, class C > void
-quaternion_rotate_about_world_x(quaternion<E,A,O,C>& q, E angle) {
- quaternion_rotate_about_world_axis(q,0,angle);
-}
-
-/* Rotate a quaternion about the world y axis */
-template < class E, class A, class O, class C > void
-quaternion_rotate_about_world_y(quaternion<E,A,O,C>& q, E angle) {
- quaternion_rotate_about_world_axis(q,1,angle);
-}
-
-/* Rotate a quaternion about the world z axis */
-template < class E, class A, class O, class C > void
-quaternion_rotate_about_world_z(quaternion<E,A,O,C>& q, E angle) {
- quaternion_rotate_about_world_axis(q,2,angle);
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Relative rotation about local axes
-//////////////////////////////////////////////////////////////////////////////
-
-/* Rotate a quaternion about the given local axis */
-template < class E, class A, class O, class C > void
-quaternion_rotate_about_local_axis(quaternion<E,A,O,C>& q,size_t axis,E angle)
-{
- typedef quaternion<E,A,O,C> quaternion_type;
- typedef typename quaternion_type::value_type value_type;
- typedef typename quaternion_type::order_type order_type;
-
- /* Checking */
- detail::CheckIndex3(axis);
-
- size_t i, j, k;
- cyclic_permutation(axis, i, j, k);
-
- const size_t W = order_type::W;
- const size_t I = order_type::X + i;
- const size_t J = order_type::X + j;
- const size_t K = order_type::X + k;
-
- angle *= value_type(.5);
- value_type s = value_type(std::sin(angle));
- value_type c = value_type(std::cos(angle));
-
- quaternion_type result;
- result[I] = c * q[I] + s * q[W];
- result[J] = c * q[J] + s * q[K];
- result[K] = c * q[K] - s * q[J];
- result[W] = c * q[W] - s * q[I];
- q = result;
-}
-
-/* Rotate a quaternion about its local x axis */
-template < class E, class A, class O, class C > void
-quaternion_rotate_about_local_x(quaternion<E,A,O,C>& q, E angle) {
- quaternion_rotate_about_local_axis(q,0,angle);
-}
-
-/* Rotate a quaternion about its local y axis */
-template < class E, class A, class O, class C > void
-quaternion_rotate_about_local_y(quaternion<E,A,O,C>& q, E angle) {
- quaternion_rotate_about_local_axis(q,1,angle);
-}
-
-/* Rotate a quaternion about its local z axis */
-template < class E, class A, class O, class C > void
-quaternion_rotate_about_local_z(quaternion<E,A,O,C>& q, E angle) {
- quaternion_rotate_about_local_axis(q,2,angle);
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Rotation from vector to vector
-//////////////////////////////////////////////////////////////////////////////
-
-/* http://www.martinb.com/maths/algebra/vectors/angleBetween/index.htm. */
-
-/** Build a quaternion to rotate from one vector to another */
-template < class E,class A,class O,class C,class VecT_1,class VecT_2 > void
-quaternion_rotation_vec_to_vec(
- quaternion<E,A,O,C>& q,
- const VecT_1& v1,
- const VecT_2& v2,
- bool unit_length_vectors = false)
-{
- typedef quaternion<E,A,O,C> quaternion_type;
- typedef typename quaternion_type::value_type value_type;
- typedef vector< value_type, fixed<3> > vector_type;
-
- /* Checking handled by cross() */
-
- /* @todo: If at some point quaternion<> has a set() function that takes a
- * vector and a scalar, this can then be written as:
- *
- * if (...) {
- * q.set(value_type(1)+dot(v1,v2), cross(v1,v2));
- * } else {
- * q.set(std::sqrt(...)+dot(v1,v2), cross(v1,v2));
- * }
- */
-
- vector_type c = cross(v1,v2);
- if (unit_length_vectors) {
- q = quaternion_type(value_type(1) + dot(v1,v2), c.data());
- } else {
- q = quaternion_type(
- std::sqrt(v1.length_squared() * v2.length_squared()) + dot(v1,v2),
- c/*.data()*/
- );
- }
- q.normalize();
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Scale the angle of a rotation matrix
-//////////////////////////////////////////////////////////////////////////////
-
-template < typename E, class A, class O, class C > void
-quaternion_scale_angle(quaternion<E,A,O,C>& q, E t,
- E tolerance = epsilon<E>::placeholder())
-{
- typedef vector< E,fixed<3> > vector_type;
- typedef typename vector_type::value_type value_type;
-
- vector_type axis;
- value_type angle;
- quaternion_to_axis_angle(q, axis, angle, tolerance);
- quaternion_rotation_axis_angle(q, axis, angle * t);
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Support functions for uniform handling of pos- and neg-cross quaternions
-//////////////////////////////////////////////////////////////////////////////
-
-namespace detail {
-
-/** Concatenate two quaternions in the order q1->q2 */
-template < class QuatT_1, class QuatT_2 >
-typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
-quaternion_rotation_difference(
- const QuatT_1& q1, const QuatT_2& q2, positive_cross)
-{
- return q2 * conjugate(q1);
-}
-
-/** Concatenate two quaternions in the order q1->q2 */
-template < class QuatT_1, class QuatT_2 >
-typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
-quaternion_rotation_difference(
- const QuatT_1& q1, const QuatT_2& q2, negative_cross)
-{
- return conjugate(q1) * q2;
-}
-
-} // namespace detail
-
-//////////////////////////////////////////////////////////////////////////////
-// Quaternions rotation difference
-//////////////////////////////////////////////////////////////////////////////
-
-/** Return the rotational 'difference' between two quaternions */
-template < class QuatT_1, class QuatT_2 >
-typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
-quaternion_rotation_difference(const QuatT_1& q1, const QuatT_2& q2) {
- return detail::quaternion_rotation_difference(
- q1, q2, typename QuatT_1::cross_type());
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Conversions
-//////////////////////////////////////////////////////////////////////////////
-
-/** Convert a quaternion to an axis-angle pair */
-template < class QuatT, typename E, class A > void
-quaternion_to_axis_angle(
- const QuatT& q,
- vector<E,A>& axis,
- E& angle,
- E tolerance = epsilon<E>::placeholder())
-{
- typedef QuatT quaternion_type;
- typedef typename quaternion_type::value_type value_type;
- typedef typename quaternion_type::order_type order_type;
-
- /* Checking */
- detail::CheckQuat(q);
-
- axis = q.imaginary();
- value_type l = length(axis);
- if (l > tolerance) {
- axis /= l;
- angle = value_type(2) * std::atan2(l,q.real());
- } else {
- axis.zero();
- angle = value_type(0);
- }
-}
-
-/** Convert a quaternion to an Euler-angle triple
- *
- * Note: I've implemented direct quaternion-to-Euler conversion, but as far as
- * I can tell it more or less reduces to converting the quaternion to a matrix
- * as you go. The direct method is a little more efficient in that it doesn't
- * require a temporary and only the necessary matrix elements need be
- * computed. However, the implementation is complex and there's considerable
- * opportunity for error, so from a development and debugging standpoint I
- * think it's better to just perform the conversion via matrix_to_euler(),
- * which is already known to be correct.
-*/
-
-template < class QuatT, typename Real > void
-quaternion_to_euler(
- const QuatT& q,
- Real& angle_0,
- Real& angle_1,
- Real& angle_2,
- EulerOrder order,
- Real tolerance = epsilon<Real>::placeholder())
-{
- typedef QuatT quaternion_type;
- typedef typename quaternion_type::value_type value_type;
- typedef matrix< value_type,fixed<3,3>,row_basis,row_major > matrix_type;
-
- matrix_type m;
- matrix_rotation_quaternion(m, q);
- matrix_to_euler(m, angle_0, angle_1, angle_2, order, tolerance);
-}
-
-} // namespace cml
-
-#endif
projects / chaz / yoink / blobdiff