Add rigid inverse for 3x3 and 4x4 matrices

CgeMath.h

@@ -39,6 +39,7 @@ void CgeMat3fTranspose(const float in[9], float out[9]);
 float CgeMat3fTrace(const float in[9]);
 float CgeMat3fDet(const float in[9]);
 int CgeMat3fInverse(const float in[9], float out[9]);
+void CgeMat3fInverseRigid(const float in[9], float out[9]);
 void CgeMat3fFromScale(float x, float y, float out[9]);
 void CgeMat3fFromTranslation(float x, float y, float out[9]);
 void CgeMat3fFromRotation(float angle, float out[9]);
@@ -54,6 +55,7 @@ void CgeMat4fTranspose(const float in[16], float out[16]);
 float CgeMat4fTrace(const float in[16]);
 float CgeMat4fDet(const float in[16]);
 int CgeMat4fInverse(const float in[16], float out[16]);
+void CgeMat4fInverseRigid(const float in[16], float out[16]);
 void CgeMat4fFromScale(float x, float y, float z, float out[16]);
 void CgeMat4fFromTranslation(float x, float y, float z, float out[16]);
 void CgeMat4fFromRotationX(float angle, float out[16]);

Mat3f.c

@@ -83,7 +83,7 @@ float CgeMat3fDet(const float in[9]) {
 
 int CgeMat3fInverse(const float in[9], float out[9]) {
     float a, b, c, det;
-    float tmp[16];
+    float tmp[9];
 
     a = in[4] * in[8] - in[7] * in[5];
     b = in[1] * in[8] - in[7] * in[2];
@@ -121,6 +121,18 @@ int CgeMat3fInverse(const float in[9], float out[9]) {
     return 1;
 }
 
+void CgeMat3fInverseRigid(const float in[9], float out[9]) {
+    float tmp[9];
+
+    CgeMat3fIdentity(tmp);
+    tmp[0] = in[0]; tmp[1] = in[3];
+    tmp[3] = in[1]; tmp[4] = in[4];
+    tmp[6] = -(in[6] * tmp[0] + in[7] * tmp[3]);
+    tmp[7] = -(in[6] * tmp[1] + in[7] * tmp[4]);
+
+    memcpy(out, tmp, sizeof(tmp));
+}
+
 void CgeMat3fFromScale(float x, float y, float out[9]) {
     CgeMat3fIdentity(out);
     out[0] = x;

Mat4f.c

@@ -156,6 +156,20 @@ int CgeMat4fInverse(const float in[16], float out[16]) {
     return 1;
 }
 
+void CgeMat4fInverseRigid(const float in[16], float out[16]) {
+    float tmp[16];
+
+    CgeMat4fIdentity(tmp);
+    tmp[0] = in[0]; tmp[1] = in[4]; tmp[2] = in[8];
+    tmp[4] = in[1]; tmp[5] = in[5]; tmp[6] = in[9];
+    tmp[8] = in[2]; tmp[9] = in[6]; tmp[10] = in[10];
+
+    tmp[12] = -(in[12] * tmp[0] + in[13] * tmp[4] + in[14] * tmp[8]);
+    tmp[13] = -(in[12] * tmp[1] + in[13] * tmp[5] + in[14] * tmp[9]);
+    tmp[14] = -(in[12] * tmp[2] + in[13] * tmp[6] + in[14] * tmp[10]);
+    memcpy(out, tmp, sizeof(tmp));
+}
+
 void CgeMat4fFromScale(float x, float y, float z, float out[16]) {
     CgeMat4fIdentity(out);
     out[0] = x;