X-Git-Url: https://git.dogcows.com/gitweb?p=chaz%2Fyoink;a=blobdiff_plain;f=src%2FMoof%2Fcml%2Fmathlib%2Fquaternion_rotation.h;fp=src%2FMoof%2Fcml%2Fmathlib%2Fquaternion_rotation.h;h=0000000000000000000000000000000000000000;hp=2e8c0e9669193733ecb1c37a58fe2992f523a554;hb=831f04d4bc19a390415ac0bbac4331c7a65509bc;hpb=299af4f2047e767e5d79501c26444473bda64c64 diff --git a/src/Moof/cml/mathlib/quaternion_rotation.h b/src/Moof/cml/mathlib/quaternion_rotation.h deleted file mode 100644 index 2e8c0e9..0000000 --- a/src/Moof/cml/mathlib/quaternion_rotation.h +++ /dev/null @@ -1,635 +0,0 @@ -/* -*- 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 - -/* 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& q, size_t axis, E angle) -{ - typedef quaternion 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& 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& 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& 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& q, const VecT& axis, E angle) -{ - typedef quaternion 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& q, const MatT& m) -{ - typedef quaternion 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& q, E angle_0, E angle_1, E angle_2, - EulerOrder order) -{ - typedef quaternion 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& q,size_t axis,E angle) -{ - typedef quaternion 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& 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& 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& 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& q,size_t axis,E angle) -{ - typedef quaternion 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& 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& 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& 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& q, - const VecT_1& v1, - const VecT_2& v2, - bool unit_length_vectors = false) -{ - typedef quaternion 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& q, E t, - E tolerance = epsilon::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::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::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::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& axis, - E& angle, - E tolerance = epsilon::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::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