public static MQuaternion FromToRotation(MVector3 aFrom, MVector3 aTo)
{
MVector3 axis = aFrom.Cross(aTo);
double angle = aFrom.Angle(aTo);
return(AngleAxis(angle, axis.Normalize()));
}
/// <summary>
/// Computes the rotation to rotate from one vector to the other
/// </summary>
/// <param name="from">The start direction</param>
/// <param name="to">The desired direction</param>
/// <returns></returns>
private static MQuaternion FromToRotation(MVector3 from, MVector3 to)
{
//Normalize both vectors
from = from.Normalize();
to = to.Normalize();
//Estimate the rotation axis
MVector3 axis = MVector3Extensions.Cross(from, to).Normalize();
//Compute the phi angle
double phi = Math.Acos(MVector3Extensions.Dot(from, to)) / (from.Magnitude() * to.Magnitude());
//Create a new quaternion representing the rotation
MQuaternion result = new MQuaternion()
{
X = Math.Sin(phi / 2) * axis.X,
Y = Math.Sin(phi / 2) * axis.Y,
Z = Math.Sin(phi / 2) * axis.Z,
W = Math.Cos(phi / 2)
};
//Perform is nan check and return identity quaternion
if (double.IsNaN(result.W) || double.IsNaN(result.X) || double.IsNaN(result.Y) || double.IsNaN(result.Z))
{
result = new MQuaternion(0, 0, 0, 1);
}
//Return the estimated rotation
return(result);
}
public static MQuaternion AngleAxis(double aAngle, MVector3 aAxis)
{
aAxis = aAxis.Normalize();
double rad = aAngle * Math.PI / 180 * 0.5;
aAxis = aAxis.Multiply(Math.Sin(rad));
return(new MQuaternion(aAxis.X, aAxis.Y, aAxis.Z, Math.Cos(rad)));
}
public static double Angle(this MVector3 from, MVector3 to)
{
double rad = from.Normalize().Dot(to.Normalize());
// clamp
rad = Math.Max(Math.Min(rad, 1), -1);
return(Math.Acos(rad) * 180 / Math.PI);
}