public static SVector3 GetRotVectorFromOrigin(SVector3 origin, SQuaternion q)
{
SQuaternion temq = new SQuaternion(origin.x, origin.y, origin.z, (Fix64)0);
SQuaternion finalq = q * temq * q.conjugated;
return(new SVector3(finalq.x, finalq.y, finalq.z));
}
public static SQuaternion LerpLargeAngle(SQuaternion a, SQuaternion b, Fix64 t)
{
if (a == b)
{
return(a);
}
Fix64 cosa = Dot(a, b);
if (cosa >= (Fix64)0)
{
b = -b;
}
Fix64 sina = (Fix64)Fix64.Sqrt((Fix64)1 - cosa * cosa);
Fix64 angle = (Fix64)Fix64.Atan2(sina, cosa);
Fix64 k0 = (Fix64)Fix64.Sin(((Fix64)1 - t) * angle) / sina;
Fix64 k1 = (Fix64)Fix64.Sin(t * angle) / sina;
return(k0 * a + k1 * b);
}
public STransform()
{
_localRotation = SQuaternion.AxisAngle(SVector3.up, (Fix64)0);
_localPosition = SVector3.zero;
}
public void RotateAroundAxis(SVector3 axis, Fix64 angle)
{
rotation = SQuaternion.AxisAngle(axis, angle) * rotation;
}
public static SVector3 GetRotVectorFromX(SVector3 origin, SQuaternion q)
{
Fix64 length = origin.magnitude;
return(GetRotVectorFromOrigin(SVector3.right, q) * length);
}
public static Fix64 Dot(SQuaternion a, SQuaternion b)
{
return(a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w);
}
public static bool DirectionEquals(SQuaternion a, SQuaternion b)
{
return(a == b || a == -b);
}