public MatrixDecompositionContext Clone()
{
var m = new MatrixDecompositionContext();
for (int i = 0; i < 3; i++)
{
m.scale[i] = this.scale[i];
}
for (int i = 0; i < 3; i++)
{
m.translation[i] = this.translation[i];
}
for (int i = 0; i < 3; i++)
{
m.rotationDegrees[i] = this.rotationDegrees[i];
}
return(m);
}
/**
* @param transformMatrix 16-element array of numbers representing 4x4 transform matrix
*/
public static void decomposeMatrix(double[] transformMatrix, ref MatrixDecompositionContext ctx)
{
if (transformMatrix.Length != 16)
{
throw new Exception("Invalid parementer transformMatrix.Length != 16.");
}
// output values
double[] perspective = ctx.perspective;
double[] quaternion = ctx.quaternion;
double[] scale = ctx.scale;
double[] skew = ctx.skew;
double[] translation = ctx.translation;
double[] rotationDegrees = ctx.rotationDegrees;
// create normalized, 2d array matrix
// and normalized 1d array perspectiveMatrix with redefined 4th column
if (isZero(transformMatrix[15]))
{
return;
}
double[][] matrix = new double[4][];
matrix[0] = new double[4];
matrix[1] = new double[4];
matrix[2] = new double[4];
matrix[3] = new double[4];
double[] perspectiveMatrix = new double[16];
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
double value = transformMatrix[(i * 4) + j] / transformMatrix[15];
matrix[i][j] = value;
perspectiveMatrix[(i * 4) + j] = j == 3 ? 0 : value;
}
}
perspectiveMatrix[15] = 1;
// test for singularity of upper 3x3 part of the perspective matrix
if (isZero(determinant(perspectiveMatrix)))
{
return;
}
// isolate perspective
if (!isZero(matrix[0][3]) || !isZero(matrix[1][3]) || !isZero(matrix[2][3]))
{
// rightHandSide is the right hand side of the equation.
// rightHandSide is a vector, or point in 3d space relative to the origin.
double[] rightHandSide = { matrix[0][3], matrix[1][3], matrix[2][3], matrix[3][3] };
// Solve the equation by inverting perspectiveMatrix and multiplying
// rightHandSide by the inverse.
double[] inversePerspectiveMatrix = inverse(
perspectiveMatrix
);
double[] transposedInversePerspectiveMatrix = transpose(
inversePerspectiveMatrix
);
multiplyVectorByMatrix(rightHandSide, transposedInversePerspectiveMatrix, perspective);
}
else
{
// no perspective
perspective[0] = perspective[1] = perspective[2] = 0d;
perspective[3] = 1d;
}
// translation is simple
for (int i = 0; i < 3; i++)
{
translation[i] = matrix[3][i];
}
// Now get scale and shear.
// 'row' is a 3 element array of 3 component vectors
double[][] row = new double[3][];
row[0] = new double[3];
row[1] = new double[3];
row[2] = new double[3];
for (int i = 0; i < 3; i++)
{
row[i][0] = matrix[i][0];
row[i][1] = matrix[i][1];
row[i][2] = matrix[i][2];
}
// Compute X scale factor and normalize first row.
scale[0] = v3Length(row[0]);
row[0] = v3Normalize(row[0], scale[0]);
// Compute XY shear factor and make 2nd row orthogonal to 1st.
skew[0] = v3Dot(row[0], row[1]);
row[1] = v3Combine(row[1], row[0], 1.0, -skew[0]);
// Compute XY shear factor and make 2nd row orthogonal to 1st.
skew[0] = v3Dot(row[0], row[1]);
row[1] = v3Combine(row[1], row[0], 1.0, -skew[0]);
// Now, compute Y scale and normalize 2nd row.
scale[1] = v3Length(row[1]);
row[1] = v3Normalize(row[1], scale[1]);
skew[0] /= scale[1];
// Compute XZ and YZ shears, orthogonalize 3rd row
skew[1] = v3Dot(row[0], row[2]);
row[2] = v3Combine(row[2], row[0], 1.0, -skew[1]);
skew[2] = v3Dot(row[1], row[2]);
row[2] = v3Combine(row[2], row[1], 1.0, -skew[2]);
// Next, get Z scale and normalize 3rd row.
scale[2] = v3Length(row[2]);
row[2] = v3Normalize(row[2], scale[2]);
skew[1] /= scale[2];
skew[2] /= scale[2];
// At this point, the matrix (in rows) is orthonormal.
// Check for a coordinate system flip. If the determinant
// is -1, then negate the matrix and the scaling factors.
double[] pdum3 = v3Cross(row[1], row[2]);
if (v3Dot(row[0], pdum3) < 0)
{
for (int i = 0; i < 3; i++)
{
scale[i] *= -1;
row[i][0] *= -1;
row[i][1] *= -1;
row[i][2] *= -1;
}
}
// Now, get the rotations out
quaternion[0] =
0.5 * Math.Sqrt(Math.Max(1 + row[0][0] - row[1][1] - row[2][2], 0));
quaternion[1] =
0.5 * Math.Sqrt(Math.Max(1 - row[0][0] + row[1][1] - row[2][2], 0));
quaternion[2] =
0.5 * Math.Sqrt(Math.Max(1 - row[0][0] - row[1][1] + row[2][2], 0));
quaternion[3] =
0.5 * Math.Sqrt(Math.Max(1 + row[0][0] + row[1][1] + row[2][2], 0));
if (row[2][1] > row[1][2])
{
quaternion[0] = -quaternion[0];
}
if (row[0][2] > row[2][0])
{
quaternion[1] = -quaternion[1];
}
if (row[1][0] > row[0][1])
{
quaternion[2] = -quaternion[2];
}
// correct for occasional, weird Euler synonyms for 2d rotation
if (quaternion[0] < 0.001 && quaternion[0] >= 0 &&
quaternion[1] < 0.001 && quaternion[1] >= 0)
{
// this is a 2d rotation on the z-axis
rotationDegrees[0] = rotationDegrees[1] = 0d;
rotationDegrees[2] = roundTo3Places(Math.Atan2(row[0][1], row[0][0]) * 180 / Math.PI);
}
else
{
quaternionToDegreesXYZ(quaternion, rotationDegrees);
}
}
