|
sl@0
|
1 |
|
|
sl@0
|
2 |
/********************************************************************************************/
|
|
sl@0
|
3 |
/* */
|
|
sl@0
|
4 |
/* HSO4.hpp header file */
|
|
sl@0
|
5 |
/* */
|
|
sl@0
|
6 |
/* This file is not currently part of the Boost library. It is simply an example of the use */
|
|
sl@0
|
7 |
/* quaternions can be put to. Hopefully it will be usefull too. */
|
|
sl@0
|
8 |
/* */
|
|
sl@0
|
9 |
/* This file provides tools to convert between quaternions and R^4 rotation matrices. */
|
|
sl@0
|
10 |
/* */
|
|
sl@0
|
11 |
/********************************************************************************************/
|
|
sl@0
|
12 |
|
|
sl@0
|
13 |
// (C) Copyright Hubert Holin 2001.
|
|
sl@0
|
14 |
// Distributed under the Boost Software License, Version 1.0. (See
|
|
sl@0
|
15 |
// accompanying file LICENSE_1_0.txt or copy at
|
|
sl@0
|
16 |
// http://www.boost.org/LICENSE_1_0.txt)
|
|
sl@0
|
17 |
|
|
sl@0
|
18 |
#ifndef TEST_HSO4_HPP
|
|
sl@0
|
19 |
#define TEST_HSO4_HPP
|
|
sl@0
|
20 |
|
|
sl@0
|
21 |
#include <utility>
|
|
sl@0
|
22 |
|
|
sl@0
|
23 |
#include "HSO3.hpp"
|
|
sl@0
|
24 |
|
|
sl@0
|
25 |
|
|
sl@0
|
26 |
template<typename TYPE_FLOAT>
|
|
sl@0
|
27 |
struct R4_matrix
|
|
sl@0
|
28 |
{
|
|
sl@0
|
29 |
TYPE_FLOAT a11, a12, a13, a14;
|
|
sl@0
|
30 |
TYPE_FLOAT a21, a22, a23, a24;
|
|
sl@0
|
31 |
TYPE_FLOAT a31, a32, a33, a34;
|
|
sl@0
|
32 |
TYPE_FLOAT a41, a42, a43, a44;
|
|
sl@0
|
33 |
};
|
|
sl@0
|
34 |
|
|
sl@0
|
35 |
|
|
sl@0
|
36 |
// Note: the input quaternions need not be of norm 1 for the following function
|
|
sl@0
|
37 |
|
|
sl@0
|
38 |
template<typename TYPE_FLOAT>
|
|
sl@0
|
39 |
R4_matrix<TYPE_FLOAT> quaternions_to_R4_rotation(::std::pair< ::boost::math::quaternion<TYPE_FLOAT> , ::boost::math::quaternion<TYPE_FLOAT> > const & pq)
|
|
sl@0
|
40 |
{
|
|
sl@0
|
41 |
using ::std::numeric_limits;
|
|
sl@0
|
42 |
|
|
sl@0
|
43 |
TYPE_FLOAT a0 = pq.first.R_component_1();
|
|
sl@0
|
44 |
TYPE_FLOAT b0 = pq.first.R_component_2();
|
|
sl@0
|
45 |
TYPE_FLOAT c0 = pq.first.R_component_3();
|
|
sl@0
|
46 |
TYPE_FLOAT d0 = pq.first.R_component_4();
|
|
sl@0
|
47 |
|
|
sl@0
|
48 |
TYPE_FLOAT norme_carre0 = a0*a0+b0*b0+c0*c0+d0*d0;
|
|
sl@0
|
49 |
|
|
sl@0
|
50 |
if (norme_carre0 <= numeric_limits<TYPE_FLOAT>::epsilon())
|
|
sl@0
|
51 |
{
|
|
sl@0
|
52 |
::std::string error_reporting("Argument to quaternions_to_R4_rotation is too small!");
|
|
sl@0
|
53 |
::std::underflow_error bad_argument(error_reporting);
|
|
sl@0
|
54 |
|
|
sl@0
|
55 |
throw(bad_argument);
|
|
sl@0
|
56 |
}
|
|
sl@0
|
57 |
|
|
sl@0
|
58 |
TYPE_FLOAT a1 = pq.second.R_component_1();
|
|
sl@0
|
59 |
TYPE_FLOAT b1 = pq.second.R_component_2();
|
|
sl@0
|
60 |
TYPE_FLOAT c1 = pq.second.R_component_3();
|
|
sl@0
|
61 |
TYPE_FLOAT d1 = pq.second.R_component_4();
|
|
sl@0
|
62 |
|
|
sl@0
|
63 |
TYPE_FLOAT norme_carre1 = a1*a1+b1*b1+c1*c1+d1*d1;
|
|
sl@0
|
64 |
|
|
sl@0
|
65 |
if (norme_carre1 <= numeric_limits<TYPE_FLOAT>::epsilon())
|
|
sl@0
|
66 |
{
|
|
sl@0
|
67 |
::std::string error_reporting("Argument to quaternions_to_R4_rotation is too small!");
|
|
sl@0
|
68 |
::std::underflow_error bad_argument(error_reporting);
|
|
sl@0
|
69 |
|
|
sl@0
|
70 |
throw(bad_argument);
|
|
sl@0
|
71 |
}
|
|
sl@0
|
72 |
|
|
sl@0
|
73 |
TYPE_FLOAT prod_norm = norme_carre0*norme_carre1;
|
|
sl@0
|
74 |
|
|
sl@0
|
75 |
TYPE_FLOAT a0a1 = a0*a1;
|
|
sl@0
|
76 |
TYPE_FLOAT a0b1 = a0*b1;
|
|
sl@0
|
77 |
TYPE_FLOAT a0c1 = a0*c1;
|
|
sl@0
|
78 |
TYPE_FLOAT a0d1 = a0*d1;
|
|
sl@0
|
79 |
TYPE_FLOAT b0a1 = b0*a1;
|
|
sl@0
|
80 |
TYPE_FLOAT b0b1 = b0*b1;
|
|
sl@0
|
81 |
TYPE_FLOAT b0c1 = b0*c1;
|
|
sl@0
|
82 |
TYPE_FLOAT b0d1 = b0*d1;
|
|
sl@0
|
83 |
TYPE_FLOAT c0a1 = c0*a1;
|
|
sl@0
|
84 |
TYPE_FLOAT c0b1 = c0*b1;
|
|
sl@0
|
85 |
TYPE_FLOAT c0c1 = c0*c1;
|
|
sl@0
|
86 |
TYPE_FLOAT c0d1 = c0*d1;
|
|
sl@0
|
87 |
TYPE_FLOAT d0a1 = d0*a1;
|
|
sl@0
|
88 |
TYPE_FLOAT d0b1 = d0*b1;
|
|
sl@0
|
89 |
TYPE_FLOAT d0c1 = d0*c1;
|
|
sl@0
|
90 |
TYPE_FLOAT d0d1 = d0*d1;
|
|
sl@0
|
91 |
|
|
sl@0
|
92 |
R4_matrix<TYPE_FLOAT> out_matrix;
|
|
sl@0
|
93 |
|
|
sl@0
|
94 |
out_matrix.a11 = (+a0a1+b0b1+c0c1+d0d1)/prod_norm;
|
|
sl@0
|
95 |
out_matrix.a12 = (+a0b1-b0a1-c0d1+d0c1)/prod_norm;
|
|
sl@0
|
96 |
out_matrix.a13 = (+a0c1+b0d1-c0a1-d0b1)/prod_norm;
|
|
sl@0
|
97 |
out_matrix.a14 = (+a0d1-b0c1+c0b1-d0a1)/prod_norm;
|
|
sl@0
|
98 |
out_matrix.a21 = (-a0b1+b0a1-c0d1+d0c1)/prod_norm;
|
|
sl@0
|
99 |
out_matrix.a22 = (+a0a1+b0b1-c0c1-d0d1)/prod_norm;
|
|
sl@0
|
100 |
out_matrix.a23 = (-a0d1+b0c1+c0b1-d0a1)/prod_norm;
|
|
sl@0
|
101 |
out_matrix.a24 = (+a0c1+b0d1+c0a1+d0b1)/prod_norm;
|
|
sl@0
|
102 |
out_matrix.a31 = (-a0c1+b0d1+c0a1-d0b1)/prod_norm;
|
|
sl@0
|
103 |
out_matrix.a32 = (+a0d1+b0c1+c0b1+d0a1)/prod_norm;
|
|
sl@0
|
104 |
out_matrix.a33 = (+a0a1-b0b1+c0c1-d0d1)/prod_norm;
|
|
sl@0
|
105 |
out_matrix.a34 = (-a0b1-b0a1+c0d1+d0c1)/prod_norm;
|
|
sl@0
|
106 |
out_matrix.a41 = (-a0d1-b0c1+c0b1+d0a1)/prod_norm;
|
|
sl@0
|
107 |
out_matrix.a42 = (-a0c1+b0d1-c0a1+d0b1)/prod_norm;
|
|
sl@0
|
108 |
out_matrix.a43 = (+a0b1+b0a1+c0d1+d0c1)/prod_norm;
|
|
sl@0
|
109 |
out_matrix.a44 = (+a0a1-b0b1-c0c1+d0d1)/prod_norm;
|
|
sl@0
|
110 |
|
|
sl@0
|
111 |
return(out_matrix);
|
|
sl@0
|
112 |
}
|
|
sl@0
|
113 |
|
|
sl@0
|
114 |
|
|
sl@0
|
115 |
template<typename TYPE_FLOAT>
|
|
sl@0
|
116 |
inline bool is_R4_rotation_matrix(R4_matrix<TYPE_FLOAT> const & mat)
|
|
sl@0
|
117 |
{
|
|
sl@0
|
118 |
using ::std::abs;
|
|
sl@0
|
119 |
|
|
sl@0
|
120 |
using ::std::numeric_limits;
|
|
sl@0
|
121 |
|
|
sl@0
|
122 |
return (
|
|
sl@0
|
123 |
!(
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())||
|
|
sl@0
|
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())
|
|
sl@0
|
140 |
)
|
|
sl@0
|
141 |
);
|
|
sl@0
|
142 |
}
|
|
sl@0
|
143 |
|
|
sl@0
|
144 |
|
|
sl@0
|
145 |
template<typename TYPE_FLOAT>
|
|
sl@0
|
146 |
::std::pair< ::boost::math::quaternion<TYPE_FLOAT> , ::boost::math::quaternion<TYPE_FLOAT> > R4_rotation_to_quaternions( R4_matrix<TYPE_FLOAT> const & rot,
|
|
sl@0
|
147 |
::std::pair< ::boost::math::quaternion<TYPE_FLOAT> , ::boost::math::quaternion<TYPE_FLOAT> > const * hint = 0)
|
|
sl@0
|
148 |
{
|
|
sl@0
|
149 |
if (!is_R4_rotation_matrix(rot))
|
|
sl@0
|
150 |
{
|
|
sl@0
|
151 |
::std::string error_reporting("Argument to R4_rotation_to_quaternions is not an R^4 rotation matrix!");
|
|
sl@0
|
152 |
::std::range_error bad_argument(error_reporting);
|
|
sl@0
|
153 |
|
|
sl@0
|
154 |
throw(bad_argument);
|
|
sl@0
|
155 |
}
|
|
sl@0
|
156 |
|
|
sl@0
|
157 |
R3_matrix<TYPE_FLOAT> mat;
|
|
sl@0
|
158 |
|
|
sl@0
|
159 |
mat.a11 = -rot.a31*rot.a42+rot.a32*rot.a41+rot.a22*rot.a11-rot.a21*rot.a12;
|
|
sl@0
|
160 |
mat.a12 = -rot.a31*rot.a43+rot.a33*rot.a41+rot.a23*rot.a11-rot.a21*rot.a13;
|
|
sl@0
|
161 |
mat.a13 = -rot.a31*rot.a44+rot.a34*rot.a41+rot.a24*rot.a11-rot.a21*rot.a14;
|
|
sl@0
|
162 |
mat.a21 = -rot.a31*rot.a12-rot.a22*rot.a41+rot.a32*rot.a11+rot.a21*rot.a42;
|
|
sl@0
|
163 |
mat.a22 = -rot.a31*rot.a13-rot.a23*rot.a41+rot.a33*rot.a11+rot.a21*rot.a43;
|
|
sl@0
|
164 |
mat.a23 = -rot.a31*rot.a14-rot.a24*rot.a41+rot.a34*rot.a11+rot.a21*rot.a44;
|
|
sl@0
|
165 |
mat.a31 = +rot.a31*rot.a22-rot.a12*rot.a41+rot.a42*rot.a11-rot.a21*rot.a32;
|
|
sl@0
|
166 |
mat.a32 = +rot.a31*rot.a23-rot.a13*rot.a41+rot.a43*rot.a11-rot.a21*rot.a33;
|
|
sl@0
|
167 |
mat.a33 = +rot.a31*rot.a24-rot.a14*rot.a41+rot.a44*rot.a11-rot.a21*rot.a34;
|
|
sl@0
|
168 |
|
|
sl@0
|
169 |
::boost::math::quaternion<TYPE_FLOAT> q = R3_rotation_to_quaternion(mat);
|
|
sl@0
|
170 |
|
|
sl@0
|
171 |
::boost::math::quaternion<TYPE_FLOAT> p =
|
|
sl@0
|
172 |
::boost::math::quaternion<TYPE_FLOAT>(rot.a11,rot.a12,rot.a13,rot.a14)*q;
|
|
sl@0
|
173 |
|
|
sl@0
|
174 |
if ((hint != 0) && (abs(hint->second+q) < abs(hint->second-q)))
|
|
sl@0
|
175 |
{
|
|
sl@0
|
176 |
return(::std::make_pair(-p,-q));
|
|
sl@0
|
177 |
}
|
|
sl@0
|
178 |
|
|
sl@0
|
179 |
return(::std::make_pair(p,q));
|
|
sl@0
|
180 |
}
|
|
sl@0
|
181 |
|
|
sl@0
|
182 |
#endif /* TEST_HSO4_HPP */
|
|
sl@0
|
183 |
|