public static Fix64Quat FromTwoVectors(Fix64Vec3 a, Fix64Vec3 b) { // From: http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final Fix64 epsilon = Fix64.FromDivision(1, 1000000); Fix64 ab = Fix64Vec3.LengthSqr(a) * Fix64Vec3.LengthSqr(b); Fix64 norm_a_norm_b = Fix64.FromRaw(NativeFixedMath.Sqrt64(ab.Raw)); Fix64 real_part = norm_a_norm_b + Fix64Vec3.Dot(a, b); Fix64Vec3 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 = Fix64.zero; bool cond = NativeFixedMath.Abs64(a.x.Raw) > NativeFixedMath.Abs64(a.z.Raw); v = cond ? new Fix64Vec3(-a.y, a.x, Fix64.zero) : new Fix64Vec3(Fix64.zero, -a.z, a.y); } else { /* Otherwise, build quaternion the standard way. */ v = Fix64Vec3.Cross(a, b); } return(Normalize(new Fix64Quat(v.x, v.y, v.z, real_part))); }
public static Fix64 Abs(Fix64 a) { return(FromRaw(NativeFixedMath.Abs64(a.Raw))); }