Esempio n. 1
0
 static type value(const boost::math::quaternion<quantity<Unit,Y> >& x)  
 { 
     const boost::math::quaternion<value_type>   tmp = 
         root<static_rational<N,D> >(boost::math::quaternion<Y>(
             x.R_component_1().value(),
             x.R_component_2().value(),
             x.R_component_3().value(),
             x.R_component_4().value()));
     
     return type(quantity_type::from_value(tmp.R_component_1()),
                 quantity_type::from_value(tmp.R_component_2()),
                 quantity_type::from_value(tmp.R_component_3()),
                 quantity_type::from_value(tmp.R_component_4()));
 }
Esempio n. 2
0
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);
}
Esempio n. 3
0
                linalg::matrix<T, 3, 3, Layout> to_matrix(boost::math::quaternion<T> const & q)
                {
                    return linalg::matrix<T, 3, 3, Layout>(
                            1.0-2*(q.R_component_3()*q.R_component_3())-2*(q.R_component_4()*q.R_component_4()),
                                2*(q.R_component_2()*q.R_component_3())-2*(q.R_component_1()*q.R_component_4()),
                                2*(q.R_component_2()*q.R_component_4())+2*(q.R_component_1()*q.R_component_3()),

                                2*(q.R_component_2()*q.R_component_3())+2*(q.R_component_1()*q.R_component_4()),
                            1.0-2*(q.R_component_2()*q.R_component_2())-2*(q.R_component_4()*q.R_component_4()),
                                2*(q.R_component_3()*q.R_component_4())-2*(q.R_component_1()*q.R_component_2()),

                                2*(q.R_component_2()*q.R_component_4())-2*(q.R_component_1()*q.R_component_3()),
                                2*(q.R_component_3()*q.R_component_4())+2*(q.R_component_1()*q.R_component_2()),
                            1.0-2*(q.R_component_2()*q.R_component_2())-2*(q.R_component_3()*q.R_component_3()));
                }