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