1
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 /********************************************************************************************/
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_HSO3_HPP
19 #define TEST_HSO3_HPP
20
21 #include <algorithm>
22
23 #if defined(__GNUC__) && (__GNUC__ < 3)
24 #include <boost/limits.hpp>
25 #else
26 #include <limits>
27 #endif
28
29 #include <stdexcept>
30 #include <string>
31
32 #include <boost/math/quaternion.hpp>
33
34
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
40
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 };
48
49
50 // Note: the input quaternion need not be of norm 1 for the following function
51
52 template<typename TYPE_FLOAT>
quaternion_to_R3_rotation(::boost::math::quaternion<TYPE_FLOAT> const & q)53 R3_matrix<TYPE_FLOAT> quaternion_to_R3_rotation(::boost::math::quaternion<TYPE_FLOAT> const & q)
54 {
55 using ::std::numeric_limits;
56
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();
61
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;
72
73 TYPE_FLOAT norme_carre = aa+bb+cc+dd;
74
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);
79
80 throw(bad_argument);
81 }
82
83 R3_matrix<TYPE_FLOAT> out_matrix;
84
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;
94
95 return(out_matrix);
96 }
97
98
99 template<typename TYPE_FLOAT>
find_invariant_vector(R3_matrix<TYPE_FLOAT> const & rot,TYPE_FLOAT & x,TYPE_FLOAT & y,TYPE_FLOAT & z)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;
106
107 using ::std::numeric_limits;
108
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);
118
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 };
131
132 TYPE_FLOAT * where = ::std::max_element(minors, minors+9);
133
134 TYPE_FLOAT det = *where;
135
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);
140
141 throw(processing_error);
142 }
143
144 switch (where-minors)
145 {
146 case 0:
147
148 z = static_cast<TYPE_FLOAT>(1);
149
150 x = (-b13*b22+b12*b23)/det;
151 y = (-b11*b23+b13*b21)/det;
152
153 break;
154
155 case 1:
156
157 y = static_cast<TYPE_FLOAT>(1);
158
159 x = (-b12*b23+b13*b22)/det;
160 z = (-b11*b22+b12*b21)/det;
161
162 break;
163
164 case 2:
165
166 x = static_cast<TYPE_FLOAT>(1);
167
168 y = (-b11*b23+b13*b21)/det;
169 z = (-b12*b21+b11*b22)/det;
170
171 break;
172
173 case 3:
174
175 z = static_cast<TYPE_FLOAT>(1);
176
177 x = (-b13*b32+b12*b33)/det;
178 y = (-b11*b33+b13*b31)/det;
179
180 break;
181
182 case 4:
183
184 y = static_cast<TYPE_FLOAT>(1);
185
186 x = (-b12*b33+b13*b32)/det;
187 z = (-b11*b32+b12*b31)/det;
188
189 break;
190
191 case 5:
192
193 x = static_cast<TYPE_FLOAT>(1);
194
195 y = (-b11*b33+b13*b31)/det;
196 z = (-b12*b31+b11*b32)/det;
197
198 break;
199
200 case 6:
201
202 z = static_cast<TYPE_FLOAT>(1);
203
204 x = (-b23*b32+b22*b33)/det;
205 y = (-b21*b33+b23*b31)/det;
206
207 break;
208
209 case 7:
210
211 y = static_cast<TYPE_FLOAT>(1);
212
213 x = (-b22*b33+b23*b32)/det;
214 z = (-b21*b32+b22*b31)/det;
215
216 break;
217
218 case 8:
219
220 x = static_cast<TYPE_FLOAT>(1);
221
222 y = (-b21*b33+b23*b31)/det;
223 z = (-b22*b31+b21*b32)/det;
224
225 break;
226
227 default:
228
229 ::std::string error_reporting("Impossible condition in find_invariant_vector");
230 ::std::logic_error processing_error(error_reporting);
231
232 throw(processing_error);
233
234 break;
235 }
236
237 TYPE_FLOAT vecnorm = sqrt(x*x+y*y+z*z);
238
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);
243
244 throw(processing_error);
245 }
246
247 x /= vecnorm;
248 y /= vecnorm;
249 z /= vecnorm;
250 }
251
252
253 template<typename TYPE_FLOAT>
find_orthogonal_vector(TYPE_FLOAT x,TYPE_FLOAT y,TYPE_FLOAT z,TYPE_FLOAT & u,TYPE_FLOAT & v,TYPE_FLOAT & w)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;
263
264 using ::std::numeric_limits;
265
266 TYPE_FLOAT vecnormsqr = x*x+y*y+z*z;
267
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);
272
273 throw(processing_error);
274 }
275
276 TYPE_FLOAT lambda;
277
278 TYPE_FLOAT components[3] =
279 {
280 abs(x),
281 abs(y),
282 abs(z)
283 };
284
285 TYPE_FLOAT * where = ::std::min_element(components, components+3);
286
287 switch (where-components)
288 {
289 case 0:
290
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;
300
301 u = static_cast<TYPE_FLOAT>(1) + lambda*x;
302 v = lambda*y;
303 w = lambda*z;
304 }
305
306 break;
307
308 case 1:
309
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;
319
320 u = lambda*x;
321 v = static_cast<TYPE_FLOAT>(1) + lambda*y;
322 w = lambda*z;
323 }
324
325 break;
326
327 case 2:
328
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;
338
339 u = lambda*x;
340 v = lambda*y;
341 w = static_cast<TYPE_FLOAT>(1) + lambda*z;
342 }
343
344 break;
345
346 default:
347
348 ::std::string error_reporting("Impossible condition in find_invariant_vector");
349 ::std::logic_error processing_error(error_reporting);
350
351 throw(processing_error);
352
353 break;
354 }
355
356 TYPE_FLOAT vecnorm = sqrt(u*u+v*v+w*w);
357
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);
362
363 throw(processing_error);
364 }
365
366 u /= vecnorm;
367 v /= vecnorm;
368 w /= vecnorm;
369 }
370
371
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.
375
376 template<typename TYPE_FLOAT>
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)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 }
391
392
393
394 template<typename TYPE_FLOAT>
is_R3_rotation_matrix(R3_matrix<TYPE_FLOAT> const & mat)395 inline bool is_R3_rotation_matrix(R3_matrix<TYPE_FLOAT> const & mat)
396 {
397 using ::std::abs;
398
399 using ::std::numeric_limits;
400
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 }
415
416
417 template<typename TYPE_FLOAT>
R3_rotation_to_quaternion(R3_matrix<TYPE_FLOAT> const & rot,::boost::math::quaternion<TYPE_FLOAT> const * hint=0)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;
422
423 using ::std::abs;
424 using ::std::sqrt;
425
426 using ::std::numeric_limits;
427
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);
432
433 throw(bad_argument);
434 }
435
436 ::boost::math::quaternion<TYPE_FLOAT> q;
437
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);
452
453 TYPE_FLOAT x;
454 TYPE_FLOAT y;
455 TYPE_FLOAT z;
456
457 find_invariant_vector(rot, x, y, z);
458
459 TYPE_FLOAT u;
460 TYPE_FLOAT v;
461 TYPE_FLOAT w;
462
463 find_orthogonal_vector(x, y, z, u, v, w);
464
465 TYPE_FLOAT r;
466 TYPE_FLOAT s;
467 TYPE_FLOAT t;
468
469 find_vector_for_BOD(x, y, z, u, v, w, r, s, t);
470
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;
474
475 TYPE_FLOAT angle_sign_determinator = r*ru+s*rv+t*rw;
476
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;
488
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 }
499
500 if ((hint != 0) && (abs(*hint+q) < abs(*hint-q)))
501 {
502 return(-q);
503 }
504
505 return(q);
506 }
507
508 #endif /* TEST_HSO3_HPP */
509
510