public static Fixed Angle(FixedQuaternion a, FixedQuaternion b)
{
Fixed f = Dot(a, b);
return(FixedMathf.Acos(FixedMathf.Min(FixedMathf.Abs(f), Fixed.One).AsFloat()) * new Fixed(2) * Fixed.Rad2Deg);
}
public static FixedQuaternion Slerp(FixedQuaternion a, FixedQuaternion b, Fixed f)
{
if (a.sqrMagnitude == Fixed.Zero)
{
if (b.sqrMagnitude == Fixed.Zero)
{
return(Identity);
}
return(b);
}
if (b.sqrMagnitude == Fixed.Zero)
{
return(a);
}
if (f > Fixed.One)
{
f = Fixed.One;
}
if (f < Fixed.Zero)
{
f = Fixed.Zero;
}
Fixed cosHalfAngle = a.w * b.w + FixedVector3.Dot(new FixedVector3(a.x, a.y, a.z), new FixedVector3(b.x, b.y, b.z));
if (cosHalfAngle >= Fixed.One || cosHalfAngle <= -Fixed.One)
{
return(a);
}
if (cosHalfAngle < Fixed.Zero)
{
b.x = -b.x;
b.y = -b.y;
b.z = -b.z;
b.w = -b.w;
cosHalfAngle = -cosHalfAngle;
}
Fixed blendA;
Fixed blendB;
if (cosHalfAngle < 0.999999)
{
Fixed halfAngle = FixedMathf.Acos(cosHalfAngle);
Fixed sinHalfAngle = FixedMathf.Sin(halfAngle);
Fixed oneOverSinHalfAngle = Fixed.One / sinHalfAngle;
blendA = FixedMathf.Sin(halfAngle * (Fixed.One - f)) * oneOverSinHalfAngle;
blendB = FixedMathf.Sin(halfAngle * f) * oneOverSinHalfAngle;
}
else
{
blendA = Fixed.One - f;
blendB = f;
}
FixedQuaternion result =
new FixedQuaternion(blendA * new FixedVector3(a.x, a.y, a.z) + blendB * new FixedVector3(b.x, b.y, b.z),
blendA * a.w + blendB * b.w);
if (result.sqrMagnitude > Fixed.Zero)
{
return(Normalize(result));
}
return(Identity);
}
