[ceph.git] / ceph / src / boost / libs / math / example / HSO3.hpp


/********************************************************************************************/
/*                                                                                          */
/*                                HSO3.hpp header file                                      */
/*                                                                                          */
/* This file is not currently part of the Boost library. It is simply an example of the use */
/* quaternions can be put to. Hopefully it will be useful too.                              */
/*                                                                                          */
/* This file provides tools to convert between quaternions and R^3 rotation matrices.       */
/*                                                                                          */
/********************************************************************************************/

//  (C) Copyright Hubert Holin 2001.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#ifndef TEST_HSO3_HPP
#define TEST_HSO3_HPP

#include <algorithm>

#if    defined(__GNUC__) && (__GNUC__ < 3)
#include <boost/limits.hpp>
#else
#include <limits>
#endif

#include <stdexcept>
#include <string>

#include <boost/math/quaternion.hpp>


#if    defined(__GNUC__) && (__GNUC__ < 3)
// gcc 2.x ignores function scope using declarations, put them here instead:
using    namespace ::std;
using    namespace ::boost::math;
#endif

template<typename TYPE_FLOAT>
struct  R3_matrix
{
    TYPE_FLOAT a11, a12, a13;
    TYPE_FLOAT a21, a22, a23;
    TYPE_FLOAT a31, a32, a33;
};


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

