00001
00002
00003
00004
00005 *
00006 * input:
00007 * mat - pointer to array of 16 floats (source matrix)
00008 * output:
00009 * dst - pointer to array of 16 floats (invert matrix)
00010 * *************************************************************/
00011
00012 void Invert2( float *mat, float *dst)
00013 {
00014 float tmp[12];
00015 float src[16];
00016 float det;
00017
00018
00019 for ( int i = 0; i < 4; i++) {
00020 src[i] = mat[i*4];
00021 src[i + 4] = mat[i*4 + 1];
00022 src[i + 8] = mat[i*4 + 2];
00023 src[i + 12] = mat[i*4 + 3];
00024 }
00025
00026
00027 tmp[0] = src[10] * src[15];
00028 tmp[1] = src[11] * src[14];
00029 tmp[2] = src[9] * src[15];
00030 tmp[3] = src[11] * src[13];
00031 tmp[4] = src[9] * src[14];
00032 tmp[5] = src[10] * src[13];
00033 tmp[6] = src[8] * src[15];
00034 tmp[7] = src[11] * src[12];
00035 tmp[8] = src[8] * src[14];
00036 tmp[9] = src[10] * src[12];
00037 tmp[10] = src[8] * src[13];
00038 tmp[11] = src[9] * src[12];
00039
00040
00041 dst[0] = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
00042 dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
00043 dst[1] = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
00044 dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
00045 dst[2] = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
00046 dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
00047 dst[3] = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
00048 dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
00049 dst[4] = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
00050 dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
00051 dst[5] = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
00052 dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
00053 dst[6] = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
00054 dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
00055 dst[7] = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
00056 dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
00057
00058
00059 tmp[0] = src[2]*src[7];
00060 tmp[1] = src[3]*src[6];
00061 tmp[2] = src[1]*src[7];
00062 tmp[3] = src[3]*src[5];
00063 tmp[4] = src[1]*src[6];
00064 tmp[5] = src[2]*src[5];
00065 tmp[6] = src[0]*src[7];
00066 tmp[7] = src[3]*src[4];
00067 tmp[8] = src[0]*src[6];
00068 tmp[9] = src[2]*src[4];
00069 tmp[10] = src[0]*src[5];
00070 tmp[11] = src[1]*src[4];
00071
00072
00073 dst[8] = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
00074 dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
00075 dst[9] = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
00076 dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
00077 dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
00078 dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
00079 dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
00080 dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
00081 dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
00082 dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
00083 dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
00084 dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
00085 dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
00086 dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
00087 dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
00088 dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
00089
00090
00091 det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
00092
00093
00094 det = 1/det;
00095 for ( int j = 0; j < 16; j++)
00096 dst[j] *= det;
00097 }