| |
| /********************************************************************************************/ |
| /* */ |
| /* HSO3.hpp header file */ |
| /* */ |
| /* This file is not currently part of the Boost library. It is simply an example of the use */ |
| /* quaternions can be put to. Hopefully it will be useful too. */ |
| /* */ |
| /* This file provides tools to convert between quaternions and R^3 rotation matrices. */ |
| /* */ |
| /********************************************************************************************/ |
| |
| // (C) Copyright Hubert Holin 2001. |
| // Distributed under the Boost Software License, Version 1.0. (See |
| // accompanying file LICENSE_1_0.txt or copy at |
| // http://www.boost.org/LICENSE_1_0.txt) |
| |
| #ifndef TEST_HSO3_HPP |
| #define TEST_HSO3_HPP |
| |
| #include <algorithm> |
| |
| #if defined(__GNUC__) && (__GNUC__ < 3) |
| #include <boost/limits.hpp> |
| #else |
| #include <limits> |
| #endif |
| |
| #include <stdexcept> |
| #include <string> |
| |
| #include <boost/math/quaternion.hpp> |
| |
| |
| #if defined(__GNUC__) && (__GNUC__ < 3) |
| // gcc 2.x ignores function scope using declarations, put them here instead: |
| using namespace ::std; |
| using namespace ::boost::math; |
| #endif |
| |
| template<typename TYPE_FLOAT> |
| struct R3_matrix |
| { |
| TYPE_FLOAT a11, a12, a13; |
| TYPE_FLOAT a21, a22, a23; |
| TYPE_FLOAT a31, a32, a33; |
| }; |
| |
| |
| // Note: the input quaternion need not be of norm 1 for the following function |
| |
| template<typename TYPE_FLOAT> |
| R3_matrix<TYPE_FLOAT> quaternion_to_R3_rotation(::boost::math::quaternion<TYPE_FLOAT> const & q) |
| { |
| using ::std::numeric_limits; |
| |
| TYPE_FLOAT a = q.R_component_1(); |
| TYPE_FLOAT b = q.R_component_2(); |
| TYPE_FLOAT c = q.R_component_3(); |
| TYPE_FLOAT d = q.R_component_4(); |
| |
| TYPE_FLOAT aa = a*a; |
| TYPE_FLOAT ab = a*b; |
| TYPE_FLOAT ac = a*c; |
| TYPE_FLOAT ad = a*d; |
| TYPE_FLOAT bb = b*b; |
| TYPE_FLOAT bc = b*c; |
| TYPE_FLOAT bd = b*d; |
| TYPE_FLOAT cc = c*c; |
| TYPE_FLOAT cd = c*d; |
| TYPE_FLOAT dd = d*d; |
| |
| TYPE_FLOAT norme_carre = aa+bb+cc+dd; |
| |
| if (norme_carre <= numeric_limits<TYPE_FLOAT>::epsilon()) |
| { |
| ::std::string error_reporting("Argument to quaternion_to_R3_rotation is too small!"); |
| ::std::underflow_error bad_argument(error_reporting); |
| |
| throw(bad_argument); |
| } |
| |
| R3_matrix<TYPE_FLOAT> out_matrix; |
| |
| out_matrix.a11 = (aa+bb-cc-dd)/norme_carre; |
| out_matrix.a12 = 2*(-ad+bc)/norme_carre; |
| out_matrix.a13 = 2*(ac+bd)/norme_carre; |
| out_matrix.a21 = 2*(ad+bc)/norme_carre; |
| out_matrix.a22 = (aa-bb+cc-dd)/norme_carre; |
| out_matrix.a23 = 2*(-ab+cd)/norme_carre; |
| out_matrix.a31 = 2*(-ac+bd)/norme_carre; |
| out_matrix.a32 = 2*(ab+cd)/norme_carre; |
| out_matrix.a33 = (aa-bb-cc+dd)/norme_carre; |
| |
| return(out_matrix); |
| } |
| |
| |
| template<typename TYPE_FLOAT> |
| void find_invariant_vector( R3_matrix<TYPE_FLOAT> const & rot, |
| TYPE_FLOAT & x, |
| TYPE_FLOAT & y, |
| TYPE_FLOAT & z) |
| { |
| using ::std::sqrt; |
| |
| using ::std::numeric_limits; |
| |
| TYPE_FLOAT b11 = rot.a11 - static_cast<TYPE_FLOAT>(1); |
| TYPE_FLOAT b12 = rot.a12; |
| TYPE_FLOAT b13 = rot.a13; |
| TYPE_FLOAT b21 = rot.a21; |
| TYPE_FLOAT b22 = rot.a22 - static_cast<TYPE_FLOAT>(1); |
| TYPE_FLOAT b23 = rot.a23; |
| TYPE_FLOAT b31 = rot.a31; |
| TYPE_FLOAT b32 = rot.a32; |
| TYPE_FLOAT b33 = rot.a33 - static_cast<TYPE_FLOAT>(1); |
| |
| TYPE_FLOAT minors[9] = |
| { |
| b11*b22-b12*b21, |
| b11*b23-b13*b21, |
| b12*b23-b13*b22, |
| b11*b32-b12*b31, |
| b11*b33-b13*b31, |
| b12*b33-b13*b32, |
| b21*b32-b22*b31, |
| b21*b33-b23*b31, |
| b22*b33-b23*b32 |
| }; |
| |
| TYPE_FLOAT * where = ::std::max_element(minors, minors+9); |
| |
| TYPE_FLOAT det = *where; |
| |
| if (det <= numeric_limits<TYPE_FLOAT>::epsilon()) |
| { |
| ::std::string error_reporting("Underflow error in find_invariant_vector!"); |
| ::std::underflow_error processing_error(error_reporting); |
| |
| throw(processing_error); |
| } |
| |
| switch (where-minors) |
| { |
| case 0: |
| |
| z = static_cast<TYPE_FLOAT>(1); |
| |
| x = (-b13*b22+b12*b23)/det; |
| y = (-b11*b23+b13*b21)/det; |
| |
| break; |
| |
| case 1: |
| |
| y = static_cast<TYPE_FLOAT>(1); |
| |
| x = (-b12*b23+b13*b22)/det; |
| z = (-b11*b22+b12*b21)/det; |
| |
| break; |
| |
| case 2: |
| |
| x = static_cast<TYPE_FLOAT>(1); |
| |
| y = (-b11*b23+b13*b21)/det; |
| z = (-b12*b21+b11*b22)/det; |
| |
| break; |
| |
| case 3: |
| |
| z = static_cast<TYPE_FLOAT>(1); |
| |
| x = (-b13*b32+b12*b33)/det; |
| y = (-b11*b33+b13*b31)/det; |
| |
| break; |
| |
| case 4: |
| |
| y = static_cast<TYPE_FLOAT>(1); |
| |
| x = (-b12*b33+b13*b32)/det; |
| z = (-b11*b32+b12*b31)/det; |
| |
| break; |
| |
| case 5: |
| |
| x = static_cast<TYPE_FLOAT>(1); |
| |
| y = (-b11*b33+b13*b31)/det; |
| z = (-b12*b31+b11*b32)/det; |
| |
| break; |
| |
| case 6: |
| |
| z = static_cast<TYPE_FLOAT>(1); |
| |
| x = (-b23*b32+b22*b33)/det; |
| y = (-b21*b33+b23*b31)/det; |
| |
| break; |
| |
| case 7: |
| |
| y = static_cast<TYPE_FLOAT>(1); |
| |
| x = (-b22*b33+b23*b32)/det; |
| z = (-b21*b32+b22*b31)/det; |
| |
| break; |
| |
| case 8: |
| |
| x = static_cast<TYPE_FLOAT>(1); |
| |
| y = (-b21*b33+b23*b31)/det; |
| z = (-b22*b31+b21*b32)/det; |
| |
| break; |
| |
| default: |
| |
| ::std::string error_reporting("Impossible condition in find_invariant_vector"); |
| ::std::logic_error processing_error(error_reporting); |
| |
| throw(processing_error); |
| |
| break; |
| } |
| |
| TYPE_FLOAT vecnorm = sqrt(x*x+y*y+z*z); |
| |
| if (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon()) |
| { |
| ::std::string error_reporting("Overflow error in find_invariant_vector!"); |
| ::std::overflow_error processing_error(error_reporting); |
| |
| throw(processing_error); |
| } |
| |
| x /= vecnorm; |
| y /= vecnorm; |
| z /= vecnorm; |
| } |
| |
| |
| template<typename TYPE_FLOAT> |
| void find_orthogonal_vector( TYPE_FLOAT x, |
| TYPE_FLOAT y, |
| TYPE_FLOAT z, |
| TYPE_FLOAT & u, |
| TYPE_FLOAT & v, |
| TYPE_FLOAT & w) |
| { |
| using ::std::abs; |
| using ::std::sqrt; |
| |
| using ::std::numeric_limits; |
| |
| TYPE_FLOAT vecnormsqr = x*x+y*y+z*z; |
| |
| if (vecnormsqr <= numeric_limits<TYPE_FLOAT>::epsilon()) |
| { |
| ::std::string error_reporting("Underflow error in find_orthogonal_vector!"); |
| ::std::underflow_error processing_error(error_reporting); |
| |
| throw(processing_error); |
| } |
| |
| TYPE_FLOAT lambda; |
| |
| TYPE_FLOAT components[3] = |
| { |
| abs(x), |
| abs(y), |
| abs(z) |
| }; |
| |
| TYPE_FLOAT * where = ::std::min_element(components, components+3); |
| |
| switch (where-components) |
| { |
| case 0: |
| |
| if (*where <= numeric_limits<TYPE_FLOAT>::epsilon()) |
| { |
| v = |
| w = static_cast<TYPE_FLOAT>(0); |
| u = static_cast<TYPE_FLOAT>(1); |
| } |
| else |
| { |
| lambda = -x/vecnormsqr; |
| |
| u = static_cast<TYPE_FLOAT>(1) + lambda*x; |
| v = lambda*y; |
| w = lambda*z; |
| } |
| |
| break; |
| |
| case 1: |
| |
| if (*where <= numeric_limits<TYPE_FLOAT>::epsilon()) |
| { |
| u = |
| w = static_cast<TYPE_FLOAT>(0); |
| v = static_cast<TYPE_FLOAT>(1); |
| } |
| else |
| { |
| lambda = -y/vecnormsqr; |
| |
| u = lambda*x; |
| v = static_cast<TYPE_FLOAT>(1) + lambda*y; |
| w = lambda*z; |
| } |
| |
| break; |
| |
| case 2: |
| |
| if (*where <= numeric_limits<TYPE_FLOAT>::epsilon()) |
| { |
| u = |
| v = static_cast<TYPE_FLOAT>(0); |
| w = static_cast<TYPE_FLOAT>(1); |
| } |
| else |
| { |
| lambda = -z/vecnormsqr; |
| |
| u = lambda*x; |
| v = lambda*y; |
| w = static_cast<TYPE_FLOAT>(1) + lambda*z; |
| } |
| |
| break; |
| |
| default: |
| |
| ::std::string error_reporting("Impossible condition in find_invariant_vector"); |
| ::std::logic_error processing_error(error_reporting); |
| |
| throw(processing_error); |
| |
| break; |
| } |
| |
| TYPE_FLOAT vecnorm = sqrt(u*u+v*v+w*w); |
| |
| if (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon()) |
| { |
| ::std::string error_reporting("Underflow error in find_orthogonal_vector!"); |
| ::std::underflow_error processing_error(error_reporting); |
| |
| throw(processing_error); |
| } |
| |
| u /= vecnorm; |
| v /= vecnorm; |
| w /= vecnorm; |
| } |
| |
| |
| // Note: we want [[v, v, w], [r, s, t], [x, y, z]] to be a direct orthogonal basis |
| // of R^3. It might not be orthonormal, however, and we do not check if the |
| // two input vectors are colinear or not. |
| |
| template<typename TYPE_FLOAT> |
| void find_vector_for_BOD(TYPE_FLOAT x, |
| TYPE_FLOAT y, |
| TYPE_FLOAT z, |
| TYPE_FLOAT u, |
| TYPE_FLOAT v, |
| TYPE_FLOAT w, |
| TYPE_FLOAT & r, |
| TYPE_FLOAT & s, |
| TYPE_FLOAT & t) |
| { |
| r = +y*w-z*v; |
| s = -x*w+z*u; |
| t = +x*v-y*u; |
| } |
| |
| |
| |
| template<typename TYPE_FLOAT> |
| inline bool is_R3_rotation_matrix(R3_matrix<TYPE_FLOAT> const & mat) |
| { |
| using ::std::abs; |
| |
| using ::std::numeric_limits; |
| |
| return ( |
| !( |
| (abs(mat.a11*mat.a11+mat.a21*mat.a21+mat.a31*mat.a31 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())|| |
| (abs(mat.a11*mat.a12+mat.a21*mat.a22+mat.a31*mat.a32 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())|| |
| (abs(mat.a11*mat.a13+mat.a21*mat.a23+mat.a31*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())|| |
| //(abs(mat.a11*mat.a12+mat.a21*mat.a22+mat.a31*mat.a32 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())|| |
| (abs(mat.a12*mat.a12+mat.a22*mat.a22+mat.a32*mat.a32 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())|| |
| (abs(mat.a12*mat.a13+mat.a22*mat.a23+mat.a32*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())|| |
| //(abs(mat.a11*mat.a13+mat.a21*mat.a23+mat.a31*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())|| |
| //(abs(mat.a12*mat.a13+mat.a22*mat.a23+mat.a32*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())|| |
| (abs(mat.a13*mat.a13+mat.a23*mat.a23+mat.a33*mat.a33 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon()) |
| ) |
| ); |
| } |
| |
| |
| template<typename TYPE_FLOAT> |
| ::boost::math::quaternion<TYPE_FLOAT> R3_rotation_to_quaternion( R3_matrix<TYPE_FLOAT> const & rot, |
| ::boost::math::quaternion<TYPE_FLOAT> const * hint = 0) |
| { |
| using ::boost::math::abs; |
| |
| using ::std::abs; |
| using ::std::sqrt; |
| |
| using ::std::numeric_limits; |
| |
| if (!is_R3_rotation_matrix(rot)) |
| { |
| ::std::string error_reporting("Argument to R3_rotation_to_quaternion is not an R^3 rotation matrix!"); |
| ::std::range_error bad_argument(error_reporting); |
| |
| throw(bad_argument); |
| } |
| |
| ::boost::math::quaternion<TYPE_FLOAT> q; |
| |
| if ( |
| (abs(rot.a11 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&& |
| (abs(rot.a22 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&& |
| (abs(rot.a33 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon()) |
| ) |
| { |
| q = ::boost::math::quaternion<TYPE_FLOAT>(1); |
| } |
| else |
| { |
| TYPE_FLOAT cos_theta = (rot.a11+rot.a22+rot.a33-static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2); |
| TYPE_FLOAT stuff = (cos_theta+static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2); |
| TYPE_FLOAT cos_theta_sur_2 = sqrt(stuff); |
| TYPE_FLOAT sin_theta_sur_2 = sqrt(1-stuff); |
| |
| TYPE_FLOAT x; |
| TYPE_FLOAT y; |
| TYPE_FLOAT z; |
| |
| find_invariant_vector(rot, x, y, z); |
| |
| TYPE_FLOAT u; |
| TYPE_FLOAT v; |
| TYPE_FLOAT w; |
| |
| find_orthogonal_vector(x, y, z, u, v, w); |
| |
| TYPE_FLOAT r; |
| TYPE_FLOAT s; |
| TYPE_FLOAT t; |
| |
| find_vector_for_BOD(x, y, z, u, v, w, r, s, t); |
| |
| TYPE_FLOAT ru = rot.a11*u+rot.a12*v+rot.a13*w; |
| TYPE_FLOAT rv = rot.a21*u+rot.a22*v+rot.a23*w; |
| TYPE_FLOAT rw = rot.a31*u+rot.a32*v+rot.a33*w; |
| |
| TYPE_FLOAT angle_sign_determinator = r*ru+s*rv+t*rw; |
| |
| if (angle_sign_determinator > +numeric_limits<TYPE_FLOAT>::epsilon()) |
| { |
| q = ::boost::math::quaternion<TYPE_FLOAT>(cos_theta_sur_2, +x*sin_theta_sur_2, +y*sin_theta_sur_2, +z*sin_theta_sur_2); |
| } |
| else if (angle_sign_determinator < -numeric_limits<TYPE_FLOAT>::epsilon()) |
| { |
| q = ::boost::math::quaternion<TYPE_FLOAT>(cos_theta_sur_2, -x*sin_theta_sur_2, -y*sin_theta_sur_2, -z*sin_theta_sur_2); |
| } |
| else |
| { |
| TYPE_FLOAT desambiguator = u*ru+v*rv+w*rw; |
| |
| if (desambiguator >= static_cast<TYPE_FLOAT>(1)) |
| { |
| q = ::boost::math::quaternion<TYPE_FLOAT>(0, +x, +y, +z); |
| } |
| else |
| { |
| q = ::boost::math::quaternion<TYPE_FLOAT>(0, -x, -y, -z); |
| } |
| } |
| } |
| |
| if ((hint != 0) && (abs(*hint+q) < abs(*hint-q))) |
| { |
| return(-q); |
| } |
| |
| return(q); |
| } |
| |
| #endif /* TEST_HSO3_HPP */ |
| |