DOCUMENTACIÓN AQUYNZA DEL DESARROLLADOR

00001 //===========================================================================
00002 //= matriz.cc                                                Agosto de 1998 =
00003 //=-------------------------------------------------------------------------=
00004 //= Definicion de la clase MATRIZ_4x4                                       =
00005 //=-------------------------------------------------------------------------=
00006 //= ADVERTENCIA: ESTE SOFTWARE NO ESTA CONCEBIDO NI DISENNADO PARA EL USO   =
00007 //= EN EQUIPO DE CONTROL EN LINEA EN ENTORNOS PELIGROSOS QUE REQUIERAN UN   =
00008 //= DESEMPENNO LIBRE DE FALLAS, COMO LA OPERACION DE PLANTAS NUCLEARES,     = 
00009 //= SISTEMAS DE NAVEGACION O COMUNICACION EN AVIONES, TRAFICO AEREO,        =
00010 //= EQUIPO MEDICO DEL CUAL DEPENDAN VIDAS HUMANAS O SISTEMAS DE ARMAMENTO,  =
00011 //= EN LOS CUALES UNA FALLA EN EL SOFTWARE PUEDA IMPLICAR DIRECTAMENTE LA   =
00012 //= MUERTE, DANNOS PERSONALES O DANNOS FISICOS Y/O AMBIENTALES GRAVES       =
00013 //= ("ACTIVIDADES DE ALGO RIESGO").                                         =
00014 //=-------------------------------------------------------------------------=
00015 //= Autor original: Oscar J. Chavarro G.  A.K.A. JEDILINK. Copyright (c),   =
00016 //= 1997 - 2003, oscarchavarro@hotmail.com                                  =
00017 //= AQUYNZA es software libre, y se rige bajo los terminos de la licencia   =
00018 //= LGPL de GNU (http://www.gnu.org). Para mayor informacion respecto a la  =
00019 //= licencia de uso, consulte el archivo ./doc/LICENCIA en la distribucion. =
00020 //===========================================================================
00021 
00022 #include<stdio.h>
00023 #include<math.h>
00024 #include "matriz4.h"
00025 #include "jed_gl.h"
00026 
00027 //===========================================================================
00028 //= CLASE MATRIZ_4x4                                                        =
00029 //===========================================================================
00030 
00031 //= OPERACIONES BASICAS =====================================================
00032 
00033 MATRIZ_4x4::MATRIZ_4x4()
00034 {
00035     identidad();
00036 }
00037 
00038 MATRIZ_4x4::~MATRIZ_4x4()
00039 {
00040     ;
00041 }
00042 
00043 // OJO: No hay un metodo `operator []` porque en el disenno se ha decidido que
00044 //      el atributo `M` (la matriz en si) es publico.  Notese que usar un
00045 //      metodo de acceso (el `operator []`) seria ineficiente!
00046 
00047 //= OPERACIONES ARITMETICAS =================================================
00048 
00049 MATRIZ_4x4
00050 MATRIZ_4x4::operator +(MATRIZ_4x4 b)
00054 {
00055     MATRIZ_4x4 resultado;
00056     int fila, columna;
00057 
00058     for( fila = 0; fila < 4 ; fila++ )
00059         for( columna = 0; columna < 4; columna++ )
00060             resultado.M[fila][columna] = M[fila][columna] + b.M[fila][columna];
00061 
00062     return resultado;
00063 }
00064 
00065 MATRIZ_4x4
00066 MATRIZ_4x4::operator *(MATRIZ_4x4 segunda)
00072 {
00073     MATRIZ_4x4 resultado;
00074     int fila_a, columna_b, fila_b;
00075     double acumulado;
00076 
00077     for( fila_a = 0; fila_a < 4; fila_a++ ) {
00078         for( columna_b = 0; columna_b < 4; columna_b++ ) {
00079             acumulado = 0;
00080             for( fila_b = 0; fila_b < 4; fila_b++ ) {
00081                 acumulado += M[fila_a][fila_b]*segunda.M[fila_b][columna_b];
00082             }
00083             resultado.M[fila_a][columna_b] = acumulado;
00084         }
00085     }
00086     return resultado;
00087 }
00088 
00089 //= OPERACIONES ESPECIFICAS DEL ALGEBRA DE MATRICES =========================
00090 
00091 void
00092 MATRIZ_4x4::identidad(void)
00093 /*
00094 (metodo revisado)
00095 */
00096 {
00097     M[0][0]=1.0;M[0][1]=0.0;M[0][2]=0.0;M[0][3]=0.0;
00098     M[1][0]=0.0;M[1][1]=1.0;M[1][2]=0.0;M[1][3]=0.0;
00099     M[2][0]=0.0;M[2][1]=0.0;M[2][2]=1.0;M[2][3]=0.0;
00100     M[3][0]=0.0;M[3][1]=0.0;M[3][2]=0.0;M[3][3]=1.0;
00101 }
00102 
00103 #ifdef NONONO
00104 MATRIZ_4x4
00105 MATRIZ_4x4::transpuesta(void)
00106 {
00107     MATRIZ_4x4 t;
00108     int i, j;
00109 
00110     for ( i = 0; i < 4; i++ ) {
00111         for ( j = 0; j < 4; j++ ) {
00112             t.M[j][i] = M[i][j];
00113         }
00114     }
00115 
00116     return t;
00117 }
00118 #endif
00119 
00120 MATRIZ_4x4
00121 MATRIZ_4x4::inversa(void)
00142 {
00143     MATRIZ_4x4 resultado;
00144 
00145     resultado.M[0][0] = M[1][1]*M[2][2] - M[2][1]*M[1][2];
00146     resultado.M[0][1] = M[0][2]*M[2][1] - M[2][2]*M[0][1];
00147     resultado.M[0][2] = M[0][1]*M[1][2] - M[1][1]*M[0][2];
00148     resultado.M[0][3] = 0;
00149 
00150     resultado.M[1][0] = M[1][2]*M[2][0] - M[2][2]*M[1][0];
00151     resultado.M[1][1] = M[0][0]*M[2][2] - M[2][0]*M[0][2];
00152     resultado.M[1][2] = M[0][2]*M[1][0] - M[1][2]*M[0][0];
00153     resultado.M[1][3] = 0;
00154 
00155     resultado.M[2][0] = M[1][0]*M[2][1] - M[2][0]*M[1][1];
00156     resultado.M[2][1] = M[0][1]*M[2][0] - M[2][1]*M[0][0];
00157     resultado.M[2][2] = M[0][0]*M[1][1] - M[1][0]*M[0][1];
00158     resultado.M[2][3] = 0;
00159 
00160     resultado.M[3][0] = 0;
00161     resultado.M[3][1] = 0;
00162     resultado.M[3][2] = 0;
00163     resultado.M[3][3] = 1;
00164 
00165     resultado = resultado * (1/determinante());
00166 
00167     return resultado;
00168 }
00169 
00170 double
00171 MATRIZ_4x4::determinante(void)
00179 {
00180     return M[0][0]*(M[1][1]*M[2][2]-M[2][1]*M[1][2]) - 
00181            M[0][1]*(M[1][0]*M[2][2]-M[2][0]*M[1][2]) + 
00182            M[0][2]*(M[1][0]*M[2][1]-M[2][0]*M[1][1]);
00183 }
00184 
00185 //= OPERACIONES GEOMETRICAS =================================================
00186 
00187 MATRIZ_4x4
00188 MATRIZ_4x4::operator *(double a)
00189 /*
00190 (metodo revisado)
00191 */
00192 {
00193     MATRIZ_4x4 resultado;
00194     int fila, columna;
00195 
00196     for( fila = 0; fila < 4 ; fila++ )
00197         for( columna = 0; columna < 4; columna++ )
00198             resultado.M[fila][columna] = a*M[fila][columna];
00199 
00200     return resultado;
00201 }
00202 
00203 void
00204 MATRIZ_4x4::translacion(double transx, double transy, double transz)
00211 {
00212     M[0][0]=1.0;M[0][1]=0.0;M[0][2]=0.0;M[0][3]=transx;
00213     M[1][0]=0.0;M[1][1]=1.0;M[1][2]=0.0;M[1][3]=transy;
00214     M[2][0]=0.0;M[2][1]=0.0;M[2][2]=1.0;M[2][3]=transz;
00215     M[3][0]=0.0;M[3][1]=0.0;M[3][2]=0.0;M[3][3]=1.0;
00216 }
00217 
00218 void
00219 MATRIZ_4x4::rotacion_angulos_euler(double yaw, double pitch, double roll)
00230 {
00231     MATRIZ_4x4 R1, R2, R3;
00232 
00233     R3.rotacion_eje(yaw, 0, 0, 1);
00234     R2.rotacion_eje(pitch, 0, -1, 0);
00235     R1.rotacion_eje(roll, 1, 0, 0);
00236 
00237     (*this) = R3 * (R2 * R1);
00238 }
00239 
00240 void
00241 MATRIZ_4x4::rotacion_eje(double angulo, double x, double y, double z)
00292 {
00293     double mag, s, c;
00294     double xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
00295 
00296     s = sin( angulo );
00297     c = cos( angulo );
00298 
00299     mag = sqrt(x*x + y*y + z*z);
00300 
00301     if (mag == 0.0) {
00302         identidad();
00303         return;
00304     }
00305 
00306     x /= mag;
00307     y /= mag;
00308     z /= mag;
00309 
00310     //----------------------------------------------------------------------
00311     xx = x * x;
00312     yy = y * y;
00313     zz = z * z;
00314     xy = x * y;
00315     yz = y * z;
00316     zx = z * x;
00317     xs = x * s;
00318     ys = y * s;
00319     zs = z * s;
00320     one_c = 1.0F - c;
00321 
00322     M[0][0] = (one_c * xx) + c;
00323     M[0][1] = (one_c * xy) - zs;
00324     M[0][2] = (one_c * zx) + ys;
00325     M[0][3] = 0.0F;
00326 
00327     M[1][0] = (one_c * xy) + zs;
00328     M[1][1] = (one_c * yy) + c;
00329     M[1][2] = (one_c * yz) - xs;
00330     M[1][3] = 0.0F;
00331 
00332     M[2][0] = (one_c * zx) - ys;
00333     M[2][1] = (one_c * yz) + xs;
00334     M[2][2] = (one_c * zz) + c;
00335     M[2][3] = 0.0F;
00336 
00337     M[3][0] = 0.0F;
00338     M[3][1] = 0.0F;
00339     M[3][2] = 0.0F;
00340     M[3][3] = 1.0F;
00341 }
00342 
00343 void
00344 MATRIZ_4x4::rotacion_punto_punto(VECTOR p1, VECTOR p2)
00350 {
00351     //- Configuracion de la ubicacion geometrica ---------------------------
00352     VECTOR v;
00353     MATRIZ_4x4 R1, R2;
00354     double yaw, pitch;
00355 
00356     v = p2 - p1;
00357     v.normalizar();
00358     v.direccion_euler(&yaw, &pitch);
00359 
00360     R1.rotacion_eje(DEG2RAD(90), 0, 1, 0);
00361     R2.rotacion_angulos_euler(yaw, pitch, 0);
00362     (*this) = R2 * R1;
00363 }
00364 
00365 //= OPERACIONES MATEMATICAS ESPECIALES ======================================
00366 
00367 void
00368 MATRIZ_4x4::importar_quaternion(QUATERNION a)
00382 {
00383     double sx, sy, sz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
00384 
00385     //- Calcule los coeficientes -------------------------------------------
00386     x2 = a.direccion.x + a.direccion.x; y2 = a.direccion.y + a.direccion.y; 
00387     z2 = a.direccion.z + a.direccion.z;
00388     xx = a.direccion.x * x2; xy = a.direccion.x * y2; xz = a.direccion.x * z2;
00389     yy = a.direccion.y * y2; yz = a.direccion.y * z2; zz = a.direccion.z * z2;
00390     sx = a.magnitud * x2;   sy = a.magnitud * y2;   sz = a.magnitud * z2;
00391 
00392     //- Llene la matriz ----------------------------------------------------
00393     M[0][0] = 1-(yy+zz); M[0][1] = xy-sz;     M[0][2] = xz+sy;     M[0][3] = 0;
00394     M[1][0] = xy+sz;     M[1][1] = 1-(xx+zz); M[1][2] = yz-sx;     M[1][3] = 0;
00395     M[2][0] = xz-sy;     M[2][1] = yz+sx;     M[2][2] = 1-(xx+yy); M[2][3] = 0;
00396     M[3][0] = 0;         M[3][1] = 0;         M[3][2] = 0;         M[3][3] = 1;
00397 }
00398 
00399 QUATERNION
00400 MATRIZ_4x4::exportar_quaternion(void)
00407 {
00408     QUATERNION quat;
00409 
00410     double  tr, s, q[4];
00411     int    i, j, k;
00412 
00413     int nxt[3];
00414 
00415     nxt[0] = 1; // No lo escriba en la linea de arriba, porque sino no le
00416     nxt[1] = 2; // gustara a los compiladores de CRAY
00417     nxt[2] = 0;
00418 
00419     tr = M[0][0] + M[1][1] + M[2][2];
00420 
00421     // check the diagonal
00422     if (tr > 0.0) {
00423         s = sqrt (tr + 1.0);
00424         quat.magnitud = s / 2.0;
00425         s = 0.5 / s;
00426         quat.direccion.x = (M[2][1] - M[1][2]) * s;
00427         quat.direccion.y = (M[0][2] - M[2][0]) * s;
00428         quat.direccion.z = (M[1][0] - M[0][1]) * s;
00429       }
00430       else {                
00431         // diagonal is negative
00432         i = 0;
00433         if (M[1][1] > M[0][0]) i = 1;
00434         if (M[2][2] > M[i][i]) i = 2;
00435         j = nxt[i];
00436         k = nxt[j];
00437 
00438         s = sqrt ((M[i][i] - (M[j][j] + M[k][k])) + 1.0);
00439 
00440         q[i] = s * 0.5;
00441 
00442         if (s != 0.0) s = 0.5 / s;
00443 
00444         q[3] = (M[k][j] - M[j][k]) * s;
00445         q[j] = (M[j][i] + M[i][j]) * s;
00446         q[k] = (M[k][i] + M[i][k]) * s;
00447 
00448         quat.direccion.x = q[0];
00449         quat.direccion.y = q[1];
00450         quat.direccion.z = q[2];
00451         quat.magnitud = q[3];
00452     }
00453 
00454     return quat;
00455 }
00456 
00457 #ifdef NONONO
00458 void
00459 MATRIZ_4x4::importar_asterisco(VECTOR a)
00460 {
00461     M[0][0]=0.0;  M[0][1]=-a.z; M[0][2]=-a.y; M[0][3]=0.0;
00462     M[1][0]=a.z;  M[1][1]=0.0;  M[1][2]=-a.x; M[1][3]=0.0;
00463     M[2][0]=-a.y; M[2][1]=a.x;  M[2][2]=0.0;  M[2][3]=0.0;
00464     M[3][0]=0.0;  M[3][1]=0.0;  M[3][2]=0.0;  M[3][3]=1.0;
00465 }
00466 #endif
00467 
00468 void
00469 MATRIZ_4x4::exportar_angulos_euler(double *yaw, double *pitch, double *roll)
00470 /*
00471 OJO: NI IDEA DE COMO HACER ESTO BIEN!
00472 
00473 Lo que se esta haciendo aqui es un machete horroroso... metodo "ad-hoc"
00474 */
00475 {
00476     VECTOR rayito;
00477 
00478     //- Pitch a lo bestia --------------------------------------------------
00479     rayito.x = 1;    rayito.y = 0;    rayito.z = 0;
00480     rayito = (*this)*rayito;
00481     rayito.normalizar();
00482     (*pitch) = DEG2RAD(90) - acos(rayito.z);
00483 
00484     //- Yaw a lo bestia ----------------------------------------------------
00485     rayito.x = 1;    rayito.y = rayito.z = 0;
00486     rayito = (*this)*rayito;
00487     rayito.z = 0;
00488     if ( fabs(DEG2RAD(90) - (*pitch)) < 4*EPSILON ) {
00489         rayito.x = 0;  rayito.y = 0;  rayito.z = -1;
00490         rayito = (*this)*rayito;
00491     }
00492     if ( fabs(DEG2RAD(-90) - (*pitch)) < 4*EPSILON ) {
00493         rayito.x = 0;  rayito.y = 0;  rayito.z = 1;
00494         rayito = (*this)*rayito;
00495     }
00496     rayito.normalizar();
00497     if ( rayito.y <= 0 ) (*yaw) = asin(rayito.x) - DEG2RAD(90);
00498     else (*yaw) = DEG2RAD(90) - asin(rayito.x);
00499 
00500     //- Roll a lo bestial --------------------------------------------------
00501     MATRIZ_4x4 R1, R2, R3;
00502 
00503     R3.rotacion_eje((*yaw), 0, 0, 1);
00504     R2.rotacion_eje((*pitch), 0, -1, 0);
00505     R1 = R2.inversa() * (R3.inversa() * (*this));
00506 
00507     QUATERNION q = R1.exportar_quaternion();  // Orthonormalice la matriz
00508     q.normalizar();
00509     R1.importar_quaternion(q);
00510 
00511     if ( R1.M[2][1] >= 0 ) {  // R1.M[2][1] ::= sin(r)
00512         // Nuestro angulo esta entre 0 y 180 grados
00513         (*roll) = acos(R1.M[1][1]);
00514       }
00515       else {
00516         // Nuestro angulo esta entre 180 y 360 grados
00517         (*roll) = -acos(R1.M[1][1]);
00518     }
00519 }
00520 
00521 //= OPERACIONES DE CONSULTA =================================================
00522 
00523 void
00524 MATRIZ_4x4::imprimir()
00525 {
00526     int fila, columna;
00527 
00528     printf("=================================\n");
00529     for( fila = 0; fila < 4; fila++ ) {
00530         for( columna = 0; columna < 4; columna++ ) {
00531             printf("(%d,%d)=%.2f\t", fila, columna, M[fila][columna]);
00532         }
00533         printf("\n");
00534     }
00535     printf("=================================\n");
00536     fflush(stdout);
00537 }
00538 
00539 #ifdef GL_ENABLED
00540 void
00541 MATRIZ_4x4::cargar_gl(void)
00568 {
00569     GLdouble Mgl[16];
00570     int fila, columna, pos;
00571 
00572     for ( pos = 0, columna = 0; columna < 4; columna++ ) {
00573         for ( fila = 0; fila < 4; fila++, pos++ ) {
00574             Mgl[pos] = M[fila][columna];
00575         }
00576     }
00577 
00578     glMultMatrixd(Mgl);
00579 }
00580 
00581 void
00582 MATRIZ_4x4::importar_gl(void)
00593 {
00594     GLdouble Mgl[16];
00595     int fila, columna, pos;
00596     GLint modo;
00597 
00598     glGetIntegerv(GL_MATRIX_MODE, &modo);
00599     if ( modo == GL_MODELVIEW ) glGetDoublev(GL_MODELVIEW_MATRIX, Mgl);
00600     else glGetDoublev(GL_PROJECTION_MATRIX, Mgl);
00601 
00602     for ( pos = 0, columna = 0; columna < 4; columna++ ) {
00603         for ( fila = 0; fila < 4; fila++, pos++ ) {
00604             M[fila][columna] = Mgl[pos];
00605         }
00606     }
00607 }
00608 #endif
00609 
00610 void
00611 MATRIZ_4x4::activar_povray(FILE *fd)
00612 {
00613     // OJO: Revisar esto!
00614     fprintf(fd,
00615         "    matrix <%f, %f, %f,\n"
00616         "            %f, %f, %f,\n"
00617         "            %f, %f, %f,\n"
00618         "            %f, %f, %f>\n",
00619         M[0][0], M[0][1], M[0][2],
00620         M[1][0], M[1][1], M[1][2],
00621         M[2][0], M[2][1], M[2][2],
00622         M[3][0], M[3][1], M[3][2]
00623     );
00624 }
00625 
00626 void
00627 MATRIZ_4x4::frustum(double plano_izquierdo, double plano_derecho,
00628                  double plano_inferior, double plano_superior,
00629                  double plano_cercano, double plano_lejano)
00654 {
00655     double A, B, C, D;
00656 
00657     A = (plano_derecho + plano_izquierdo) / (plano_derecho - plano_izquierdo);
00658     B = (plano_superior + plano_inferior) / (plano_superior - plano_inferior); 
00659     C = - ((plano_lejano + plano_cercano) / (plano_lejano - plano_cercano));
00660     D = - ((2 * plano_lejano * plano_cercano) / (plano_lejano - plano_cercano));
00661 
00662     M[0][0] = 2 * plano_cercano / (plano_derecho - plano_izquierdo);
00663     M[0][1] = 0;
00664     M[0][2] = A;
00665     M[0][3] = 0;
00666 
00667     M[1][0] = 0;
00668     M[1][1] = 2 * plano_cercano / (plano_superior - plano_inferior);
00669     M[1][2] = B;
00670     M[1][3] = 0;
00671 
00672     M[2][0] = 0;
00673     M[2][1] = 0;
00674     M[2][2] = C;
00675     M[2][3] = D;
00676 
00677     M[3][0] = 0;
00678     M[3][1] = 0;
00679     M[3][2] = -1;
00680     M[3][3] = 0;
00681 }
00682 
00683 void
00684 MATRIZ_4x4::ortho(double plano_izquierdo, double plano_derecho,
00685                  double plano_inferior, double plano_superior,
00686                  double plano_cercano, double plano_lejano)
00697 {
00698     double tx, ty, tz;;
00699 
00700     tx = - ( (plano_derecho + plano_izquierdo) / 
00701              (plano_derecho - plano_izquierdo) );
00702     ty = - ( (plano_superior + plano_inferior) / 
00703              (plano_superior - plano_inferior) );
00704     tz = - ( (plano_lejano + plano_cercano) / 
00705              (plano_lejano - plano_cercano) );
00706 
00707     M[0][0] = 2 / (plano_derecho - plano_izquierdo);
00708     M[0][1] = 0;
00709     M[0][2] = 0;
00710     M[0][3] = tx;
00711 
00712     M[1][0] = 0;
00713     M[1][1] = 2 / (plano_superior - plano_inferior);
00714     M[1][2] = 0;
00715     M[1][3] = ty;
00716 
00717     M[2][0] = 0;
00718     M[2][1] = 0;
00719     M[2][2] = -2 / (plano_lejano - plano_cercano);
00720     M[2][3] = tz;
00721 
00722     M[3][0] = 0;
00723     M[3][1] = 0;
00724     M[3][2] = 0;
00725     M[3][3] = 1;
00726 }
00727 
00728 //===========================================================================
00729 //= EOF                                                                     =
00730 //===========================================================================
00731
matriz4.C
