hso3.hpp

C++的一个好库。。。现在很流行

/********************************************************************************************/
/*                                                                                          */
/*                                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 usefull 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);
}


namespace
{
    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,