public static float Angle(vec2 a, vec2 b)
{
float cos = Dot(a, b);
float sin = -vec3.Cross(a.xyz(0), b.xyz(0)).z;
return(cos.Acos() * sin.Sign());
}
/// <summary>
/// Spherical linear interpolation between two quaternions
/// Note: SLerp is not commutative
/// </summary>
public Quat SLerp(Quat q, float t)
{
var q1 = q;
// Calculate cosine
float cosTheta = X * q.X + Y * q.Y + Z * q.Z + W * q.W;
// Use the shortest path
if (cosTheta < 0)
{
cosTheta = -cosTheta;
q1.X = -q.X; q1.Y = -q.Y;
q1.Z = -q.Z; q1.W = -q.W;
}
// Initialize with linear interpolation
float scale0 = 1 - t, scale1 = t;
// Use spherical interpolation only if the quaternions are not very close
if ((1 - cosTheta) > 0.001f)
{
// SLERP
float theta = cosTheta.Acos();
float sinTheta = theta.Sin();
scale0 = Math.Sin((1 - t) * theta) / sinTheta;
scale1 = Math.Sin(t * theta) / sinTheta;
}
// Calculate final quaternion
return(new Quat(X * scale0 + q1.X * scale1, Y * scale0 + q1.Y * scale1,
Z * scale0 + q1.Z * scale1, W * scale0 + q1.W * scale1));
}
public static float Angle(vec3 a, vec3 b)
{
vec3 c = Cross(a, b);
float cos = Dot(a, b);
float sin = Dot(c, c.s(1, -1, 1));
return(cos.Acos() * sin.Sign());
}