template<typename TYPE_FLOAT>
R3_matrix<TYPE_FLOAT>    quaternion_to_R3_rotation(::boost::math::quaternion<TYPE_FLOAT> const & q)
{
    using    ::std::numeric_limits;
    
    TYPE_FLOAT    a = q.R_component_1();
    TYPE_FLOAT    b = q.R_component_2();
    TYPE_FLOAT    c = q.R_component_3();
    TYPE_FLOAT    d = q.R_component_4();
    
    TYPE_FLOAT    aa = a*a;
    TYPE_FLOAT    ab = a*b;
    TYPE_FLOAT    ac = a*c;
    TYPE_FLOAT    ad = a*d;
    TYPE_FLOAT    bb = b*b;
    TYPE_FLOAT    bc = b*c;
    TYPE_FLOAT    bd = b*d;
    TYPE_FLOAT    cc = c*c;
    TYPE_FLOAT    cd = c*d;
    TYPE_FLOAT    dd = d*d;
    
    TYPE_FLOAT    norme_carre = aa+bb+cc+dd;
    
    if    (norme_carre <= numeric_limits<TYPE_FLOAT>::epsilon())
    {
        ::std::string            error_reporting("Argument to quaternion_to_R3_rotation is too small!");
        ::std::underflow_error   bad_argument(error_reporting);
        
        throw(bad_argument);
    }
    
    R3_matrix<TYPE_FLOAT>    out_matrix;
    
    out_matrix.a11 = (aa+bb-cc-dd)/norme_carre;
    out_matrix.a12 = 2*(-ad+bc)/norme_carre;
    out_matrix.a13 = 2*(ac+bd)/norme_carre;
    out_matrix.a21 = 2*(ad+bc)/norme_carre;
    out_matrix.a22 = (aa-bb+cc-dd)/norme_carre;
    out_matrix.a23 = 2*(-ab+cd)/norme_carre;
    out_matrix.a31 = 2*(-ac+bd)/norme_carre;
    out_matrix.a32 = 2*(ab+cd)/norme_carre;
    out_matrix.a33 = (aa-bb-cc+dd)/norme_carre;
    
    return(out_matrix);
}


    template<typename TYPE_FLOAT>
    void    find_invariant_vector(  R3_matrix<TYPE_FLOAT> const & rot,
                                    TYPE_FLOAT & x,
                                    TYPE_FLOAT & y,
                                    TYPE_FLOAT & z)
    {
        using    ::std::sqrt;
        
        using    ::std::numeric_limits;
        
        TYPE_FLOAT    b11 = rot.a11 - static_cast<TYPE_FLOAT>(1);
        TYPE_FLOAT    b12 = rot.a12;
        TYPE_FLOAT    b13 = rot.a13;
        TYPE_FLOAT    b21 = rot.a21;
        TYPE_FLOAT    b22 = rot.a22 - static_cast<TYPE_FLOAT>(1);
        TYPE_FLOAT    b23 = rot.a23;
        TYPE_FLOAT    b31 = rot.a31;
        TYPE_FLOAT    b32 = rot.a32;
        TYPE_FLOAT    b33 = rot.a33 - static_cast<TYPE_FLOAT>(1);
        
        TYPE_FLOAT    minors[9] =
        {
            b11*b22-b12*b21,
            b11*b23-b13*b21,
            b12*b23-b13*b22,
            b11*b32-b12*b31,
            b11*b33-b13*b31,
            b12*b33-b13*b32,
            b21*b32-b22*b31,
            b21*b33-b23*b31,
            b22*b33-b23*b32
        };
        
        TYPE_FLOAT *        where = ::std::max_element(minors, minors+9);
        
        TYPE_FLOAT          det = *where;
        
        if    (det <= numeric_limits<TYPE_FLOAT>::epsilon())
        {
            ::std::string            error_reporting("Underflow error in find_invariant_vector!");
            ::std::underflow_error   processing_error(error_reporting);
            
            throw(processing_error);
        }
        
        switch    (where-minors)
        {
            case 0:
                
                z = static_cast<TYPE_FLOAT>(1);
                
                x = (-b13*b22+b12*b23)/det;
                y = (-b11*b23+b13*b21)/det;
                
                break;
                
            case 1:
                
                y = static_cast<TYPE_FLOAT>(1);
                
                x = (-b12*b23+b13*b22)/det;
                z = (-b11*b22+b12*b21)/det;
                
                break;
                
            case 2:
                
                x = static_cast<TYPE_FLOAT>(1);
                
                y = (-b11*b23+b13*b21)/det;
                z = (-b12*b21+b11*b22)/det;
                
                break;
                
            case 3:
                
                z = static_cast<TYPE_FLOAT>(1);
                
                x = (-b13*b32+b12*b33)/det;
                y = (-b11*b33+b13*b31)/det;
                
                break;
                
            case 4:
                
                y = static_cast<TYPE_FLOAT>(1);
                
                x = (-b12*b33+b13*b32)/det;
                z = (-b11*b32+b12*b31)/det;
                
                break;
                
            case 5:
                
                x = static_cast<TYPE_FLOAT>(1);
                
                y = (-b11*b33+b13*b31)/det;
                z = (-b12*b31+b11*b32)/det;
                
                break;
                
            case 6:
                
                z = static_cast<TYPE_FLOAT>(1);
                
                x = (-b23*b32+b22*b33)/det;
                y = (-b21*b33+b23*b31)/det;
                
                break;
                
            case 7:
                
                y = static_cast<TYPE_FLOAT>(1);
                
                x = (-b22*b33+b23*b32)/det;
                z = (-b21*b32+b22*b31)/det;
                
                break;
                
            case 8:
                
                x = static_cast<TYPE_FLOAT>(1);
                
                y = (-b21*b33+b23*b31)/det;
                z = (-b22*b31+b21*b32)/det;
                
                break;
                
            default:
                
                ::std::string        error_reporting("Impossible condition in find_invariant_vector");
                ::std::logic_error   processing_error(error_reporting);
                
                throw(processing_error);
                
                break;
        }
        
        TYPE_FLOAT    vecnorm = sqrt(x*x+y*y+z*z);
        
        if    (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon())
        {
            ::std::string            error_reporting("Overflow error in find_invariant_vector!");
            ::std::overflow_error    processing_error(error_reporting);
            
            throw(processing_error);
        }
        
        x /= vecnorm;
        y /= vecnorm;
        z /= vecnorm;
    }
    
    
    template<typename TYPE_FLOAT>
    void    find_orthogonal_vector( TYPE_FLOAT x,
                                    TYPE_FLOAT y,
                                    TYPE_FLOAT z,
                                    TYPE_FLOAT & u,
                                    TYPE_FLOAT & v,
                                    TYPE_FLOAT & w)
    {
        using    ::std::abs;
        using    ::std::sqrt;
        
        using    ::std::numeric_limits;
        
        TYPE_FLOAT    vecnormsqr = x*x+y*y+z*z;
        
        if    (vecnormsqr <= numeric_limits<TYPE_FLOAT>::epsilon())
        {
            ::std::string            error_reporting("Underflow error in find_orthogonal_vector!");
            ::std::underflow_error   processing_error(error_reporting);
            
            throw(processing_error);
        }
        
        TYPE_FLOAT        lambda;
        
        TYPE_FLOAT        components[3] =
        {
            abs(x),
            abs(y),
            abs(z)
        };
        
        TYPE_FLOAT *    where = ::std::min_element(components, components+3);
        
        switch    (where-components)
        {
            case 0:
                
                if    (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
                {
                    v =
                    w = static_cast<TYPE_FLOAT>(0);
                    u = static_cast<TYPE_FLOAT>(1);
                }
                else
                {
                    lambda = -x/vecnormsqr;
                    
                    u = static_cast<TYPE_FLOAT>(1) + lambda*x;
                    v = lambda*y;
                    w = lambda*z;
                }
                
                break;
                
            case 1:
                
                if    (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
                {
                    u =
                    w = static_cast<TYPE_FLOAT>(0);
                    v = static_cast<TYPE_FLOAT>(1);
                }
                else
                {
                    lambda = -y/vecnormsqr;
                    
                    u = lambda*x;
                    v = static_cast<TYPE_FLOAT>(1) + lambda*y;
                    w = lambda*z;
                }
                
                break;
                
            case 2:
                
                if    (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
                {
                    u =
                    v = static_cast<TYPE_FLOAT>(0);
                    w = static_cast<TYPE_FLOAT>(1);
                }
                else
                {
                    lambda = -z/vecnormsqr;
                    
                    u = lambda*x;
                    v = lambda*y;
                    w = static_cast<TYPE_FLOAT>(1) + lambda*z;
                }
                
                break;
                
            default:
                
                ::std::string        error_reporting("Impossible condition in find_invariant_vector");
                ::std::logic_error   processing_error(error_reporting);
                
                throw(processing_error);
                
                break;
        }
        
        TYPE_FLOAT    vecnorm = sqrt(u*u+v*v+w*w);
        
        if    (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon())
        {
            ::std::string            error_reporting("Underflow error in find_orthogonal_vector!");
            ::std::underflow_error   processing_error(error_reporting);
            
            throw(processing_error);
        }
        
        u /= vecnorm;
        v /= vecnorm;
        w /= vecnorm;
    }
    
    
    // Note:    we want [[v, v, w], [r, s, t], [x, y, z]] to be a direct orthogonal basis
    //            of R^3. It might not be orthonormal, however, and we do not check if the
    //            two input vectors are colinear or not.
    
    template<typename TYPE_FLOAT>
    void    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)
    {
        r = +y*w-z*v;
        s = -x*w+z*u;
        t = +x*v-y*u;
    }


template<typename TYPE_FLOAT>
inline bool                                is_R3_rotation_matrix(R3_matrix<TYPE_FLOAT> const & mat)
{
    using    ::std::abs;
    
    using    ::std::numeric_limits;
    
    return    (
                !(
                    (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())||
                    (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())||
                    (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())||
                    //(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())||
                    (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())||
                    (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())||
                    //(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())||
                    //(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())||
                    (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())
                )
            );
}


template<typename TYPE_FLOAT>
::boost::math::quaternion<TYPE_FLOAT>    R3_rotation_to_quaternion(    R3_matrix<TYPE_FLOAT> const & rot,
                                                                    ::boost::math::quaternion<TYPE_FLOAT> const * hint = 0)
{
    using    ::boost::math::abs;
    
    using    ::std::abs;
    using    ::std::sqrt;
    
    using    ::std::numeric_limits;
    
    if    (!is_R3_rotation_matrix(rot))
    {
        ::std::string        error_reporting("Argument to R3_rotation_to_quaternion is not an R^3 rotation matrix!");
        ::std::range_error   bad_argument(error_reporting);
        
        throw(bad_argument);
    }
    
    ::boost::math::quaternion<TYPE_FLOAT>    q;
    
    if    (
            (abs(rot.a11 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&&
            (abs(rot.a22 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&&
            (abs(rot.a33 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())
        )
    {
        q = ::boost::math::quaternion<TYPE_FLOAT>(1);
    }
    else
    {
        TYPE_FLOAT    cos_theta = (rot.a11+rot.a22+rot.a33-static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2);
        TYPE_FLOAT    stuff = (cos_theta+static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2);
        TYPE_FLOAT    cos_theta_sur_2 = sqrt(stuff);
        TYPE_FLOAT    sin_theta_sur_2 = sqrt(1-stuff);
        
        TYPE_FLOAT    x;
        TYPE_FLOAT    y;
        TYPE_FLOAT    z;
        
        find_invariant_vector(rot, x, y, z);
        
        TYPE_FLOAT    u;
        TYPE_FLOAT    v;
        TYPE_FLOAT    w;
        
        find_orthogonal_vector(x, y, z, u, v, w);
        
        TYPE_FLOAT    r;
        TYPE_FLOAT    s;
        TYPE_FLOAT    t;
        
        find_vector_for_BOD(x, y, z, u, v, w, r, s, t);
        
        TYPE_FLOAT    ru = rot.a11*u+rot.a12*v+rot.a13*w;
        TYPE_FLOAT    rv = rot.a21*u+rot.a22*v+rot.a23*w;
        TYPE_FLOAT    rw = rot.a31*u+rot.a32*v+rot.a33*w;
        
        TYPE_FLOAT    angle_sign_determinator = r*ru+s*rv+t*rw;
        
        if        (angle_sign_determinator > +numeric_limits<TYPE_FLOAT>::epsilon())
        {
            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);
        }
        else if    (angle_sign_determinator < -numeric_limits<TYPE_FLOAT>::epsilon())
        {
            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);
        }
        else
        {
            TYPE_FLOAT    desambiguator = u*ru+v*rv+w*rw;
            
            if    (desambiguator >= static_cast<TYPE_FLOAT>(1))
            {
                q = ::boost::math::quaternion<TYPE_FLOAT>(0, +x, +y, +z);
            }
            else
            {
                q = ::boost::math::quaternion<TYPE_FLOAT>(0, -x, -y, -z);
            }
        }
    }
    
    if    ((hint != 0) && (abs(*hint+q) < abs(*hint-q)))
    {
        return(-q);
    }
    
    return(q);
}

#endif /* TEST_HSO3_HPP */
Commit	Line	Data
7c673cae FG	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>
	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>
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	b11b22-b12b21,
122	b11b23-b13b21,
123	b12b23-b13b22,
124	b11b32-b12b31,
125	b11b33-b13b31,
126	b12b33-b13b32,
127	b21b32-b22b31,
128	b21b33-b23b31,
129	b22b33-b23b32
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 = (-b13b22+b12b23)/det;
151	y = (-b11b23+b13b21)/det;
152
153	break;
154
155	case 1:
156
157	y = static_cast<TYPE_FLOAT>(1);
158
159	x = (-b12b23+b13b22)/det;
160	z = (-b11b22+b12b21)/det;
161
162	break;
163
164	case 2:
165
166	x = static_cast<TYPE_FLOAT>(1);
167
168	y = (-b11b23+b13b21)/det;
169	z = (-b12b21+b11b22)/det;
170
171	break;
172
173	case 3:
174
175	z = static_cast<TYPE_FLOAT>(1);
176
177	x = (-b13b32+b12b33)/det;
178	y = (-b11b33+b13b31)/det;
179
180	break;
181
182	case 4:
183
184	y = static_cast<TYPE_FLOAT>(1);
185
186	x = (-b12b33+b13b32)/det;
187	z = (-b11b32+b12b31)/det;
188
189	break;
190
191	case 5:
192
193	x = static_cast<TYPE_FLOAT>(1);
194
195	y = (-b11b33+b13b31)/det;
196	z = (-b12b31+b11b32)/det;
197
198	break;
199
200	case 6:
201
202	z = static_cast<TYPE_FLOAT>(1);
203
204	x = (-b23b32+b22b33)/det;
205	y = (-b21b33+b23b31)/det;
206
207	break;
208
209	case 7:
210
211	y = static_cast<TYPE_FLOAT>(1);
212
213	x = (-b22b33+b23b32)/det;
214	z = (-b21b32+b22b31)/det;
215
216	break;
217
218	case 8:
219
220	x = static_cast<TYPE_FLOAT>(1);
221
222	y = (-b21b33+b23b31)/det;
223	z = (-b22b31+b21b32)/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(xx+yy+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 = xx+yy+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(uu+vv+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 = +yw-zv;
388	s = -xw+zu;
389	t = +xv-yu;
390	}
391
392
393
394	template<typename TYPE_FLOAT>
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.a11mat.a11+mat.a21mat.a21+mat.a31mat.a31 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)numeric_limits<TYPE_FLOAT>::epsilon())\|\|
404	(abs(mat.a11mat.a12+mat.a21mat.a22+mat.a31mat.a32 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)numeric_limits<TYPE_FLOAT>::epsilon())\|\|
405	(abs(mat.a11mat.a13+mat.a21mat.a23+mat.a31mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)numeric_limits<TYPE_FLOAT>::epsilon())\|\|
406	//(abs(mat.a11mat.a12+mat.a21mat.a22+mat.a31mat.a32 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)numeric_limits<TYPE_FLOAT>::epsilon())\|\|
407	(abs(mat.a12mat.a12+mat.a22mat.a22+mat.a32mat.a32 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)numeric_limits<TYPE_FLOAT>::epsilon())\|\|
408	(abs(mat.a12mat.a13+mat.a22mat.a23+mat.a32mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)numeric_limits<TYPE_FLOAT>::epsilon())\|\|
409	//(abs(mat.a11mat.a13+mat.a21mat.a23+mat.a31mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)numeric_limits<TYPE_FLOAT>::epsilon())\|\|
410	//(abs(mat.a12mat.a13+mat.a22mat.a23+mat.a32mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)numeric_limits<TYPE_FLOAT>::epsilon())\|\|
411	(abs(mat.a13mat.a13+mat.a23mat.a23+mat.a33mat.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>
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.a11u+rot.a12v+rot.a13*w;
472	TYPE_FLOAT rv = rot.a21u+rot.a22v+rot.a23*w;
473	TYPE_FLOAT rw = rot.a31u+rot.a32v+rot.a33*w;
474
475	TYPE_FLOAT angle_sign_determinator = rru+srv+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, +xsin_theta_sur_2, +ysin_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, -xsin_theta_sur_2, -ysin_theta_sur_2, -z*sin_theta_sur_2);
484	}
485	else
486	{
487	TYPE_FLOAT desambiguator = uru+vrv+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