/// <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); }