private void UpdateOrientation()
{
Quatf q = orientation.GetRotation();
q.Normalize();
orientation = Mat3f.CreateRotation(q);
}
public void GetAngleAxis(out Vec3f axis, out float angle)
{
Quatf q = this;
if (MathUtils.Absolute(q.w) > 1.0f)
{
q.Normalize();
}
angle = 2.0f * (float)MathUtils.Acos(q.w);
var den = (float)MathUtils.Sqrt(1.0f - (q.w * q.w));
if (den > 0.0001f)
{
axis = q.XYZ / den;
}
else
{
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
axis = Vec3f.Up;
}
}