/// <summary>
///
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static Quaterniond CreateFromTo(Vec3d from, Vec3d to)
{
// impl refs
// https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
// http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors
if (!from.Unitize() || !to.Unitize())
{
return(Identity);
}
var ca = Vec3d.Dot(from, to);
// parallel check
if (zMath.ApproxEquals(Math.Abs(ca), 1.0))
{
//opposite check
if (ca < 0.0)
{
var perp = from.X < 1.0 ? from.CrossX() : from.CrossY();
var t = 1.0 / perp.Length;
return(new Quaterniond(perp.X * t, perp.Y * t, perp.Z * t, 0.0));
}
return(Identity);
}
// can assume axis is valid
var axis = Vec3d.Cross(from, to);
var q = new Quaterniond(axis.X, axis.Y, axis.Z, ca + 1);
q.Unitize();
return(q);
}
/// <summary>
///
/// </summary>
/// <param name="rotation"></param>
public bool Set(Quaterniond rotation)
{
if (!rotation.Unitize())
{
return(false);
}
var c2 = rotation.W;
var s2 = 1.0 - c2 * c2; // pythag's identity
if (s2 > 0.0)
{
s2 = Math.Sqrt(s2);
var s2Inv = 1.0 / s2;
_axis.X = rotation.X * s2Inv;
_axis.Y = rotation.Y * s2Inv;
_axis.Z = rotation.Z * s2Inv;
_angle = 2.0 * Math.Acos(c2);
// double-angle identities
_cosAngle = 2.0 * c2 * c2 - 1.0;
_sinAngle = 2.0 * c2 * s2;
return(true);
}
return(false);
}
