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