public static FPQuaternion Slerp(FPQuaternion from, FPQuaternion to, Fix64 t)
{
t = FPMath.Clamp(t, 0, 1);
Fix64 dot = Dot(from, to);
if (dot < 0.0f)
{
to = Multiply(to, -1);
dot = -dot;
}
Fix64 halfTheta = Fix64.Acos(dot);
return(Multiply(Multiply(from, Fix64.Sin((1 - t) * halfTheta)) + Multiply(to, Fix64.Sin(t * halfTheta)), 1 / Fix64.Sin(halfTheta)));
}
public static void CreateFromYawPitchRoll(Fix64 yaw, Fix64 pitch, Fix64 roll, out FPQuaternion result)
{
Fix64 num9 = roll * Fix64.Half;
Fix64 num6 = Fix64.Sin(num9);
Fix64 num5 = Fix64.Cos(num9);
Fix64 num8 = pitch * Fix64.Half;
Fix64 num4 = Fix64.Sin(num8);
Fix64 num3 = Fix64.Cos(num8);
Fix64 num7 = yaw * Fix64.Half;
Fix64 num2 = Fix64.Sin(num7);
Fix64 num = Fix64.Cos(num7);
result.x = ((num * num4) * num5) + ((num2 * num3) * num6);
result.y = ((num2 * num3) * num5) - ((num * num4) * num6);
result.z = ((num * num3) * num6) - ((num2 * num4) * num5);
result.w = ((num * num3) * num5) + ((num2 * num4) * num6);
}
public static FPQuaternion AngleAxis(Fix64 angle, FPVector axis)
{
axis = axis * Fix64.Deg2Rad;
axis.Normalize();
Fix64 halfAngle = angle * Fix64.Deg2Rad * Fix64.Half;
FPQuaternion rotation;
Fix64 sin = Fix64.Sin(halfAngle);
rotation.x = axis.x * sin;
rotation.y = axis.y * sin;
rotation.z = axis.z * sin;
rotation.w = Fix64.Cos(halfAngle);
return(rotation);
}
public static FPQuaternion RotateTowards(FPQuaternion from, FPQuaternion to, Fix64 maxDegreesDelta)
{
Fix64 dot = Dot(from, to);
if (dot < 0.0f)
{
to = Multiply(to, -1);
dot = -dot;
}
Fix64 halfTheta = Fix64.Acos(dot);
Fix64 theta = halfTheta * 2;
maxDegreesDelta *= Fix64.Deg2Rad;
if (maxDegreesDelta >= theta)
{
return(to);
}
maxDegreesDelta /= theta;
return(Multiply(Multiply(from, Fix64.Sin((1 - maxDegreesDelta) * halfTheta)) + Multiply(to, Fix64.Sin(maxDegreesDelta * halfTheta)), 1 / Fix64.Sin(halfTheta)));
}
