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