/// <summary>
/// Computes the rotation matrix to transform a source vector into a target vector
/// (routine from Thomas Moller)
/// </summary>
/// <param name="_Source">The source vector</param>
/// <param name="_Target">The target vector</param>
/// <returns>The rotation matrix to apply that will transform</returns>
public static float3x3 ComputeRotationMatrix(float3 _Source, float3 _Target)
{
SharpMath.float3x3 Result = new float3x3();
Result.MakeIdentity();
float e = _Source | _Target;
bool bReverse = e < 0.0f;
if (bReverse)
{ // Revert target
_Target = -_Target;
e = -e;
}
if (e > 1.0f - 0.000001f)
{
if (bReverse)
{ // Reverse final matrix
Result.SetRow0(-Result.GetRow0());
Result.SetRow1(-Result.GetRow1());
Result.SetRow2(-Result.GetRow2());
}
return(Result); // No rotation needed...
}
float3 Ortho = _Source ^ _Target;
float h = 1.0f / (1.0f + e); // Optimization by Gottfried Chen
Result.SetRow0(new float3(e + h * Ortho.x * Ortho.x,
h * Ortho.x * Ortho.y + Ortho.z,
h * Ortho.x * Ortho.z - Ortho.y));
Result.SetRow1(new float3(h * Ortho.x * Ortho.y - Ortho.z,
e + h * Ortho.y * Ortho.y,
h * Ortho.y * Ortho.z + Ortho.x));
Result.SetRow2(new float3(h * Ortho.x * Ortho.z + Ortho.y,
h * Ortho.y * Ortho.z - Ortho.x,
e + h * Ortho.z * Ortho.z));
if (bReverse)
{ // Reverse final matrix
Result.SetRow0(-Result.GetRow0());
Result.SetRow1(-Result.GetRow1());
Result.SetRow2(-Result.GetRow2());
}
return(Result);
}