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)));
}