MercurialSymaptic / file revision
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | raw | help
os/ossrv/stdcpp/tsrc/Boost_test/math/quaternion/inc/HSO4.hpp
author sl
Tue, 10 Jun 2014 14:32:02 +0200
changeset 1 260cb5ec6c19
permissions -rw-r--r--
Update contrib. 
     1 

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

     3 /*                                                                                          */

     4 /*                                HSO4.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 usefull too.                             */

     8 /*                                                                                          */

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

    10 /*                                                                                          */

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

    12 

    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)

    17 

    18 #ifndef TEST_HSO4_HPP

    19 #define TEST_HSO4_HPP

    20 

    21 #include <utility>

    22 

    23 #include "HSO3.hpp"

    24 

    25 

    26 template<typename TYPE_FLOAT>

    27 struct  R4_matrix

    28 {

    29     TYPE_FLOAT a11, a12, a13, a14;

    30     TYPE_FLOAT a21, a22, a23, a24;

    31     TYPE_FLOAT a31, a32, a33, a34;

    32     TYPE_FLOAT a41, a42, a43, a44;

    33 };

    34 

    35 

    36 // Note:    the input quaternions need not be of norm 1 for the following function

    37 

    38 template<typename TYPE_FLOAT>

    39 R4_matrix<TYPE_FLOAT>    quaternions_to_R4_rotation(::std::pair< ::boost::math::quaternion<TYPE_FLOAT> , ::boost::math::quaternion<TYPE_FLOAT> > const & pq)

    40 {

    41     using    ::std::numeric_limits;

    42     

    43     TYPE_FLOAT    a0 = pq.first.R_component_1();

    44     TYPE_FLOAT    b0 = pq.first.R_component_2();

    45     TYPE_FLOAT    c0 = pq.first.R_component_3();

    46     TYPE_FLOAT    d0 = pq.first.R_component_4();

    47     

    48     TYPE_FLOAT    norme_carre0 = a0*a0+b0*b0+c0*c0+d0*d0;

    49     

    50     if    (norme_carre0 <= numeric_limits<TYPE_FLOAT>::epsilon())

    51     {

    52         ::std::string            error_reporting("Argument to quaternions_to_R4_rotation is too small!");

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

    54         

    55         throw(bad_argument);

    56     }

    57     

    58     TYPE_FLOAT    a1 = pq.second.R_component_1();

    59     TYPE_FLOAT    b1 = pq.second.R_component_2();

    60     TYPE_FLOAT    c1 = pq.second.R_component_3();

    61     TYPE_FLOAT    d1 = pq.second.R_component_4();

    62     

    63     TYPE_FLOAT    norme_carre1 = a1*a1+b1*b1+c1*c1+d1*d1;

    64     

    65     if    (norme_carre1 <= numeric_limits<TYPE_FLOAT>::epsilon())

    66     {

    67         ::std::string            error_reporting("Argument to quaternions_to_R4_rotation is too small!");

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

    69         

    70         throw(bad_argument);

    71     }

    72     

    73     TYPE_FLOAT    prod_norm = norme_carre0*norme_carre1;

    74     

    75     TYPE_FLOAT    a0a1 = a0*a1;

    76     TYPE_FLOAT    a0b1 = a0*b1;

    77     TYPE_FLOAT    a0c1 = a0*c1;

    78     TYPE_FLOAT    a0d1 = a0*d1;

    79     TYPE_FLOAT    b0a1 = b0*a1;

    80     TYPE_FLOAT    b0b1 = b0*b1;

    81     TYPE_FLOAT    b0c1 = b0*c1;

    82     TYPE_FLOAT    b0d1 = b0*d1;

    83     TYPE_FLOAT    c0a1 = c0*a1;

    84     TYPE_FLOAT    c0b1 = c0*b1;

    85     TYPE_FLOAT    c0c1 = c0*c1;

    86     TYPE_FLOAT    c0d1 = c0*d1;

    87     TYPE_FLOAT    d0a1 = d0*a1;

    88     TYPE_FLOAT    d0b1 = d0*b1;

    89     TYPE_FLOAT    d0c1 = d0*c1;

    90     TYPE_FLOAT    d0d1 = d0*d1;

    91     

    92     R4_matrix<TYPE_FLOAT>    out_matrix;

    93     

    94     out_matrix.a11 = (+a0a1+b0b1+c0c1+d0d1)/prod_norm;

    95     out_matrix.a12 = (+a0b1-b0a1-c0d1+d0c1)/prod_norm;

    96     out_matrix.a13 = (+a0c1+b0d1-c0a1-d0b1)/prod_norm;

    97     out_matrix.a14 = (+a0d1-b0c1+c0b1-d0a1)/prod_norm;

    98     out_matrix.a21 = (-a0b1+b0a1-c0d1+d0c1)/prod_norm;

    99     out_matrix.a22 = (+a0a1+b0b1-c0c1-d0d1)/prod_norm;

   100     out_matrix.a23 = (-a0d1+b0c1+c0b1-d0a1)/prod_norm;

   101     out_matrix.a24 = (+a0c1+b0d1+c0a1+d0b1)/prod_norm;

   102     out_matrix.a31 = (-a0c1+b0d1+c0a1-d0b1)/prod_norm;

   103     out_matrix.a32 = (+a0d1+b0c1+c0b1+d0a1)/prod_norm;

   104     out_matrix.a33 = (+a0a1-b0b1+c0c1-d0d1)/prod_norm;

   105     out_matrix.a34 = (-a0b1-b0a1+c0d1+d0c1)/prod_norm;

   106     out_matrix.a41 = (-a0d1-b0c1+c0b1+d0a1)/prod_norm;

   107     out_matrix.a42 = (-a0c1+b0d1-c0a1+d0b1)/prod_norm;

   108     out_matrix.a43 = (+a0b1+b0a1+c0d1+d0c1)/prod_norm;

   109     out_matrix.a44 = (+a0a1-b0b1-c0c1+d0d1)/prod_norm;

   110     

   111     return(out_matrix);

   112 }

   113 

   114 

   115 template<typename TYPE_FLOAT>

   116 inline bool                        is_R4_rotation_matrix(R4_matrix<TYPE_FLOAT> const & mat)

   117 {

   118     using    ::std::abs;

   119     

   120     using    ::std::numeric_limits;

   121     

   122     return    (

   123                 !(

   124                     (abs(mat.a11*mat.a11+mat.a21*mat.a21+mat.a31*mat.a31+mat.a41*mat.a41 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   125                     (abs(mat.a11*mat.a12+mat.a21*mat.a22+mat.a31*mat.a32+mat.a41*mat.a42 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   126                     (abs(mat.a11*mat.a13+mat.a21*mat.a23+mat.a31*mat.a33+mat.a41*mat.a43 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   127                     (abs(mat.a11*mat.a14+mat.a21*mat.a24+mat.a31*mat.a34+mat.a41*mat.a44 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   128                     //(abs(mat.a11*mat.a12+mat.a21*mat.a22+mat.a31*mat.a32+mat.a41*mat.a42 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   129                     (abs(mat.a12*mat.a12+mat.a22*mat.a22+mat.a32*mat.a32+mat.a42*mat.a42 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   130                     (abs(mat.a12*mat.a13+mat.a22*mat.a23+mat.a32*mat.a33+mat.a42*mat.a43 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   131                     (abs(mat.a12*mat.a14+mat.a22*mat.a24+mat.a32*mat.a34+mat.a42*mat.a44 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   132                     //(abs(mat.a11*mat.a13+mat.a21*mat.a23+mat.a31*mat.a33+mat.a41*mat.a43 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   133                     //(abs(mat.a12*mat.a13+mat.a22*mat.a23+mat.a32*mat.a33+mat.a42*mat.a43 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   134                     (abs(mat.a13*mat.a13+mat.a23*mat.a23+mat.a33*mat.a33+mat.a43*mat.a43 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   135                     (abs(mat.a13*mat.a14+mat.a23*mat.a24+mat.a33*mat.a34+mat.a43*mat.a44 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   136                     //(abs(mat.a11*mat.a14+mat.a21*mat.a24+mat.a31*mat.a34+mat.a41*mat.a44 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   137                     //(abs(mat.a12*mat.a14+mat.a22*mat.a24+mat.a32*mat.a34+mat.a42*mat.a44 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   138                     //(abs(mat.a13*mat.a14+mat.a23*mat.a24+mat.a33*mat.a34+mat.a43*mat.a44 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||

   139                     (abs(mat.a14*mat.a14+mat.a24*mat.a24+mat.a34*mat.a34+mat.a44*mat.a44 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())

   140                 )

   141             );

   142 }

   143 

   144 

   145 template<typename TYPE_FLOAT>

   146 ::std::pair< ::boost::math::quaternion<TYPE_FLOAT> , ::boost::math::quaternion<TYPE_FLOAT> >    R4_rotation_to_quaternions(    R4_matrix<TYPE_FLOAT> const & rot,

   147                                                                                                                             ::std::pair< ::boost::math::quaternion<TYPE_FLOAT> , ::boost::math::quaternion<TYPE_FLOAT> > const * hint = 0)

   148 {

   149     if    (!is_R4_rotation_matrix(rot))

   150     {

   151         ::std::string        error_reporting("Argument to R4_rotation_to_quaternions is not an R^4 rotation matrix!");

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

   153         

   154         throw(bad_argument);

   155     }

   156     

   157     R3_matrix<TYPE_FLOAT>    mat;

   158     

   159     mat.a11 = -rot.a31*rot.a42+rot.a32*rot.a41+rot.a22*rot.a11-rot.a21*rot.a12;

   160     mat.a12 = -rot.a31*rot.a43+rot.a33*rot.a41+rot.a23*rot.a11-rot.a21*rot.a13;

   161     mat.a13 = -rot.a31*rot.a44+rot.a34*rot.a41+rot.a24*rot.a11-rot.a21*rot.a14;

   162     mat.a21 = -rot.a31*rot.a12-rot.a22*rot.a41+rot.a32*rot.a11+rot.a21*rot.a42;

   163     mat.a22 = -rot.a31*rot.a13-rot.a23*rot.a41+rot.a33*rot.a11+rot.a21*rot.a43;

   164     mat.a23 = -rot.a31*rot.a14-rot.a24*rot.a41+rot.a34*rot.a11+rot.a21*rot.a44;

   165     mat.a31 = +rot.a31*rot.a22-rot.a12*rot.a41+rot.a42*rot.a11-rot.a21*rot.a32;

   166     mat.a32 = +rot.a31*rot.a23-rot.a13*rot.a41+rot.a43*rot.a11-rot.a21*rot.a33;

   167     mat.a33 = +rot.a31*rot.a24-rot.a14*rot.a41+rot.a44*rot.a11-rot.a21*rot.a34;

   168     

   169     ::boost::math::quaternion<TYPE_FLOAT>    q = R3_rotation_to_quaternion(mat);

   170     

   171     ::boost::math::quaternion<TYPE_FLOAT>    p =

   172         ::boost::math::quaternion<TYPE_FLOAT>(rot.a11,rot.a12,rot.a13,rot.a14)*q;

   173     

   174     if    ((hint != 0) && (abs(hint->second+q) < abs(hint->second-q)))

   175     {

   176         return(::std::make_pair(-p,-q));

   177     }

   178     

   179     return(::std::make_pair(p,q));

   180 }

   181 

   182 #endif /* TEST_HSO4_HPP */

   183
Symaptic