00001
00002 #include <stdio.h>
00003 #include <math.h>
00004 #include <vpmatrix.h>
00005 #include <vppoint.h>
00006
00007
00008 VPMatrix::VPMatrix( void ){
00009
00010
00011 data[0][1] = data[0][2] = data[0][3] =
00012 data[1][0] = data[1][2] = data[1][3] =
00013 data[2][0] = data[2][1] = data[2][3] =
00014 data[3][0] = data[3][1] = data[3][2] = 0.0;
00015 data[0][0] = data[1][1] = data[2][2] = data[3][3] = 1.0;
00016
00017 }
00018
00019 VPMatrix::VPMatrix( float fLines[] ){
00020
00021 for( int l = 0; l < 4; l++ ){
00022 for( int c = 0; c < 4; c++ ){
00023 data[l][c] = fLines[l*4+c];
00024 }
00025 }
00026 }
00027
00028 VPMatrix::VPMatrix( float fm44[][4] ){
00029
00030 for( int l = 0; l < 4; l++ ){
00031 for( int c = 0; c < 4; c++ ){
00032 data[l][c] = fm44[l][c];
00033 }
00034 }
00035 }
00036
00037
00038 VPMatrix::~VPMatrix( void ){
00039
00040 }
00041
00042 float
00043 VPMatrix::vpGetValueAt( int iL, int iC ){
00044
00045 return data[iL][iC];
00046 }
00047
00048 void
00049 VPMatrix::vpSetValueAt( int iL, int iC, float fValue ){
00050
00051 data[iL][iC] = fValue;
00052 }
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071 void
00072 VPMatrix::vpGetMatrixF( float **M ){
00073
00074 for( int l = 0; l < 4; l++ ){
00075 for( int c = 0; c < 4; c++ ){
00076 M[l][c] = data[c][l];
00077 }
00078 }
00079
00080
00081 }
00082
00083 float*
00084 VPMatrix::vpGetMatrixVF( void ){
00085
00086 float *M = new float[16];
00087
00088 for( int l = 0; l < 4; l++ ){
00089 for( int c = 0; c < 4; c++ ){
00090 M[l*4+c] = data[l][c];
00091 }
00092 }
00093 return M;
00094 }
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104 VPMatrix*
00105 VPMatrix :: vpGetInverse( void ){
00106
00107 VPMatrix *Inv;
00108 float fDet;
00109
00110
00111 Inv = vpGetAdjoint();
00112
00113
00114
00115 fDet = Inv->vpGetDeterminent();
00116 if (fabs(fDet) < 0.0001){
00117 printf("Non-singular matrix, no inverse!");
00118 return Inv;
00119 }
00120
00121 Inv->vpMultiplyScalar( 1/fDet );
00122
00123 return Inv;
00124 }
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138 VPMatrix*
00139 VPMatrix::vpGetAdjoint(){
00140
00141 VPMatrix *A = new VPMatrix();
00142 float a1, a2, a3, a4, b1, b2, b3, b4,
00143 c1, c2, c3, c4, d1, d2, d3, d4;
00144
00145
00146 a1 = data[0][0]; b1 = data[0][1];
00147 c1 = data[0][2]; d1 = data[0][3];
00148
00149 a2 = data[1][0]; b2 = data[1][1];
00150 c2 = data[1][2]; d2 = data[1][3];
00151
00152 a3 = data[2][0]; b3 = data[2][1];
00153 c3 = data[2][2]; d3 = data[2][3];
00154
00155 a4 = data[3][0]; b4 = data[3][1];
00156 c4 = data[3][2]; d4 = data[3][3];
00157
00158
00159 A->vpSetValueAt( 0, 0, vpGetDet33(b2, b3, b4, c2, c3, c4, d2, d3, d4) );
00160 A->vpSetValueAt( 1, 0, -vpGetDet33(a2, a3, a4, c2, c3, c4, d2, d3, d4) );
00161 A->vpSetValueAt( 2, 0, vpGetDet33(a2, a3, a4, b2, b3, b4, d2, d3, d4) );
00162 A->vpSetValueAt( 3, 0, -vpGetDet33(a2, a3, a4, b2, b3, b4, c2, c3, c4) );
00163
00164 A->vpSetValueAt( 0, 1, -vpGetDet33(b1, b3, b4, c1, c3, c4, d1, d3, d4) );
00165 A->vpSetValueAt( 1, 1, vpGetDet33(a1, a3, a4, c1, c3, c4, d1, d3, d4) );
00166 A->vpSetValueAt( 2, 1, -vpGetDet33(a1, a3, a4, b1, b3, b4, d1, d3, d4) );
00167 A->vpSetValueAt( 3, 1, vpGetDet33(a1, a3, a4, b1, b3, b4, c1, c3, c4) );
00168
00169 A->vpSetValueAt( 0, 2, vpGetDet33(b1, b2, b4, c1, c2, c4, d1, d2, d4) );
00170 A->vpSetValueAt( 1, 2, -vpGetDet33(a1, a2, a4, c1, c2, c4, d1, d2, d4) );
00171 A->vpSetValueAt( 2, 2, vpGetDet33(a1, a2, a4, b1, b2, b4, d1, d2, d4) );
00172 A->vpSetValueAt( 3, 2, -vpGetDet33(a1, a2, a4, b1, b2, b4, c1, c2, c4) );
00173
00174 A->vpSetValueAt( 0, 3, -vpGetDet33(b1, b2, b3, c1, c2, c3, d1, d2, d3) );
00175 A->vpSetValueAt( 1, 3, vpGetDet33(a1, a2, a3, c1, c2, c3, d1, d2, d3) );
00176 A->vpSetValueAt( 2, 3, -vpGetDet33(a1, a2, a3, b1, b2, b3, d1, d2, d3) );
00177 A->vpSetValueAt( 3, 3, vpGetDet33(a1, a2, a3, b1, b2, b3, c1, c2, c3) );
00178
00179 return A;
00180 }
00181
00182
00183
00184 float
00185 VPMatrix::vpGetDeterminent( void ){
00186
00187 float ans, a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
00188
00189
00190
00191 a1 = data[0][0]; b1 = data[0][1];
00192 c1 = data[0][2]; d1 = data[0][3];
00193
00194 a2 = data[1][0]; b2 = data[1][1];
00195 c2 = data[1][2]; d2 = data[1][3];
00196
00197 a3 = data[2][0]; b3 = data[2][1];
00198 c3 = data[2][2]; d3 = data[2][3];
00199
00200 a4 = data[3][0]; b4 = data[3][1];
00201 c4 = data[3][2]; d4 = data[3][3];
00202
00203 ans = a1 * vpGetDet33(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
00204 b1 * vpGetDet33(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
00205 c1 * vpGetDet33(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
00206 d1 * vpGetDet33(a2, a3, a4, b2, b3, b4, c2, c3, c4);
00207
00208 return ans;
00209 }
00210
00211
00212
00213
00214
00215
00216
00217
00218 float
00219 VPMatrix::vpGetDet33(float a1, float a2, float a3, float b1, float b2, float b3,
00220 float c1, float c2, float c3){
00221
00222 float ans;
00223 float aux1, aux2, aux3;
00224
00225 aux1 = vpGetDet22(b2, b3, c2, c3);
00226 aux2 = vpGetDet22(a2, a3, c2, c3);
00227 aux3 = vpGetDet22(a2, a3, b2, b3);
00228 ans = a1 * aux1 - b1 * aux2 + c1 * aux3;
00229
00230 return ans;
00231 }
00232
00233
00234
00235
00236
00237 float
00238 VPMatrix::vpGetDet22(float a, float b, float c, float d){
00239
00240 return (a * d - b * c);
00241 }
00242
00243
00245
00246
00247 VPMatrix*
00248 VPMatrix::vpMultiplyScalar( float x ){
00249
00250 VPMatrix *M = new VPMatrix();
00251
00252 for( int l = 0; l < 4; l++ ){
00253 for( int c = 0; c < 4; c++ ){
00254 M->vpSetValueAt( l, c, x * data[l][c] );
00255 }
00256 }
00257 return M;
00258 }
00259
00261
00262
00263 VPMatrix*
00264 VPMatrix::vpMultiply( VPMatrix B ){
00265
00266 VPMatrix *M = new VPMatrix();
00267
00268 M->vpSetValueAt( 0, 0, data[0][0]*B.vpGetValueAt( 0, 0 )+
00269 data[0][1]*B.vpGetValueAt( 1, 0 )+
00270 data[0][2]*B.vpGetValueAt( 2, 0 )+
00271 data[0][3]*B.vpGetValueAt( 3, 0 ) );
00272 M->vpSetValueAt( 0, 1, data[0][0]*B.vpGetValueAt( 0, 1 )+
00273 data[0][1]*B.vpGetValueAt( 1, 1 )+
00274 data[0][2]*B.vpGetValueAt( 2, 1 )+
00275 data[0][3]*B.vpGetValueAt( 3, 1 ) );
00276 M->vpSetValueAt( 0, 2, data[0][0]*B.vpGetValueAt( 0, 2 )+
00277 data[0][1]*B.vpGetValueAt( 1, 2 )+
00278 data[0][2]*B.vpGetValueAt( 2, 2 )+
00279 data[0][3]*B.vpGetValueAt( 3, 2 ) );
00280 M->vpSetValueAt( 0, 3, data[0][0]*B.vpGetValueAt( 0, 3 )+
00281 data[0][1]*B.vpGetValueAt( 1, 3 )+
00282 data[0][2]*B.vpGetValueAt( 2, 3 )+
00283 data[0][3]*B.vpGetValueAt( 3, 3 ) );
00284
00285 M->vpSetValueAt( 1, 0, data[1][0]*B.vpGetValueAt( 0, 0 )+
00286 data[1][1]*B.vpGetValueAt( 1, 0 )+
00287 data[1][2]*B.vpGetValueAt( 2, 0 )+
00288 data[1][3]*B.vpGetValueAt( 3, 0 ) );
00289 M->vpSetValueAt( 1, 1, data[1][0]*B.vpGetValueAt( 0, 1 )+
00290 data[1][1]*B.vpGetValueAt( 1, 1 )+
00291 data[1][2]*B.vpGetValueAt( 2, 1 )+
00292 data[1][3]*B.vpGetValueAt( 3, 1 ) );
00293 M->vpSetValueAt( 1, 2, data[1][0]*B.vpGetValueAt( 0, 2 )+
00294 data[1][1]*B.vpGetValueAt( 1, 2 )+
00295 data[1][2]*B.vpGetValueAt( 2, 2 )+
00296 data[1][3]*B.vpGetValueAt( 3, 2 ) );
00297 M->vpSetValueAt( 1, 3, data[1][0]*B.vpGetValueAt( 0, 3 )+
00298 data[1][1]*B.vpGetValueAt( 1, 3 )+
00299 data[1][2]*B.vpGetValueAt( 2, 3 )+
00300 data[1][3]*B.vpGetValueAt( 3, 3 ) );
00301
00302 M->vpSetValueAt( 2, 0, data[2][0]*B.vpGetValueAt( 0, 0 )+
00303 data[2][1]*B.vpGetValueAt( 1, 0 )+
00304 data[2][2]*B.vpGetValueAt( 2, 0 )+
00305 data[2][3]*B.vpGetValueAt( 3, 0 ) );
00306 M->vpSetValueAt( 2, 1, data[2][0]*B.vpGetValueAt( 0, 1 )+
00307 data[2][1]*B.vpGetValueAt( 1, 1 )+
00308 data[2][2]*B.vpGetValueAt( 2, 1 )+
00309 data[2][3]*B.vpGetValueAt( 3, 1 ) );
00310 M->vpSetValueAt( 2, 2, data[2][0]*B.vpGetValueAt( 0, 2 )+
00311 data[2][1]*B.vpGetValueAt( 1, 2 )+
00312 data[2][2]*B.vpGetValueAt( 2, 2 )+
00313 data[2][3]*B.vpGetValueAt( 3, 2 ) );
00314 M->vpSetValueAt( 2, 3, data[2][0]*B.vpGetValueAt( 0, 3 )+
00315 data[2][1]*B.vpGetValueAt( 1, 3 )+
00316 data[2][2]*B.vpGetValueAt( 2, 3 )+
00317 data[2][3]*B.vpGetValueAt( 3, 3 ) );
00318
00319 M->vpSetValueAt( 3, 0, data[3][0]*B.vpGetValueAt( 0, 0 )+
00320 data[3][1]*B.vpGetValueAt( 1, 0 )+
00321 data[3][2]*B.vpGetValueAt( 2, 0 )+
00322 data[3][3]*B.vpGetValueAt( 3, 0 ) );
00323 M->vpSetValueAt( 3, 1, data[3][0]*B.vpGetValueAt( 0, 1 )+
00324 data[3][1]*B.vpGetValueAt( 1, 1 )+
00325 data[3][2]*B.vpGetValueAt( 2, 1 )+
00326 data[3][3]*B.vpGetValueAt( 3, 1 ) );
00327 M->vpSetValueAt( 3, 2, data[3][0]*B.vpGetValueAt( 0, 2 )+
00328 data[3][1]*B.vpGetValueAt( 1, 2 )+
00329 data[3][2]*B.vpGetValueAt( 2, 2 )+
00330 data[3][3]*B.vpGetValueAt( 3, 2 ) );
00331 M->vpSetValueAt( 3, 3, data[3][0]*B.vpGetValueAt( 0, 3 )+
00332 data[3][1]*B.vpGetValueAt( 1, 3 )+
00333 data[3][2]*B.vpGetValueAt( 2, 3 )+
00334 data[3][3]*B.vpGetValueAt( 3, 3 ) );
00335
00336 return M;
00337 }
00338
00339 VPMatrix*
00340 VPMatrix::vpSubtract( VPMatrix B ){
00341
00342 VPMatrix *M = new VPMatrix();
00343
00344 for( int l = 0; l < 4; l++ ){
00345 for( int c = 0; c < 4; c++ ){
00346 M->vpSetValueAt( l, c,
00347 data[l][c] - B.vpGetValueAt( l, c ) );
00348 }
00349 }
00350 return M;
00351 }
00352
00353 VPMatrix*
00354 VPMatrix::vpAdd( VPMatrix B ){
00355
00356 VPMatrix *M = new VPMatrix();
00357
00358 for( int l = 0; l < 4; l++ ){
00359 for( int c = 0; c < 4; c++ ){
00360 M->vpSetValueAt( l, c,
00361 data[l][c] + B.vpGetValueAt( l, c ) );
00362 }
00363 }
00364 return M;
00365 }
00366
00367 VPPoint3D*
00368 VPMatrix::vpMultiply( VPPoint3D p ){
00369
00370 VPPoint3D *temp = new VPPoint3D();
00371 float x,y,z,w;
00372
00373 w = data[3][0] * p.vpGetX() + data[3][1] * p.vpGetY() +
00374 data[3][2] * p.vpGetZ() + data[3][3] * 1;
00375 x = (data[0][0] * p.vpGetX() + data[0][1] * p.vpGetY() +
00376 data[0][2] * p.vpGetZ() + data[0][3] * 1)/w;
00377 y = (data[1][0] * p.vpGetX() + data[1][1] * p.vpGetY() +
00378 data[1][2] * p.vpGetZ() + data[1][3] * 1)/w;
00379 z = (data[2][0] * p.vpGetX() + data[2][1] * p.vpGetY() +
00380 data[2][2] * p.vpGetZ() + data[2][3] * 1)/w;
00381
00382 temp->vpSetXYZ(x,y,z);
00383
00384 return temp;
00385 }