// Creates a unit quaternion that represents the rotation from a to b. a and b do not need to be normalized. public static F64Quat FromTwoVectors(F64Vec3 a, F64Vec3 b) { // From: http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final F64 epsilon = F64.Ratio(1, 1000000); F64 norm_a_norm_b = F64.SqrtFastest(F64Vec3.LengthSqr(a) * F64Vec3.LengthSqr(b)); F64 real_part = norm_a_norm_b + F64Vec3.Dot(a, b); F64Vec3 v; if (real_part < (epsilon * norm_a_norm_b)) { /* If u and v are exactly opposite, rotate 180 degrees * around an arbitrary orthogonal axis. Axis normalization * can happen later, when we normalize the quaternion. */ real_part = F64.Zero; bool cond = F64.Abs(a.X) > F64.Abs(a.Z); v = cond ? new F64Vec3(-a.Y, a.X, F64.Zero) : new F64Vec3(F64.Zero, -a.Z, a.Y); } else { /* Otherwise, build quaternion the standard way. */ v = F64Vec3.Cross(a, b); } return(NormalizeFastest(new F64Quat(v, real_part))); }
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)); }