// 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)));
}
// Rotates a vector by the unit quaternion.
public static F64Vec3 RotateVector(F64Quat rot, F64Vec3 v)
{ // From https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion
F64Vec3 u = new F64Vec3(rot.X, rot.Y, rot.Z);
F64 s = rot.W;
return
((F64.Two * F64Vec3.Dot(u, v)) * u +
(s * s - F64Vec3.Dot(u, u)) * v +
(F64.Two * s) * F64Vec3.Cross(u, v));
}
