/* Test code for comparison of several Mesa matrix inverse functions. * Compile with (for maximum speed): * * gcc -o inv inv.c -O6 -ffast-math */ #include #include #include typedef double GLdouble; #undef GL_FALSE #define GL_FALSE 0 #undef GL_TRUE #define GL_TRUE 1 __inline__ unsigned long long int rdtsc() { unsigned long long int counter; __asm__ volatile (".byte 0x0f, 0x31" : "=A" (counter)); return counter; } void printMatrix(GLdouble m[16]) { printf("%f, %f, %f, %f\n", m[0+4*0], m[0+4*1], m[0+4*2], m[0+4*3]); printf("%f, %f, %f, %f\n", m[1+4*0], m[1+4*1], m[1+4*2], m[1+4*3]); printf("%f, %f, %f, %f\n", m[2+4*0], m[2+4*1], m[2+4*2], m[2+4*3]); printf("%f, %f, %f, %f\n\n", m[3+4*0], m[3+4*1], m[3+4*2], m[3+4*3]); } /* ** Make m an identity matrix */ static void __gluMakeIdentityd(GLdouble m[16]) { m[0+4*0] = 1; m[0+4*1] = 0; m[0+4*2] = 0; m[0+4*3] = 0; m[1+4*0] = 0; m[1+4*1] = 1; m[1+4*2] = 0; m[1+4*3] = 0; m[2+4*0] = 0; m[2+4*1] = 0; m[2+4*2] = 1; m[2+4*3] = 0; m[3+4*0] = 0; m[3+4*1] = 0; m[3+4*2] = 0; m[3+4*3] = 1; } /* The original Mesa inverse from pre-2006 */ static int old__gluInvertMatrixd(const GLdouble src[16], GLdouble inverse[16]) { int i, j, k, swap; double t; GLdouble temp[4][4]; for (i=0; i<4; i++) { for (j=0; j<4; j++) { temp[i][j] = src[i*4+j]; } } __gluMakeIdentityd(inverse); for (i = 0; i < 4; i++) { /* ** Look for largest element in column */ swap = i; for (j = i + 1; j < 4; j++) { if (fabs(temp[j][i]) > fabs(temp[i][i])) { swap = j; } } if (swap != i) { /* ** Swap rows. */ for (k = 0; k < 4; k++) { t = temp[i][k]; temp[i][k] = temp[swap][k]; temp[swap][k] = t; t = inverse[i*4+k]; inverse[i*4+k] = inverse[swap*4+k]; inverse[swap*4+k] = t; } } if (temp[i][i] == 0) { /* ** No non-zero pivot. The matrix is singular, which shouldn't ** happen. This means the user gave us a bad matrix. */ return GL_FALSE; } t = temp[i][i]; for (k = 0; k < 4; k++) { temp[i][k] /= t; inverse[i*4+k] /= t; } for (j = 0; j < 4; j++) { if (j != i) { t = temp[j][i]; for (k = 0; k < 4; k++) { temp[j][k] -= temp[i][k]*t; inverse[j*4+k] -= inverse[i*4+k]*t; } } } } return GL_TRUE; } /* Shan Hao Bo's inverse from 2006. Has been in Mesa ever since. */ static int shanhaobo__gluInvertMatrixd(const GLdouble src[16], GLdouble inverse[16]) { int i, j, k; double t; GLdouble temp[4][4]; for (i=0; i<4; i++) { for (j=0; j<4; j++) { temp[i][j] = src[i*4+j]; } } __gluMakeIdentityd(inverse); for (i = 0; i < 4; i++) { if (temp[i][i] == 0.0f) { /* ** Look for non-zero element in column */ for (j = i + 1; j < 4; j++) { if (temp[j][i] != 0.0f) { break; } } if (j != 4) { /* ** Swap rows. */ for (k = 0; k < 4; k++) { t = temp[i][k]; temp[i][k] = temp[j][k]; temp[j][k] = t; t = inverse[i*4+k]; inverse[i*4+k] = inverse[j*4+k]; inverse[j*4+k] = t; } } else { /* ** No non-zero pivot. The matrix is singular, which shouldn't ** happen. This means the user gave us a bad matrix. */ return GL_FALSE; } } t = 1.0f / temp[i][i]; for (k = 0; k < 4; k++) { temp[i][k] *= t; inverse[i*4+k] *= t; } for (j = 0; j < 4; j++) { if (j != i) { t = temp[j][i]; for (k = 0; k < 4; k++) { temp[j][k] -= temp[i][k]*t; inverse[j*4+k] -= inverse[i*4+k]*t; } } } } return GL_TRUE; } /* Suggested replacement for both of the above. Calculates the 4x4 * exactly with no need for any pivoting or looping. Will be faster * than both of the above in most cases (may need -ffast-math to be * faster in some cases). */ static int __gluInvertMatrixd (const GLdouble m[16], GLdouble inv[16]) { double det; int i; inv[0] = m[5]*m[10]*m[15] - m[5]*m[11]*m[14] - m[9]*m[6]*m[15] + m[9]*m[7]*m[14] + m[13]*m[6]*m[11] - m[13]*m[7]*m[10]; inv[4] = -m[4]*m[10]*m[15] + m[4]*m[11]*m[14] + m[8]*m[6]*m[15] - m[8]*m[7]*m[14] - m[12]*m[6]*m[11] + m[12]*m[7]*m[10]; inv[8] = m[4]*m[9]*m[15] - m[4]*m[11]*m[13] - m[8]*m[5]*m[15] + m[8]*m[7]*m[13] + m[12]*m[5]*m[11] - m[12]*m[7]*m[9]; inv[12] = -m[4]*m[9]*m[14] + m[4]*m[10]*m[13] + m[8]*m[5]*m[14] - m[8]*m[6]*m[13] - m[12]*m[5]*m[10] + m[12]*m[6]*m[9]; inv[1] = -m[1]*m[10]*m[15] + m[1]*m[11]*m[14] + m[9]*m[2]*m[15] - m[9]*m[3]*m[14] - m[13]*m[2]*m[11] + m[13]*m[3]*m[10]; inv[5] = m[0]*m[10]*m[15] - m[0]*m[11]*m[14] - m[8]*m[2]*m[15] + m[8]*m[3]*m[14] + m[12]*m[2]*m[11] - m[12]*m[3]*m[10]; inv[9] = -m[0]*m[9]*m[15] + m[0]*m[11]*m[13] + m[8]*m[1]*m[15] - m[8]*m[3]*m[13] - m[12]*m[1]*m[11] + m[12]*m[3]*m[9]; inv[13] = m[0]*m[9]*m[14] - m[0]*m[10]*m[13] - m[8]*m[1]*m[14] + m[8]*m[2]*m[13] + m[12]*m[1]*m[10] - m[12]*m[2]*m[9]; inv[2] = m[1]*m[6]*m[15] - m[1]*m[7]*m[14] - m[5]*m[2]*m[15] + m[5]*m[3]*m[14] + m[13]*m[2]*m[7] - m[13]*m[3]*m[6]; inv[6] = -m[0]*m[6]*m[15] + m[0]*m[7]*m[14] + m[4]*m[2]*m[15] - m[4]*m[3]*m[14] - m[12]*m[2]*m[7] + m[12]*m[3]*m[6]; inv[10] = m[0]*m[5]*m[15] - m[0]*m[7]*m[13] - m[4]*m[1]*m[15] + m[4]*m[3]*m[13] + m[12]*m[1]*m[7] - m[12]*m[3]*m[5]; inv[14] = -m[0]*m[5]*m[14] + m[0]*m[6]*m[13] + m[4]*m[1]*m[14] - m[4]*m[2]*m[13] - m[12]*m[1]*m[6] + m[12]*m[2]*m[5]; inv[3] = -m[1]*m[6]*m[11] + m[1]*m[7]*m[10] + m[5]*m[2]*m[11] - m[5]*m[3]*m[10] - m[9]*m[2]*m[7] + m[9]*m[3]*m[6]; inv[7] = m[0]*m[6]*m[11] - m[0]*m[7]*m[10] - m[4]*m[2]*m[11] + m[4]*m[3]*m[10] + m[8]*m[2]*m[7] - m[8]*m[3]*m[6]; inv[11] = -m[0]*m[5]*m[11] + m[0]*m[7]*m[9] + m[4]*m[1]*m[11] - m[4]*m[3]*m[9] - m[8]*m[1]*m[7] + m[8]*m[3]*m[5]; inv[15] = m[0]*m[5]*m[10] - m[0]*m[6]*m[9] - m[4]*m[1]*m[10] + m[4]*m[2]*m[9] + m[8]*m[1]*m[6] - m[8]*m[2]*m[5]; det = m[0]*inv[0] + m[1]*inv[4] + m[2]*inv[8] + m[3]*inv[12]; if (det == 0) return GL_FALSE; for (i = 0; i < 16; i++) inv[i] /= det; return GL_TRUE; } int main(int argc, char *argv[]) { GLdouble mat[16] = { -3.43e-17, 1.71, 0.174, 0.174, -1.42, -3.156e-17, -3.61e-17, -3.608e-17, 4.58e-17, 0.302, -0.985, -0.985, 5.59e-31, -3.08e-15, 49.8, 50, }; GLdouble inv[16], r[16]; printf ("Starting with matrix:\n"); printMatrix(mat); printf("Inverse using original pre-2006 Mesa function:\n"); if (old__gluInvertMatrixd(mat, inv) == GL_FALSE) printf("this is singular matrix\n"); else printMatrix(inv); printf("Inverse using Shan Hao Bo function (2006 onward):\n"); if (shanhaobo__gluInvertMatrixd(mat, inv) == GL_FALSE) printf("this is singular matrix\n"); else printMatrix(inv); printf("Inverse using Moore's proposed function:\n"); if (__gluInvertMatrixd(mat, inv) == GL_FALSE) printf("this is singular matrix\n"); else printMatrix(inv); unsigned long long int x; unsigned long long int oc = 0; unsigned long long int bc = 0; unsigned long long int mc = 0; int i = 0; for (i = 0; i < 100; i++) { x = rdtsc(); old__gluInvertMatrixd(mat, inv); oc += (rdtsc() - x); x = rdtsc(); shanhaobo__gluInvertMatrixd(mat, inv); bc += (rdtsc() - x); x = rdtsc(); __gluInvertMatrixd (mat, inv); mc += (rdtsc() - x); } printf("\nold function: %d ticks\n", oc); printf("\nShan Hao Bo function: %d ticks\n", bc); printf("\nMoore function: %d ticks\n", mc); return 0; }