//Angle in degrees between two normalized vectors
public static Fix64 Anlge(Fix64Vec3 a, Fix64Vec3 b)
{
Fix64 dot = Fix64Vec3.Dot(a.normalized, b.normalized);
Fix64 clamped = Fix64Math.Clamp(dot, -Fix64.one, Fix64.one);
Fix64 rad = Fix64.FromRaw(NativeFixedMath.Acos64(clamped.Raw));
return(rad * Fix64.RadToDegree);
}
public static Fix64Quat Slerp(Fix64Quat q1, Fix64Quat q2, Fix64 t)
{
Fix64 epsilon = Fix64.FromDivision(1, 1000000);
Fix64 cos_omega = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
bool flip = false;
if (cos_omega < Fix64.zero)
{
flip = true;
cos_omega = -cos_omega;
}
Fix64 s1, s2;
if (cos_omega > (Fix64.one - epsilon))
{
// Too close, do straight linear interpolation.
s1 = Fix64.one - t;
s2 = (flip) ? -t : t;
}
else
{
Fix64 omega = Fix64.FromRaw(NativeFixedMath.Acos64(cos_omega.Raw));
Fix64 inv_sin_omega = Fix64.one / Fix64.FromRaw(NativeFixedMath.Sin64(omega.Raw));
Fix64 v1 = (Fix64.one - t) * omega;
Fix64 v2 = t * omega;
s1 = Fix64.FromRaw(NativeFixedMath.Sin64(v1.Raw)) * inv_sin_omega;
s2 = Fix64.FromRaw(NativeFixedMath.Sin64(v2.Raw)) * inv_sin_omega;
s2 = (flip) ? -s2 : s2;
}
return(new Fix64Quat(
s1 * q1.x + s2 * q2.x,
s1 * q1.y + s2 * q2.y,
s1 * q1.z + s2 * q2.z,
s1 * q1.w + s2 * q2.w));
}