public Matrix33(Matrix33 <ScalarType, MathType> other) :
this(
other[0, 0], other[0, 1], other[0, 2],
other[1, 0], other[1, 1], other[1, 2],
other[2, 0], other[2, 1], other[2, 2]
)
{
}
/// <summary>Builds a quaternion with the same orientation as a matrix</summary>
/// <param name="matrix">Matrix from which the orientation is taken</param>
/// <returns>A quaternion with an identical orientation as the matrix</returns>
/// <remarks>
/// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
/// article "Quaternion Calculus and Fast Animation".
/// </remarks>
public static Quaternion <ScalarType, MathType> FromMatrix(
Matrix33 <ScalarType, MathType> matrix
)
{
Quaternion <ScalarType, MathType> quaternion = new Quaternion <ScalarType, MathType>();
Number <ScalarType, MathType> trace = n(matrix[0][0]) + n(matrix[1][1]) + n(matrix[2][2]);
Number <ScalarType, MathType> root;
if (trace > math.Zero)
{
root = math.Sqrt(trace + math.One);
quaternion.W = math.Scale(root, 0.5);
root = math.Scale(math.Inv(root), 0.5);
quaternion.X = n(matrix[2][1]) - n(matrix[1][2]) * root;
quaternion.Y = n(matrix[0][2]) - n(matrix[2][0]) * root;
quaternion.Z = n(matrix[1][0]) - n(matrix[0][1]) * root;
}
else
{
int[] next = new int[] { 1, 2, 0 };
int i = 0;
if (n(matrix[1][1]) > n(matrix[0][0]))
{
i = 1;
}
if (n(matrix[2][2]) > n(matrix[i][i]))
{
i = 2;
}
int j = next[i];
int k = next[j];
root = math.Sqrt(n(matrix[i][i]) - n(matrix[j][j]) - n(matrix[k][k]) + math.One);
quaternion[i] = math.Scale(root, 0.5);
root = math.Scale(math.Inv(root), 0.5);
quaternion.W = (n(matrix[k][j]) - n(matrix[j][k])) * root;
quaternion[j] = (n(matrix[j][i]) + n(matrix[i][j])) * root;
quaternion[k] = (n(matrix[k][i]) + n(matrix[i][k])) * root;
}
return(quaternion);
}
