private void SetAxisAngle(Vect3f axis, float angle)
{
axis.Normalize();
float norm = axis.Length();
if (norm < 1E-8)
{
q[0] = 0.0f;
q[1] = 0.0f;
q[2] = 0.0f;
q[3] = 1.0f;
}
else
{
float sin_half_angle = (float)Math.Sin(angle / 2.0);
q[0] = sin_half_angle * axis.X / norm;
q[1] = sin_half_angle * axis.Y / norm;
q[2] = sin_half_angle * axis.Z / norm;
q[3] = (float)Math.Cos(angle / 2.0);
}
}
public static float AngleBetweenVectors(Vect3f v1, Vect3f v2)
{
return(RadiansToDegrees(
(float)Math.Acos((v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z) /
(v1.Length() * v2.Length()))));
}