hso3.hpp

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

                                    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 */