quaternion_s make_quaternion_rotation(vect_s axis, double angle)
{
double s = sin(angle/2);
double c = cos(angle/2);
return make_quaternion(
c,
s * axis.x,
s * axis.y,
s * axis.z);
}
quaternion_s quaternion_normalize(quaternion_s q)
{
double sqrmag = quaternion_sqrmag(q);
if(fabs(quaternion_sqrmag(q) - 1.0) > 0.000001)
{
double mag = sqrt(sqrmag);
return make_quaternion(q.w/mag, q.x/mag, q.y/mag, q.z/mag);
}
else
{
return q;
}
}
Quaternion make_quaternion(const float4x4 &rot)
{
return make_quaternion(make_matrix3x3(rot));
}
