/// <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="from"></param>
/// <param name="to"></param>
/// <returns></returns>
public static AxisAngle3d CreateFromTo(Vec3d from, Vec3d to)
{
if (!from.Unitize() || !to.Unitize())
{
return(Identity);
}
var ct = Vec3d.Dot(from, to);
// parallel check
if (1.0 - Math.Abs(ct) < zMath.ZeroTolerance)
{
// opposite check
if (ct < 0.0)
{
var perp = from.X < 1.0 ? from.CrossX() : from.CrossY();
return(new AxisAngle3d()
{
_axis = perp / perp.Length,
_angle = Math.PI,
_cosAngle = -1.0,
_sinAngle = 0.0
});
}
return(Identity);
}
// can assume axis is valid
var axis = Vec3d.Cross(from, to);
var st = axis.Length;
return(new AxisAngle3d()
{
_axis = axis / st,
_angle = Math.Acos(ct),
_sinAngle = st,
_cosAngle = ct
});
}
