Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: downloads/boost_1_34_1/libs/math/quaternion/HSO4.hpp @ 33

Last change on this file since 33 was 29, checked in by landauf, 16 years ago

updated boost from 1_33_1 to 1_34_1
File size: 9.4 KB
Line 
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
26template<typename TYPE_FLOAT>
27struct  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
38template<typename TYPE_FLOAT>
39R4_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
115template<typename TYPE_FLOAT>
116inline 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
145template<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
Note: See TracBrowser for help on using the repository browser.