00001 #include "string.h"
00002
00003
00004
00005
00006
00007
00008
00009
00010 template<class T>
00011 void Multiply( const T* M, const T* v, T* r )
00012 {
00013 *(r++) = M[0]*v[0] + M[1]*v[1] + M[2]*v[2] + M[3]*v[3];
00014 *(r++) = M[4]*v[0] + M[5]*v[1] + M[6]*v[2] + M[7]*v[3];
00015 *(r++) = M[8]*v[0] + M[9]*v[1] + M[10]*v[2] + M[11]*v[3];
00016 *(r++) = M[12]*v[0] + M[13]*v[1] + M[14]*v[2] + M[15]*v[3];
00017 }
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 template<class T>
00033 void Invert( const T *pb, T *pa)
00034 {
00035 T a[4][4];
00036 T b[4][4];
00037 memcpy( a, pa, sizeof( T ) * 16);
00038 memcpy( b, pb, sizeof( T ) * 16);
00039
00040 long indxc[4], indxr[4], ipiv[4];
00041 long i, icol=0, irow=0, j, ir, ic;
00042 float big, dum, pivinv, temp, bb;
00043 ipiv[0] = -1; ipiv[1] = -1; ipiv[2] = -1; ipiv[3] = -1;
00044 a[0][0] = b[0][0];
00045 a[1][0] = b[1][0];
00046 a[2][0] = b[2][0];
00047 a[3][0] = b[3][0];
00048 a[0][1] = b[0][1];
00049 a[1][1] = b[1][1];
00050 a[2][1] = b[2][1];
00051 a[3][1] = b[3][1];
00052 a[0][2] = b[0][2];
00053 a[1][2] = b[1][2];
00054 a[2][2] = b[2][2];
00055 a[3][2] = b[3][2];
00056 a[0][3] = b[0][3];
00057 a[1][3] = b[1][3];
00058 a[2][3] = b[2][3];
00059 a[3][3] = b[3][3];
00060 for (i = 0; i < 4; i++) {
00061 big = 0.0f;
00062 for (j = 0; j < 4; j++) {
00063 if (ipiv[j] != 0) {
00064 if (ipiv[0] == -1) {
00065 if ((bb = ( float) fabs(a[j][0])) > big) {
00066 big = bb; irow = j; icol = 0;
00067 }
00068 }
00069 else if (ipiv[0] > 0) {
00070 memcpy( pa, a, 16*sizeof( T ));
00071 return;
00072 }
00073
00074 if (ipiv[1] == -1) {
00075 if ((bb = ( float) fabs(( float) a[j][1])) > big) {
00076 big = bb; irow = j; icol = 1;
00077 }
00078 }
00079 else if (ipiv[1] > 0) {
00080 memcpy( pa, a, 16*sizeof( T ));
00081 return;
00082 }
00083
00084 if (ipiv[2] == -1) {
00085 if ((bb = ( float) fabs(( float) a[j][2])) > big) {
00086 big = bb; irow = j; icol = 2;
00087 }
00088 }
00089 else if (ipiv[2] > 0) {
00090 memcpy( pa, a, 16*sizeof( T ));
00091 return;
00092 }
00093
00094 if (ipiv[3] == -1) {
00095 if ((bb = ( float) fabs(( float) a[j][3])) > big) {
00096 big = bb; irow = j; icol = 3;
00097 }
00098 }
00099 else if (ipiv[3] > 0) {
00100 memcpy( pa, a, 16*sizeof( T ));
00101 return;
00102 }
00103 }
00104 }
00105 ++(ipiv[icol]);
00106
00107 if (irow != icol) {
00108 temp = a[irow][0]; a[irow][0] = a[icol][0]; a[icol][0] = temp;
00109 temp = a[irow][1]; a[irow][1] = a[icol][1]; a[icol][1] = temp;
00110 temp = a[irow][2]; a[irow][2] = a[icol][2]; a[icol][2] = temp;
00111 temp = a[irow][3]; a[irow][3] = a[icol][3]; a[icol][3] = temp;
00112 }
00113
00114 indxr[i] = irow; indxc[i] = icol;
00115 if (a[icol][icol] == 0.0) {
00116 memcpy( pa, a, 16*sizeof( T ));
00117 return;
00118 }
00119
00120 pivinv = 1.0f / a[icol][icol];
00121 a[icol][icol] = 1.0f;
00122 a[icol][0] *= pivinv;
00123 a[icol][1] *= pivinv;
00124 a[icol][2] *= pivinv;
00125 a[icol][3] *= pivinv;
00126
00127 if (icol != 0) {
00128 dum = a[0][icol];
00129 a[0][icol] = 0.0f;
00130 a[0][0] -= a[icol][0] * dum;
00131 a[0][1] -= a[icol][1] * dum;
00132 a[0][2] -= a[icol][2] * dum;
00133 a[0][3] -= a[icol][3] * dum;
00134 }
00135
00136 if (icol != 1) {
00137 dum = a[1][icol];
00138 a[1][icol] = 0.0f;
00139 a[1][0] -= a[icol][0] * dum;
00140 a[1][1] -= a[icol][1] * dum;
00141 a[1][2] -= a[icol][2] * dum;
00142 a[1][3] -= a[icol][3] * dum;
00143 }
00144
00145 if (icol != 2) {
00146 dum = a[2][icol];
00147 a[2][icol] = 0.0f;
00148 a[2][0] -= a[icol][0] * dum;
00149 a[2][1] -= a[icol][1] * dum;
00150 a[2][2] -= a[icol][2] * dum;
00151 a[2][3] -= a[icol][3] * dum;
00152 }
00153
00154 if (icol != 3) {
00155 dum = a[3][icol];
00156 a[3][icol] = 0.0f;
00157 a[3][0] -= a[icol][0] * dum;
00158 a[3][1] -= a[icol][1] * dum;
00159 a[3][2] -= a[icol][2] * dum;
00160 a[3][3] -= a[icol][3] * dum;
00161 }
00162 }
00163
00164 if (indxr[3] != indxc[3]) {
00165 ir = indxr[3]; ic = indxc[3];
00166 temp = a[0][ir]; a[0][ir] = a[0][ic]; a[0][ic] = temp;
00167 temp = a[1][ir]; a[1][ir] = a[1][ic]; a[1][ic] = temp;
00168 temp = a[2][ir]; a[2][ir] = a[2][ic]; a[2][ic] = temp;
00169 temp = a[3][ir]; a[3][ir] = a[3][ic]; a[3][ic] = temp;
00170 }
00171
00172 if (indxr[2] != indxc[2]) {
00173 ir = indxr[2]; ic = indxc[2];
00174 temp = a[0][ir]; a[0][ir] = a[0][ic]; a[0][ic] = temp;
00175 temp = a[1][ir]; a[1][ir] = a[1][ic]; a[1][ic] = temp;
00176 temp = a[2][ir]; a[2][ir] = a[2][ic]; a[2][ic] = temp;
00177 temp = a[3][ir]; a[3][ir] = a[3][ic]; a[3][ic] = temp;
00178 }
00179
00180 if (indxr[1] != indxc[1]) {
00181 ir = indxr[1]; ic = indxc[1];
00182 temp = a[0][ir]; a[0][ir] = a[0][ic]; a[0][ic] = temp;
00183 temp = a[1][ir]; a[1][ir] = a[1][ic]; a[1][ic] = temp;
00184 temp = a[2][ir]; a[2][ir] = a[2][ic]; a[2][ic] = temp;
00185 temp = a[3][ir]; a[3][ir] = a[3][ic]; a[3][ic] = temp;
00186 }
00187
00188 if (indxr[0] != indxc[0]) {
00189 ir = indxr[0]; ic = indxc[0];
00190 temp = a[0][ir]; a[0][ir] = a[0][ic]; a[0][ic] = temp;
00191 temp = a[1][ir]; a[1][ir] = a[1][ic]; a[1][ic] = temp;
00192 temp = a[2][ir]; a[2][ir] = a[2][ic]; a[2][ic] = temp;
00193 temp = a[3][ir]; a[3][ir] = a[3][ic]; a[3][ic] = temp;
00194 }
00195
00196 memcpy( pa, a, 16*sizeof( T ));
00197 }
00198
00199
00200
