public static F64Quat FromYawPitchRoll(F64 yaw_y, F64 pitch_x, F64 roll_z)
{
// Roll first, about axis the object is facing, then
// pitch upward, then yaw to face into the new heading
F64 half_roll = F64.Div2(roll_z);
F64 sr = F64.SinFastest(half_roll);
F64 cr = F64.CosFastest(half_roll);
F64 half_pitch = F64.Div2(pitch_x);
F64 sp = F64.SinFastest(half_pitch);
F64 cp = F64.CosFastest(half_pitch);
F64 half_yaw = F64.Div2(yaw_y);
F64 sy = F64.SinFastest(half_yaw);
F64 cy = F64.CosFastest(half_yaw);
return(new F64Quat(
cy * sp * cr + sy * cp * sr,
sy * cp * cr - cy * sp * sr,
cy * cp * sr - sy * sp * cr,
cy * cp * cr + sy * sp * sr));
}
public static F64Quat Slerp(F64Quat q1, F64Quat q2, F64 t)
{
F64 epsilon = F64.Ratio(1, 1000000);
F64 cos_omega = q1.X * q2.X + q1.Y * q2.Y + q1.Z * q2.Z + q1.W * q2.W;
bool flip = false;
if (cos_omega < 0)
{
flip = true;
cos_omega = -cos_omega;
}
F64 s1, s2;
if (cos_omega > (F64.One - epsilon))
{
// Too close, do straight linear interpolation.
s1 = F64.One - t;
s2 = (flip) ? -t : t;
}
else
{
F64 omega = F64.AcosFastest(cos_omega);
F64 inv_sin_omega = F64.RcpFastest(F64.SinFastest(omega));
s1 = F64.SinFastest((F64.One - t) * omega) * inv_sin_omega;
s2 = (flip)
? -F64.SinFastest(t * omega) * inv_sin_omega
: F64.SinFastest(t * omega) * inv_sin_omega;
}
return(new F64Quat(
s1 * q1.X + s2 * q2.X,
s1 * q1.Y + s2 * q2.Y,
s1 * q1.Z + s2 * q2.Z,
s1 * q1.W + s2 * q2.W));
}
public static F64Quat FromAxisAngle(F64Vec3 axis, F64 angle)
{
F64 half_angle = F64.Div2(angle);
return(new F64Quat(axis * F64.SinFastest(half_angle), F64.CosFastest(half_angle)));
}
