/// <summary>
/// Creates a rotation matrix that rotates a vector called "from" into another
/// vector called "to". Based on an algorithm by Tomas Moller and John Hudges:
/// <para>
/// "Efficiently Building a Matrix to Rotate One Vector to Another"
/// Journal of Graphics Tools, 4(4):1-4, 1999
/// </para>
/// </summary>
/// <param name="from">Starting vector</param>
/// <param name="to">Ending vector</param>
/// <returns>Rotation matrix to rotate from the start to end.</returns>
public static Matrix3x3 FromToMatrix(Vector3D from, Vector3D to)
{
float e = Vector3D.Dot(from, to);
float f = (e < 0) ? -e : e;
Matrix3x3 m = Identity;
//"from" and "to" vectors almost parallel
if (f > 1.0f - 0.00001f)
{
Vector3D u, v; //Temp variables
Vector3D x; //Vector almost orthogonal to "from"
x.X = (from.X > 0.0f) ? from.X : -from.X;
x.Y = (from.Y > 0.0f) ? from.Y : -from.Y;
x.Z = (from.Z > 0.0f) ? from.Z : -from.Z;
if (x.X < x.Y)
{
if (x.X < x.Z)
{
x.X = 1.0f;
x.Y = 0.0f;
x.Z = 0.0f;
}
else
{
x.X = 0.0f;
x.Y = 0.0f;
x.Z = 1.0f;
}
}
else
{
if (x.Y < x.Z)
{
x.X = 0.0f;
x.Y = 1.0f;
x.Z = 0.0f;
}
else
{
x.X = 0.0f;
x.Y = 0.0f;
x.Z = 1.0f;
}
}
u.X = x.X - from.X;
u.Y = x.Y - from.Y;
u.Z = x.Z - from.Z;
v.X = x.X - to.X;
v.Y = x.Y - to.Y;
v.Z = x.Z - to.Z;
float c1 = 2.0f / Vector3D.Dot(u, u);
float c2 = 2.0f / Vector3D.Dot(v, v);
float c3 = c1 * c2 * Vector3D.Dot(u, v);
for (int i = 1; i < 4; i++)
{
for (int j = 1; j < 4; j++)
{
//This is somewhat unreadable, but the indices for u, v vectors are "zero-based" while
//matrix indices are "one-based" always subtract by one to index those
m[i, j] = -c1 * u[i - 1] * u[j - 1] - c2 * v[i - 1] * v[j - 1] + c3 * v[i - 1] * u[j - 1];
}
m[i, i] += 1.0f;
}
}
else
{
//Most common case, unless "from" = "to" or "from" =- "to"
Vector3D v = Vector3D.Cross(from, to);
//Hand optimized version (9 mults less) by Gottfried Chen
float h = 1.0f / (1.0f + e);
float hvx = h * v.X;
float hvz = h * v.Z;
float hvxy = hvx * v.Y;
float hvxz = hvx * v.Z;
float hvyz = hvz * v.Y;
m.A1 = e + hvx * v.X;
m.A2 = hvxy - v.Z;
m.A3 = hvxz + v.Y;
m.B1 = hvxy + v.Z;
m.B2 = e + h * v.Y * v.Y;
m.B3 = hvyz - v.X;
m.C1 = hvxz - v.Y;
m.C2 = hvyz + v.X;
m.C3 = e + hvz * v.Z;
}
return(m);
}
