void
CombineQuats(vec4 Q1, vec4 Q2, vec4 Dest)
{ vec3 t1, t2, t3;
v3Copy(Q1,t1); v3Scale(t1,Q2[3]);
v3Copy(Q2,t2); v3Scale(t2,Q1[3]);
v3Cross(Q2,Q1,t3);
v3Add(t1,t2,Dest);
v3Add(t3,Dest,Dest);
Dest[3] = Q1[3] * Q2[3] - v3Dot(Q1,Q2);
v4Normalize(Dest);
}
static bool decompose(const TransformationMatrix::Matrix4& mat, TransformationMatrix::DecomposedType& result)
{
TransformationMatrix::Matrix4 localMatrix;
memcpy(localMatrix, mat, sizeof(TransformationMatrix::Matrix4));
// Normalize the matrix.
if (localMatrix[3][3] == 0)
return false;
int i, j;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
localMatrix[i][j] /= localMatrix[3][3];
// perspectiveMatrix is used to solve for perspective, but it also provides
// an easy way to test for singularity of the upper 3x3 component.
TransformationMatrix::Matrix4 perspectiveMatrix;
memcpy(perspectiveMatrix, localMatrix, sizeof(TransformationMatrix::Matrix4));
for (i = 0; i < 3; i++)
perspectiveMatrix[i][3] = 0;
perspectiveMatrix[3][3] = 1;
if (determinant4x4(perspectiveMatrix) == 0)
return false;
// First, isolate perspective. This is the messiest.
if (localMatrix[0][3] != 0 || localMatrix[1][3] != 0 || localMatrix[2][3] != 0) {
// rightHandSide is the right hand side of the equation.
Vector4 rightHandSide;
rightHandSide[0] = localMatrix[0][3];
rightHandSide[1] = localMatrix[1][3];
rightHandSide[2] = localMatrix[2][3];
rightHandSide[3] = localMatrix[3][3];
// Solve the equation by inverting perspectiveMatrix and multiplying
// rightHandSide by the inverse. (This is the easiest way, not
// necessarily the best.)
TransformationMatrix::Matrix4 inversePerspectiveMatrix, transposedInversePerspectiveMatrix;
inverse(perspectiveMatrix, inversePerspectiveMatrix);
transposeMatrix4(inversePerspectiveMatrix, transposedInversePerspectiveMatrix);
Vector4 perspectivePoint;
v4MulPointByMatrix(rightHandSide, transposedInversePerspectiveMatrix, perspectivePoint);
result.perspectiveX = perspectivePoint[0];
result.perspectiveY = perspectivePoint[1];
result.perspectiveZ = perspectivePoint[2];
result.perspectiveW = perspectivePoint[3];
// Clear the perspective partition
localMatrix[0][3] = localMatrix[1][3] = localMatrix[2][3] = 0;
localMatrix[3][3] = 1;
} else {
// No perspective.
result.perspectiveX = result.perspectiveY = result.perspectiveZ = 0;
result.perspectiveW = 1;
}
// Next take care of translation (easy).
result.translateX = localMatrix[3][0];
localMatrix[3][0] = 0;
result.translateY = localMatrix[3][1];
localMatrix[3][1] = 0;
result.translateZ = localMatrix[3][2];
localMatrix[3][2] = 0;
// Vector4 type and functions need to be added to the common set.
Vector3 row[3], pdum3;
// Now get scale and shear.
for (i = 0; i < 3; i++) {
row[i][0] = localMatrix[i][0];
row[i][1] = localMatrix[i][1];
row[i][2] = localMatrix[i][2];
}
// Compute X scale factor and normalize first row.
result.scaleX = v3Length(row[0]);
v3Scale(row[0], 1.0);
// Compute XY shear factor and make 2nd row orthogonal to 1st.
result.skewXY = v3Dot(row[0], row[1]);
v3Combine(row[1], row[0], row[1], 1.0, -result.skewXY);
// Now, compute Y scale and normalize 2nd row.
result.scaleY = v3Length(row[1]);
v3Scale(row[1], 1.0);
result.skewXY /= result.scaleY;
// Compute XZ and YZ shears, orthogonalize 3rd row.
result.skewXZ = v3Dot(row[0], row[2]);
v3Combine(row[2], row[0], row[2], 1.0, -result.skewXZ);
result.skewYZ = v3Dot(row[1], row[2]);
v3Combine(row[2], row[1], row[2], 1.0, -result.skewYZ);
// Next, get Z scale and normalize 3rd row.
result.scaleZ = v3Length(row[2]);
v3Scale(row[2], 1.0);
result.skewXZ /= result.scaleZ;
result.skewYZ /= result.scaleZ;
// 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.
v3Cross(row[1], row[2], pdum3);
if (v3Dot(row[0], pdum3) < 0) {
for (i = 0; i < 3; i++) {
result.scaleX *= -1;
row[i][0] *= -1;
row[i][1] *= -1;
row[i][2] *= -1;
}
}
// Now, get the rotations out, as described in the gem.
// FIXME - Add the ability to return either quaternions (which are
// easier to recompose with) or Euler angles (rx, ry, rz), which
// are easier for authors to deal with. The latter will only be useful
// when we fix https://bugs.webkit.org/show_bug.cgi?id=23799, so I
// will leave the Euler angle code here for now.
// ret.rotateY = asin(-row[0][2]);
// if (cos(ret.rotateY) != 0) {
// ret.rotateX = atan2(row[1][2], row[2][2]);
// ret.rotateZ = atan2(row[0][1], row[0][0]);
// } else {
// ret.rotateX = atan2(-row[2][0], row[1][1]);
// ret.rotateZ = 0;
// }
double s, t, x, y, z, w;
t = row[0][0] + row[1][1] + row[2][2] + 1.0;
if (t > 1e-4) {
s = 0.5 / sqrt(t);
w = 0.25 / s;
x = (row[2][1] - row[1][2]) * s;
y = (row[0][2] - row[2][0]) * s;
z = (row[1][0] - row[0][1]) * s;
} else if (row[0][0] > row[1][1] && row[0][0] > row[2][2]) {
s = sqrt (1.0 + row[0][0] - row[1][1] - row[2][2]) * 2.0; // S=4*qx
x = 0.25 * s;
y = (row[0][1] + row[1][0]) / s;
z = (row[0][2] + row[2][0]) / s;
w = (row[2][1] - row[1][2]) / s;
} else if (row[1][1] > row[2][2]) {
s = sqrt (1.0 + row[1][1] - row[0][0] - row[2][2]) * 2.0; // S=4*qy
x = (row[0][1] + row[1][0]) / s;
y = 0.25 * s;
z = (row[1][2] + row[2][1]) / s;
w = (row[0][2] - row[2][0]) / s;
} else {
s = sqrt(1.0 + row[2][2] - row[0][0] - row[1][1]) * 2.0; // S=4*qz
x = (row[0][2] + row[2][0]) / s;
y = (row[1][2] + row[2][1]) / s;
z = 0.25 * s;
w = (row[1][0] - row[0][1]) / s;
}
result.quaternionX = x;
result.quaternionY = y;
result.quaternionZ = z;
result.quaternionW = w;
return true;
}
float
v3Length(vec3 V)
{
return sqrt(v3Dot(V,V));
}