123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509 |
- /********************************************************************************************/
- /* */
- /* 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 */
|