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()));
}
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);
}
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()));
}
