Symaptic: os/ossrv/stdcpp/tsrc/Boost_test/math/quaternion/inc/HSO3.hpp@260cb5ec6c19

     2 /********************************************************************************************/

     3 /*                                                                                          */

     4 /*                                HSO3.hpp header file                                      */

     5 /*                                                                                          */

     6 /* This file is not currently part of the Boost library. It is simply an example of the use */

     7 /* quaternions can be put to. Hopefully it will be useful too.                              */

     8 /*                                                                                          */

     9 /* This file provides tools to convert between quaternions and R^3 rotation matrices.       */

    10 /*                                                                                          */

    11 /********************************************************************************************/

    13 //  (C) Copyright Hubert Holin 2001.

    14 //  Distributed under the Boost Software License, Version 1.0. (See

    15 //  accompanying file LICENSE_1_0.txt or copy at

    16 //  http://www.boost.org/LICENSE_1_0.txt)

    18 #ifndef TEST_HSO3_HPP

    19 #define TEST_HSO3_HPP

    21 #include <algorithm>

    23 #if    defined(__GNUC__) && (__GNUC__ < 3)

    24 #include <boost/limits.hpp>

    25 #else

    26 #include <limits>

    27 #endif

    29 #include <stdexcept>

    30 #include <string>

    32 #include <boost/math/quaternion.hpp>

    35 #if    defined(__GNUC__) && (__GNUC__ < 3)

    36 // gcc 2.x ignores function scope using declarations, put them here instead:

    37 using    namespace ::std;

    38 using    namespace ::boost::math;

    39 #endif

    41 template<typename TYPE_FLOAT>

    42 struct  R3_matrix

    43 {

    44     TYPE_FLOAT a11, a12, a13;

    45     TYPE_FLOAT a21, a22, a23;

    46     TYPE_FLOAT a31, a32, a33;

    47 };

    50 // Note:    the input quaternion need not be of norm 1 for the following function

    52 template<typename TYPE_FLOAT>

    53 R3_matrix<TYPE_FLOAT>    quaternion_to_R3_rotation(::boost::math::quaternion<TYPE_FLOAT> const & q)

    54 {

    55     using    ::std::numeric_limits;

    57     TYPE_FLOAT    a = q.R_component_1();

    58     TYPE_FLOAT    b = q.R_component_2();

    59     TYPE_FLOAT    c = q.R_component_3();

    60     TYPE_FLOAT    d = q.R_component_4();

    62     TYPE_FLOAT    aa = a*a;

    63     TYPE_FLOAT    ab = a*b;

    64     TYPE_FLOAT    ac = a*c;

    65     TYPE_FLOAT    ad = a*d;

    66     TYPE_FLOAT    bb = b*b;

    67     TYPE_FLOAT    bc = b*c;

    68     TYPE_FLOAT    bd = b*d;

    69     TYPE_FLOAT    cc = c*c;

    70     TYPE_FLOAT    cd = c*d;

    71     TYPE_FLOAT    dd = d*d;

    73     TYPE_FLOAT    norme_carre = aa+bb+cc+dd;

    75     if    (norme_carre <= numeric_limits<TYPE_FLOAT>::epsilon())

    76     {

    77         ::std::string            error_reporting("Argument to quaternion_to_R3_rotation is too small!");

    78         ::std::underflow_error   bad_argument(error_reporting);

    80         throw(bad_argument);

    81     }

    83     R3_matrix<TYPE_FLOAT>    out_matrix;

    85     out_matrix.a11 = (aa+bb-cc-dd)/norme_carre;

    86     out_matrix.a12 = 2*(-ad+bc)/norme_carre;

    87     out_matrix.a13 = 2*(ac+bd)/norme_carre;

    88     out_matrix.a21 = 2*(ad+bc)/norme_carre;

    89     out_matrix.a22 = (aa-bb+cc-dd)/norme_carre;

    90     out_matrix.a23 = 2*(-ab+cd)/norme_carre;

    91     out_matrix.a31 = 2*(-ac+bd)/norme_carre;

    92     out_matrix.a32 = 2*(ab+cd)/norme_carre;

    93     out_matrix.a33 = (aa-bb-cc+dd)/norme_carre;

    95     return(out_matrix);

    96 }

    99     template<typename TYPE_FLOAT>

   100     void    find_invariant_vector(  R3_matrix<TYPE_FLOAT> const & rot,

   101                                     TYPE_FLOAT & x,

   102                                     TYPE_FLOAT & y,

   103                                     TYPE_FLOAT & z)

   104     {

   105         using    ::std::sqrt;

   107         using    ::std::numeric_limits;

   109         TYPE_FLOAT    b11 = rot.a11 - static_cast<TYPE_FLOAT>(1);

   110         TYPE_FLOAT    b12 = rot.a12;

   111         TYPE_FLOAT    b13 = rot.a13;

   112         TYPE_FLOAT    b21 = rot.a21;

   113         TYPE_FLOAT    b22 = rot.a22 - static_cast<TYPE_FLOAT>(1);

   114         TYPE_FLOAT    b23 = rot.a23;

   115         TYPE_FLOAT    b31 = rot.a31;

   116         TYPE_FLOAT    b32 = rot.a32;

   117         TYPE_FLOAT    b33 = rot.a33 - static_cast<TYPE_FLOAT>(1);

   119         TYPE_FLOAT    minors[9] =

   120         {

   121             b11*b22-b12*b21,

   122             b11*b23-b13*b21,

   123             b12*b23-b13*b22,

   124             b11*b32-b12*b31,

   125             b11*b33-b13*b31,

   126             b12*b33-b13*b32,

   127             b21*b32-b22*b31,

   128             b21*b33-b23*b31,

   129             b22*b33-b23*b32

   130         };

   132         TYPE_FLOAT *        where = ::std::max_element(minors, minors+9);

   134         TYPE_FLOAT          det = *where;

   136         if    (det <= numeric_limits<TYPE_FLOAT>::epsilon())

   137         {

   138             ::std::string            error_reporting("Underflow error in find_invariant_vector!");

   139             ::std::underflow_error   processing_error(error_reporting);

   141             throw(processing_error);

   142         }

   144         switch    (where-minors)

   145         {

   146             case 0:

   148                 z = static_cast<TYPE_FLOAT>(1);

   150                 x = (-b13*b22+b12*b23)/det;

   151                 y = (-b11*b23+b13*b21)/det;

   153                 break;

   155             case 1:

   157                 y = static_cast<TYPE_FLOAT>(1);

   159                 x = (-b12*b23+b13*b22)/det;

   160                 z = (-b11*b22+b12*b21)/det;

   162                 break;

   164             case 2:

   166                 x = static_cast<TYPE_FLOAT>(1);

   168                 y = (-b11*b23+b13*b21)/det;

   169                 z = (-b12*b21+b11*b22)/det;

   171                 break;

   173             case 3:

   175                 z = static_cast<TYPE_FLOAT>(1);

   177                 x = (-b13*b32+b12*b33)/det;

   178                 y = (-b11*b33+b13*b31)/det;

   180                 break;

   182             case 4:

   184                 y = static_cast<TYPE_FLOAT>(1);

   186                 x = (-b12*b33+b13*b32)/det;

   187                 z = (-b11*b32+b12*b31)/det;

   189                 break;

   191             case 5:

   193                 x = static_cast<TYPE_FLOAT>(1);

   195                 y = (-b11*b33+b13*b31)/det;

   196                 z = (-b12*b31+b11*b32)/det;

   198                 break;

   200             case 6:

   202                 z = static_cast<TYPE_FLOAT>(1);

   204                 x = (-b23*b32+b22*b33)/det;

   205                 y = (-b21*b33+b23*b31)/det;

   207                 break;

   209             case 7:

   211                 y = static_cast<TYPE_FLOAT>(1);

   213                 x = (-b22*b33+b23*b32)/det;

   214                 z = (-b21*b32+b22*b31)/det;

   216                 break;

   218             case 8:

   220                 x = static_cast<TYPE_FLOAT>(1);

   222                 y = (-b21*b33+b23*b31)/det;

   223                 z = (-b22*b31+b21*b32)/det;

   225                 break;

   227             default:

   229                 ::std::string        error_reporting("Impossible condition in find_invariant_vector");

   230                 ::std::logic_error   processing_error(error_reporting);

   232                 throw(processing_error);

   234                 break;

   235         }

   237         TYPE_FLOAT    vecnorm = sqrt(x*x+y*y+z*z);

   239         if    (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon())

   240         {

   241             ::std::string            error_reporting("Overflow error in find_invariant_vector!");

   242             ::std::overflow_error    processing_error(error_reporting);

   244             throw(processing_error);

   245         }

   247         x /= vecnorm;

   248         y /= vecnorm;

   249         z /= vecnorm;

   250     }

   253     template<typename TYPE_FLOAT>

   254     void    find_orthogonal_vector( TYPE_FLOAT x,

   255                                     TYPE_FLOAT y,

   256                                     TYPE_FLOAT z,

   257                                     TYPE_FLOAT & u,

   258                                     TYPE_FLOAT & v,

   259                                     TYPE_FLOAT & w)

   260     {

   261         using    ::std::abs;

   262         using    ::std::sqrt;

   264         using    ::std::numeric_limits;

   266         TYPE_FLOAT    vecnormsqr = x*x+y*y+z*z;

   268         if    (vecnormsqr <= numeric_limits<TYPE_FLOAT>::epsilon())

   269         {

   270             ::std::string            error_reporting("Underflow error in find_orthogonal_vector!");

   271             ::std::underflow_error   processing_error(error_reporting);

   273             throw(processing_error);

   274         }

   276         TYPE_FLOAT        lambda;

   278         TYPE_FLOAT        components[3] =

   279         {

   280             abs(x),

   281             abs(y),

   282             abs(z)

   283         };

   285         TYPE_FLOAT *    where = ::std::min_element(components, components+3);

   287         switch    (where-components)

   288         {

   289             case 0:

   291                 if    (*where <= numeric_limits<TYPE_FLOAT>::epsilon())

   292                 {

   293                     v =

   294                     w = static_cast<TYPE_FLOAT>(0);

   295                     u = static_cast<TYPE_FLOAT>(1);

   296                 }

   297                 else

   298                 {

   299                     lambda = -x/vecnormsqr;

   301                     u = static_cast<TYPE_FLOAT>(1) + lambda*x;

   302                     v = lambda*y;

   303                     w = lambda*z;

   304                 }

   306                 break;

   308             case 1:

   310                 if    (*where <= numeric_limits<TYPE_FLOAT>::epsilon())

   311                 {

   312                     u =

   313                     w = static_cast<TYPE_FLOAT>(0);

   314                     v = static_cast<TYPE_FLOAT>(1);

   315                 }

   316                 else

   317                 {

   318                     lambda = -y/vecnormsqr;

   320                     u = lambda*x;

   321                     v = static_cast<TYPE_FLOAT>(1) + lambda*y;

   322                     w = lambda*z;

   323                 }

   325                 break;

   327             case 2:

   329                 if    (*where <= numeric_limits<TYPE_FLOAT>::epsilon())

   330                 {

   331                     u =

   332                     v = static_cast<TYPE_FLOAT>(0);

   333                     w = static_cast<TYPE_FLOAT>(1);

   334                 }

   335                 else

   336                 {

   337                     lambda = -z/vecnormsqr;

   339                     u = lambda*x;

   340                     v = lambda*y;

   341                     w = static_cast<TYPE_FLOAT>(1) + lambda*z;

   342                 }

   344                 break;

   346             default:

   348                 ::std::string        error_reporting("Impossible condition in find_invariant_vector");

   349                 ::std::logic_error   processing_error(error_reporting);

   351                 throw(processing_error);

   353                 break;

   354         }

   356         TYPE_FLOAT    vecnorm = sqrt(u*u+v*v+w*w);

   358         if    (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon())

   359         {

   360             ::std::string            error_reporting("Underflow error in find_orthogonal_vector!");

   361             ::std::underflow_error   processing_error(error_reporting);

   363             throw(processing_error);

   364         }

   366         u /= vecnorm;

   367         v /= vecnorm;

   368         w /= vecnorm;

   369     }

   372     // Note:    we want [[v, v, w], [r, s, t], [x, y, z]] to be a direct orthogonal basis

   373     //            of R^3. It might not be orthonormal, however, and we do not check if the

   374     //            two input vectors are colinear or not.

   376     template<typename TYPE_FLOAT>

   377     void    find_vector_for_BOD(TYPE_FLOAT x,

   378                                 TYPE_FLOAT y,

   379                                 TYPE_FLOAT z,

   380                                 TYPE_FLOAT u,

   381                                 TYPE_FLOAT v,

   382                                 TYPE_FLOAT w,

   383                                 TYPE_FLOAT & r,

   384                                 TYPE_FLOAT & s,

   385                                 TYPE_FLOAT & t)

   386     {

   387         r = +y*w-z*v;

   388         s = -x*w+z*u;

   389         t = +x*v-y*u;

   390     }

   394 template<typename TYPE_FLOAT>

   395 inline bool                                is_R3_rotation_matrix(R3_matrix<TYPE_FLOAT> const & mat)

   396 {

   397     using    ::std::abs;

   399     using    ::std::numeric_limits;

   401     return    (

   402                 !(

   403                     (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())||

   404                     (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())||

   405                     (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())||

   406                     //(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())||

   407                     (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())||

   408                     (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())||

   409                     //(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())||

   410                     //(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())||

   411                     (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())

   412                 )

   413             );

   414 }

   417 template<typename TYPE_FLOAT>

   418 ::boost::math::quaternion<TYPE_FLOAT>    R3_rotation_to_quaternion(    R3_matrix<TYPE_FLOAT> const & rot,

   419                                                                     ::boost::math::quaternion<TYPE_FLOAT> const * hint = 0)

   420 {

   421     using    ::boost::math::abs;

   423     using    ::std::abs;

   424     using    ::std::sqrt;

   426     using    ::std::numeric_limits;

   428     if    (!is_R3_rotation_matrix(rot))

   429     {

   430         ::std::string        error_reporting("Argument to R3_rotation_to_quaternion is not an R^3 rotation matrix!");

   431         ::std::range_error   bad_argument(error_reporting);

   433         throw(bad_argument);

   434     }

   436     ::boost::math::quaternion<TYPE_FLOAT>    q;

   438     if    (

   439             (abs(rot.a11 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&&

   440             (abs(rot.a22 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&&

   441             (abs(rot.a33 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())

   442         )

   443     {

   444         q = ::boost::math::quaternion<TYPE_FLOAT>(1);

   445     }

   446     else

   447     {

   448         TYPE_FLOAT    cos_theta = (rot.a11+rot.a22+rot.a33-static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2);

   449         TYPE_FLOAT    stuff = (cos_theta+static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2);

   450         TYPE_FLOAT    cos_theta_sur_2 = sqrt(stuff);

   451         TYPE_FLOAT    sin_theta_sur_2 = sqrt(1-stuff);

   453         TYPE_FLOAT    x;

   454         TYPE_FLOAT    y;

   455         TYPE_FLOAT    z;

   457         find_invariant_vector(rot, x, y, z);

   459         TYPE_FLOAT    u;

   460         TYPE_FLOAT    v;

   461         TYPE_FLOAT    w;

   463         find_orthogonal_vector(x, y, z, u, v, w);

   465         TYPE_FLOAT    r;

   466         TYPE_FLOAT    s;

   467         TYPE_FLOAT    t;

   469         find_vector_for_BOD(x, y, z, u, v, w, r, s, t);

   471         TYPE_FLOAT    ru = rot.a11*u+rot.a12*v+rot.a13*w;

   472         TYPE_FLOAT    rv = rot.a21*u+rot.a22*v+rot.a23*w;

   473         TYPE_FLOAT    rw = rot.a31*u+rot.a32*v+rot.a33*w;

   475         TYPE_FLOAT    angle_sign_determinator = r*ru+s*rv+t*rw;

   477         if        (angle_sign_determinator > +numeric_limits<TYPE_FLOAT>::epsilon())

   478         {

   479             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);

   480         }

   481         else if    (angle_sign_determinator < -numeric_limits<TYPE_FLOAT>::epsilon())

   482         {

   483             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);

   484         }

   485         else

   486         {

   487             TYPE_FLOAT    desambiguator = u*ru+v*rv+w*rw;

   489             if    (desambiguator >= static_cast<TYPE_FLOAT>(1))

   490             {

   491                 q = ::boost::math::quaternion<TYPE_FLOAT>(0, +x, +y, +z);

   492             }

   493             else

   494             {

   495                 q = ::boost::math::quaternion<TYPE_FLOAT>(0, -x, -y, -z);

   496             }

   497         }

   498     }

   500     if    ((hint != 0) && (abs(*hint+q) < abs(*hint-q)))

   501     {

   502         return(-q);

   503     }

   505     return(q);

   506 }

   508 #endif /* TEST_HSO3_HPP */

author	sl
	Tue, 10 Jun 2014 14:32:02 +0200
changeset 1	260cb5ec6c19
permissions	-rw-r--r--