int Matrix4x4_Invert_Full (matrix4x4_t *out, const matrix4x4_t *in1)
{
float det;
- int i, j;
// note: orientation does not matter, as transpose(invert(transpose(m))) == invert(m), proof:
// transpose(invert(transpose(m))) * m
out->m[3][3] = (m00*(m11*m22 - m12*m21) - m10*(m01*m22 - m02*m21) + m20*(m01*m12 - m02*m11));
// calculate the determinant (as inverse == 1/det * adjoint, adjoint * m == identity * det, so this calculates the det)
- det = in1->m[0][0]*out->m[0][0] + in1->m[1][0]*out->m[0][1] + in1->m[2][0]*out->m[0][2] + in1->m[3][0]*out->m[0][3];
+ det = m00*out->m[0][0] + m10*out->m[0][1] + m20*out->m[0][2] + m30*out->m[0][3];
if (det == 0.0f)
return 0;
void Matrix4x4_TransformPositivePlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
{
+ float scale = sqrt(in->m[0][0] * in->m[0][0] + in->m[0][1] * in->m[0][1] + in->m[0][2] * in->m[0][2]);
+ float iscale = 1.0f / scale;
#ifdef MATRIX4x4_OPENGLORIENTATION
- o[0] = x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0];
- o[1] = x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1];
- o[2] = x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2];
- o[3] = d + (x * in->m[3][0] + y * in->m[3][1] + z * in->m[3][2]);
+ o[0] = (x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0]) * iscale;
+ o[1] = (x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1]) * iscale;
+ o[2] = (x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2]) * iscale;
+ o[3] = d * scale + (o[0] * in->m[3][0] + o[1] * in->m[3][1] + o[2] * in->m[3][2]);
#else
- o[0] = x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2];
- o[1] = x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2];
- o[2] = x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2];
- o[3] = d + (x * in->m[0][3] + y * in->m[1][3] + z * in->m[2][3]);
+ o[0] = (x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2]) * iscale;
+ o[1] = (x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2]) * iscale;
+ o[2] = (x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2]) * iscale;
+ o[3] = d * scale + (o[0] * in->m[0][3] + o[1] * in->m[1][3] + o[2] * in->m[2][3]);
#endif
}
void Matrix4x4_TransformStandardPlane(const matrix4x4_t *in, float x, float y, float z, float d, float *o)
{
+ float scale = sqrt(in->m[0][0] * in->m[0][0] + in->m[0][1] * in->m[0][1] + in->m[0][2] * in->m[0][2]);
+ float iscale = 1.0f / scale;
#ifdef MATRIX4x4_OPENGLORIENTATION
- o[0] = x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0];
- o[1] = x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1];
- o[2] = x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2];
- o[3] = d - (x * in->m[3][0] + y * in->m[3][1] + z * in->m[3][2]);
+ o[0] = (x * in->m[0][0] + y * in->m[1][0] + z * in->m[2][0]) * iscale;
+ o[1] = (x * in->m[0][1] + y * in->m[1][1] + z * in->m[2][1]) * iscale;
+ o[2] = (x * in->m[0][2] + y * in->m[1][2] + z * in->m[2][2]) * iscale;
+ o[3] = d * scale - (o[0] * in->m[3][0] + o[1] * in->m[3][1] + o[2] * in->m[3][2]);
#else
- o[0] = x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2];
- o[1] = x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2];
- o[2] = x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2];
- o[3] = d - (x * in->m[0][3] + y * in->m[1][3] + z * in->m[2][3]);
+ o[0] = (x * in->m[0][0] + y * in->m[0][1] + z * in->m[0][2]) * iscale;
+ o[1] = (x * in->m[1][0] + y * in->m[1][1] + z * in->m[1][2]) * iscale;
+ o[2] = (x * in->m[2][0] + y * in->m[2][1] + z * in->m[2][2]) * iscale;
+ o[3] = d * scale - (o[0] * in->m[0][3] + o[1] * in->m[1][3] + o[2] * in->m[2][3]);
#endif
}
out->m[2][3] *= originscale;
#endif
}
+
+void Matrix4x4_Abs (matrix4x4_t *out)
+{
+ out->m[0][0] = fabs(out->m[0][0]);
+ out->m[0][1] = fabs(out->m[0][1]);
+ out->m[0][2] = fabs(out->m[0][2]);
+ out->m[1][0] = fabs(out->m[1][0]);
+ out->m[1][1] = fabs(out->m[1][1]);
+ out->m[1][2] = fabs(out->m[1][2]);
+ out->m[2][0] = fabs(out->m[2][0]);
+ out->m[2][1] = fabs(out->m[2][1]);
+ out->m[2][2] = fabs(out->m[2][2]);
+}
+