Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: downloads/boost_1_34_1/libs/math/quaternion/HSO3.hpp @ 35

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

updated boost from 1_33_1 to 1_34_1
File size: 16.9 KB
Line 
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:
37using    namespace ::std;
38using    namespace ::boost::math;
39#endif
40
41template<typename TYPE_FLOAT>
42struct  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
52template<typename TYPE_FLOAT>
53R3_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>
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>
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>
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
394template<typename TYPE_FLOAT>
395inline 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
417template<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;
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
Note: See TracBrowser for help on using the repository browser.