Main Page   Modules   Namespace List   Class Hierarchy   Alphabetical List   Compound List   File List   Namespace Members   Compound Members   File Members  

inverse.cpp

Go to the documentation of this file.
00001 // I grabbed this from somewhere that I've now forgotten...
00002 // it badly needs to be replaced, in any case
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]; /* temp array for pairs */
00015    float src[16]; /* array of transpose source matrix */
00016    float det; /* determinant*/
00017 
00018    /* transpose matrix */
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    /* calculate pairs for first 8 elements (cofactors) */
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    /* calculate first 8 elements (cofactors) */
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    /* calculate pairs for second 8 elements (cofactors) */
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    /* calculate second 8 elements (cofactors) */
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    /* calculate determinant */
00091    det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
00092 
00093    /* calculate matrix inverse */
00094    det = 1/det;
00095    for ( int j = 0; j < 16; j++)
00096       dst[j] *= det;
00097 }

ps2gl version cvs