cosa_rigida.C

Ir a la documentación de este archivo.

//===========================================================================
//= cosa_rigida.C                                             Julio de 1998 =
//=-------------------------------------------------------------------------=
//= Definiciones de la clase COSA                                           =
//=-------------------------------------------------------------------------=
//= ADVERTENCIA: ESTE SOFTWARE NO ESTA CONCEBIDO NI DISENNADO PARA EL USO   =
//= EN EQUIPO DE CONTROL EN LINEA EN ENTORNOS PELIGROSOS QUE REQUIERAN UN   =
//= DESEMPENNO LIBRE DE FALLAS, COMO LA OPERACION DE PLANTAS NUCLEARES,     = 
//= SISTEMAS DE NAVEGACION O COMUNICACION EN AVIONES, TRAFICO AEREO,        =
//= EQUIPO MEDICO DEL CUAL DEPENDAN VIDAS HUMANAS O SISTEMAS DE ARMAMENTO,  =
//= EN LOS CUALES UNA FALLA EN EL SOFTWARE PUEDA IMPLICAR DIRECTAMENTE LA   =
//= MUERTE, DANNOS PERSONALES O DANNOS FISICOS Y/O AMBIENTALES GRAVES       =
//= ("ACTIVIDADES DE ALGO RIESGO").                                         =
//=-------------------------------------------------------------------------=
//= Autor original: Oscar J. Chavarro G.  A.K.A. JEDILINK. Copyright (c),   =
//= 1997 - 2003, oscarchavarro@hotmail.com                                  =
//= AQUYNZA es software libre, y se rige bajo los terminos de la licencia   =
//= LGPL de GNU (http://www.gnu.org). Para mayor informacion respecto a la  =
//= licencia de uso, consulte el archivo ./doc/LICENCIA en la distribucion. =
//===========================================================================

#include "jed_gl.h"
#include "core/cosas/cosa.h"
#include "lista.cc"
#include "toolkits/media/jed_img.h"
#include "toolkits/geom/mesh.h"
#include "toolkits/util/3dsload.h"
#include "matriz4.h"

#include <ctype.h>
#include <string.h>
#include <stdlib.h>

//===========================================================================
//= VARIABLES GLOBALES Y MACROS                                             =
//===========================================================================

#define ESPERO(tipo, msg) \
    if ( tipo_token != (tipo) ) { \
        fprintf(stderr, "<COSA_RIGIDA> ERROR: Esperaba %s y recibi [%s].\n", \
          (msg), cad); fflush(stderr);  return FALSE; \
    }

#define GENERAR_ESQUINAS(e)                                     \
    (e)[0].x = mmi.x;    (e)[0].y = mmi.y;    (e)[0].z = mmi.z; \
    (e)[1].x = mmx.x;    (e)[1].y = mmi.y;    (e)[1].z = mmi.z; \
    (e)[2].x = mmi.x;    (e)[2].y = mmi.y;    (e)[2].z = mmx.z; \
    (e)[3].x = mmx.x;    (e)[3].y = mmi.y;    (e)[3].z = mmx.z; \
    (e)[4].x = mmi.x;    (e)[4].y = mmx.y;    (e)[4].z = mmi.z; \
    (e)[5].x = mmx.x;    (e)[5].y = mmx.y;    (e)[5].z = mmi.z; \
    (e)[6].x = mmi.x;    (e)[6].y = mmx.y;    (e)[6].z = mmx.z; \
    (e)[7].x = mmx.x;    (e)[7].y = mmx.y;    (e)[7].z = mmx.z;

//===========================================================================
//= Clase COSA_RIGIDA                                                       =
//===========================================================================

COSA_RIGIDA::COSA_RIGIDA() : COSA()
{
    float magnitud = 0;
    VECTOR k(0, 0, 1);

    _tipo_de_cosa = CC_COSA_RIGIDA;

    _orientacion.importar_angulo_eje(magnitud, k);

    _velocidad_angular.x = 0;
    _velocidad_angular.y = 0;
    _velocidad_angular.z = 0;

    _centro_de_masa.x = 0;
    _centro_de_masa.y = 0;
    _centro_de_masa.z = 0;

    // OJO: Aqui toca asignar un tensor de inercia por defecto!
    tensor_de_inercia_0.identidad(); // Esto es para un cubo unitario de masa
    tensor_de_inercia_0 = tensor_de_inercia_0 * (1.0/12.0); // total 1 y con
                                     // distribucion de masa uniforme!

    Geometria = NULL;
    _ultimo_Kd = 0.1;
}

COSA_RIGIDA::~COSA_RIGIDA()
{
    if ( _nombre_padre ) delete _nombre_padre;
}

GEOMETRIA *
COSA_RIGIDA::geometria(void)
{
    return Geometria;
}

static QUATERNION TMP_quaternion;  // Espacio de intercambio en introspeccion
static VECTOR TMP_vector;
static double TMP_float;

BOOLEAN
COSA_RIGIDA::consultar_variable(const char *nombre_variable, int &tipo, void **ref)
{
    double yaw, pitch, roll;
    MATRIZ_4x4 R;

    if ( strcmp(nombre_variable, "orientacion_absoluta_euler") == 0 ) {
        R.importar_quaternion(orientacion_absoluta());
        R.exportar_angulos_euler(&yaw, &pitch, &roll);
        TMP_vector.x = yaw;
        TMP_vector.y = pitch;
        TMP_vector.z = roll;
        tipo = T_VECTOR;(*ref) = &TMP_vector;    return TRUE;
      }
      else if ( strcmp(nombre_variable, "yaw") == 0 ) {
        R.importar_quaternion(orientacion_absoluta());
        R.exportar_angulos_euler(&yaw, &pitch, &roll);
        TMP_float = yaw;
        tipo = T_FLOAT; (*ref) = &TMP_float;    return TRUE;
      }
      else if ( strcmp(nombre_variable, "pitch") == 0 ) {
        R.importar_quaternion(orientacion_absoluta());
        R.exportar_angulos_euler(&yaw, &pitch, &roll);
        TMP_float = pitch;
        tipo = T_FLOAT; (*ref) = &TMP_float;    return TRUE;
      }
      else if ( strcmp(nombre_variable, "roll") == 0 ) {
        R.importar_quaternion(orientacion_absoluta());
        R.exportar_angulos_euler(&yaw, &pitch, &roll);
        TMP_float = roll;
        tipo = T_FLOAT; (*ref) = &TMP_float;    return TRUE;
      }
      else if ( strcmp(nombre_variable, "yaw_relativo") == 0 ) {
        R.importar_quaternion(_orientacion);
        R.exportar_angulos_euler(&yaw, &pitch, &roll);
        TMP_float = yaw;
        tipo = T_FLOAT; (*ref) = &TMP_float;    return TRUE;
      }
      else if ( strcmp(nombre_variable, "pitch_relativo") == 0 ) {
        R.importar_quaternion(_orientacion);
        R.exportar_angulos_euler(&yaw, &pitch, &roll);
        TMP_float = pitch;
        tipo = T_FLOAT; (*ref) = &TMP_float;    return TRUE;
      }
      else if ( strcmp(nombre_variable, "roll_relativo") == 0 ) {
        R.importar_quaternion(_orientacion);
        R.exportar_angulos_euler(&yaw, &pitch, &roll);
        TMP_float = roll;
        tipo = T_FLOAT; (*ref) = &TMP_float;    return TRUE;
      }
      else if ( strcmp(nombre_variable, "orientacion_absoluta") == 0 ) {
        TMP_quaternion = orientacion_absoluta();
        tipo = T_QUATERNION;    (*ref) = &TMP_quaternion;    return TRUE;
      }
      else if ( strcmp(nombre_variable, "velocidad_angular") == 0 ) {
        tipo = T_VECTOR;    (*ref) = &_orientacion;    return TRUE;
      }
      else if ( strcmp(nombre_variable, "centro_de_masa") == 0 ) {
        tipo = T_VECTOR;    (*ref) = &_centro_de_masa;    return TRUE;
      }
    ;

    return COSA::consultar_variable(nombre_variable, tipo, ref);
}

BOOLEAN
COSA_RIGIDA::actualizar_variable(const char *nombre_variable, int tipo, void *ref)
{
    MATRIZ_4x4 R;
    double yaw, pitch, roll;

    if ( strcmp(nombre_variable, "orientacion_absoluta_euler") == 0 ) {
        if ( tipo != T_VECTOR ) return FALSE;
        TMP_vector = *((VECTOR *)(ref));
        R.rotacion_angulos_euler(TMP_vector.x, TMP_vector.y, TMP_vector.z);
        set_orientacion_absoluta(R.exportar_quaternion());
      }
      else if ( strcmp(nombre_variable, "yaw") == 0 ) {
        if ( tipo != T_FLOAT ) return FALSE;
        R.importar_quaternion(orientacion_absoluta());
        R.exportar_angulos_euler(&TMP_float, &pitch, &roll);
        TMP_float = *((double *)(ref));
        R.rotacion_angulos_euler(TMP_float, pitch, roll);
        set_orientacion_absoluta(R.exportar_quaternion());
      }
      else if ( strcmp(nombre_variable, "pitch") == 0 ) {
        if ( tipo != T_FLOAT ) return FALSE;
        R.importar_quaternion(orientacion_absoluta());
        R.exportar_angulos_euler(&yaw, &TMP_float, &roll);
        TMP_float = *((double *)(ref));
        R.rotacion_angulos_euler(yaw, TMP_float, roll);
        set_orientacion_absoluta(R.exportar_quaternion());
      }
      else if ( strcmp(nombre_variable, "roll") == 0 ) {
        if ( tipo != T_FLOAT ) return FALSE;
        R.importar_quaternion(orientacion_absoluta());
        R.exportar_angulos_euler(&yaw, &pitch, &TMP_float);
        TMP_float = *((double *)(ref));
        R.rotacion_angulos_euler(yaw, pitch, TMP_float);
        set_orientacion_absoluta(R.exportar_quaternion());
      }
      else if ( strcmp(nombre_variable, "yaw_relativo") == 0 ) {
        if ( tipo != T_FLOAT ) return FALSE;
        R.importar_quaternion(_orientacion);
        R.exportar_angulos_euler(&TMP_float, &pitch, &roll);
        TMP_float = *((double *)(ref));
        R.rotacion_angulos_euler(TMP_float, pitch, roll);
        _orientacion = R.exportar_quaternion();
      }
      else if ( strcmp(nombre_variable, "pitch_relativo") == 0 ) {
        if ( tipo != T_FLOAT ) return FALSE;
        R.importar_quaternion(_orientacion);
        R.exportar_angulos_euler(&yaw, &TMP_float, &roll);
        TMP_float = *((double *)(ref));
        R.rotacion_angulos_euler(yaw, TMP_float, roll);
        _orientacion = R.exportar_quaternion();
      }
      else if ( strcmp(nombre_variable, "roll_relativo") == 0 ) {
        if ( tipo != T_FLOAT ) return FALSE;
        R.importar_quaternion(_orientacion);
        R.exportar_angulos_euler(&yaw, &pitch, &TMP_float);
        TMP_float = *((double *)(ref));
        R.rotacion_angulos_euler(yaw, pitch, TMP_float);
        _orientacion = R.exportar_quaternion();
      }
      else {
        return COSA::actualizar_variable(nombre_variable, tipo, ref);
      }
    ;
    return TRUE;
}

COSA *
COSA_RIGIDA::crear_copia(void)
{
    GEOMETRIA *Forma;
    COSA_RIGIDA *Peluda;

    Forma = Geometria->crear_copia();
    if ( !Forma ) return NULL;

    Peluda = new COSA_RIGIDA;
    if ( !Peluda ) return NULL;

    Peluda->Geometria = Forma;
    (*Peluda) = (*this);
    return (COSA *)Peluda;
}

IMAGEN *
cargar_imagen(char *archivo)
{
    IMAGEN *Img;
    FILE *fd;

    fd = fopen(archivo, "rb");
    if ( !fd ) {
        fprintf(stderr, 
            "<COSA> Warning: No se puede abrir el archivo \"%s\"!\n",
            archivo);
        fflush(stderr);
        return NULL;
    }
    Img = new IMAGEN_RGB();
    if ( !( ((IMAGEN_RGB*)Img)->importar_ppm(fd) ) ) {
        fprintf(stderr, "<COSA> Warning: No se puede crear el bitmap IMG!\n");
        fflush(stderr); delete Img;
        return NULL;
    }
    fclose(fd);
    return Img;
}

#define CARGA_OPCIONAL_DE_IMAGEN() \
        tipo_token = Sabiondo->siguiente_token(cad); \
        ESPERO(TK_CADENA, "una cadena (imagen)"); \
        if ( strlen(cad) > 2 ) { /* Carga de imagenes */ \
            des_comille(cad); \
            Img = cargar_imagen(cad); \
            if ( Img ) { \
                g->anexar_textura(Img); \
              } \
              else { \
                fprintf(stderr,  \
                "<COSA> Warning: no se pudo cargar una imagen del archivo\n" \
                "\"%s\".  La imagen a sido desactivada.\n", cad); \
                fflush(stderr); \
            } \
        }


GEOMETRIA *
leer_geometria(TOKENIZADOR *Sabiondo)
{
    char cad[1000];
#ifdef GLTT_ENABLED
    char cad2[1000];
#endif
    int tipo_token;
    GEOMETRIA *g = NULL;
    IMAGEN *Img = NULL;
    VECTOR lados;
    ESCENA_3DS *Escena;
    BOOLEAN alphablending;
    double f1, f2, f3, f4, dummy, escala, nr = 1;
    double r1, r2, h;
    FILE *fd;

    tipo_token = Sabiondo->siguiente_token(cad);
    ESPERO(TK_IDENTIFICADOR, "un identificador de geometria");

    if ( strcmp(cad, "ESFERA") == 0 ) {
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "un numero (radio)");
        g = new ESFERA(atof(cad));
        CARGA_OPCIONAL_DE_IMAGEN();
      }
      else if ( strcmp(cad, "CUBO") == 0 ) {
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "un numero (lado)");
        g = new CUBO(atof(cad));
        CARGA_OPCIONAL_DE_IMAGEN();
      }
      else if ( strcmp(cad, "PLANTA_FRACTAL") == 0 ) {
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "un numero (nivel de recursion)");
        nr = atof(cad);
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "un numero (escala)");
        escala = atof(cad);
        g = new PLANTA_FRACTAL((int)(floor(nr)), escala);
        //CARGA_OPCIONAL_DE_IMAGEN();
      }
      else if ( strcmp(cad, "OCTREE") == 0 ) {
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "un numero (lado)");
        g = new OCTREE(atof(cad));
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_CADENA, "una cadena (archivo .oct)");
        des_comille(cad);
        fd = fopen(cad, "rb");
        if ( !fd ) {
            fprintf(stderr,"<COSA_RIGIDA> "
                "ERROR abriendo archivo de OCTREE!\n");
            fflush(stderr);
            return NULL;
        }
        ((OCTREE *)(g))->leer(fd);
        fclose(fd);
        CARGA_OPCIONAL_DE_IMAGEN();
      }
      else if ( strcmp(cad, "FLECHA") == 0 ) {
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "un numero (longitud flecha)");
        f1 = atof(cad);
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "un numero (longitud punta)");
        f2 = atof(cad);
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "un numero (grosor cuerpo)");
        f3 = atof(cad);
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "un numero (grosor punta)");
        f4 = atof(cad);
        g = new FLECHA(f1, f2, f3, f4);
        CARGA_OPCIONAL_DE_IMAGEN();
      }
      else if ( strcmp(cad, "PARALELEPIPEDO") == 0 ) {
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_VECTOR_INICIO, "el inicio de un VECTOR");
        lados.x = atof(&cad[1]);
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "un numero (dato 2 de un VECTOR)");
        lados.y = atof(cad);
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_VECTOR_FIN, "el final de un VECTOR");
        lados.z = atof(cad);
        g = new PARALELEPIPEDO(lados);
        CARGA_OPCIONAL_DE_IMAGEN();
      }
      else if ( strcmp(cad, "CILINDRO") == 0 ) {
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_VECTOR_INICIO, "el inicio de un VECTOR");
        r1 = atof(&cad[1]);
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "un numero (dato 2 de un VECTOR)");
        r2 = atof(cad);
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_VECTOR_FIN, "el final de un VECTOR");
        h = atof(cad);
        g = new CILINDRO(r1, r2, h);
        CARGA_OPCIONAL_DE_IMAGEN();
      }
    #ifdef GLTT_ENABLED
      else if ( strcmp(cad, "TEXTO_3D") == 0 ) {
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_CADENA, "una cadena (truetype font)");
        strcpy(cad2, cad);
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "una numero (escala de la letra)");
        f1 = atof(cad);
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_CADENA, "una cadena (texto)");
        des_comille(cad2); des_comille(cad);
        g = new TEXTO_3D(cad2, cad, f1);
        CARGA_OPCIONAL_DE_IMAGEN();
      }
    #else
      else if ( strcmp(cad, "TEXTO_3D") == 0 ) {
        fprintf(stderr, 
        "<TEXTO_3D>: No disponible en esta version, debido a que AQUYNZA fue\n"
        "compilado si la libreria GLTT/FreeType.\n");
        fflush(stderr);
        exit(1);
      }
    #endif
      else if ( strcmp(cad, "MESH_3DS") == 0 ) {
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_CADENA, "un archivo .3DS");
        des_comille(cad);
        Escena = new ESCENA_3DS(cad);
        tipo_token = Sabiondo->siguiente_token(cad);
        ESPERO(TK_NUMERO, "una numero (escala de la letra)");
        escala = atof(cad);
        // OJO: Algunas tiras quedan mal armadas! ... porque cambian de
        // orientacion!
        g = new MESH(80, FALSE/*strip*/, TRUE/*centrar*/, escala /*OJO!*/);
        //((MESH*)g)->set_material_global(TRUE);
        alphablending = FALSE;
        if ( !Escena || Escena->muerta() ) {
            if ( Escena ) delete Escena;
            fprintf(stderr, "<COSA> - ERROR: No puedo leer \"%s\"\n", cad);
            fflush(stderr);
            exit(-1);
        }
        importar_3DS(Escena, (MESH*)g, &alphablending, &dummy);
        delete Escena;
      }
      else {
        fprintf(stderr, 
        "<COSA_RIGIDA> "
        "ERROR: El tipo de GEOMETRIA \"%s\" es desconocido\n", cad);
        fflush(stderr);
        exit(1);
      }
    ;

    return g;
}

BOOLEAN
COSA_RIGIDA::resolver(LISTA <COSA *> *Cosas)
{
    if ( !_nombre_padre ) return TRUE;

    int i;

    for ( i = 0; i < Cosas->tam(); i++ ) {
        if ( strcmp(_nombre_padre, (*Cosas)[i]->nombre()) == 0 ) {
            Padre = (*Cosas)[i];
            return TRUE;
        }
    }
    return FALSE;
}

BOOLEAN
COSA_RIGIDA::leer_rigida(TOKENIZADOR *Sabiondo, char *cad)
{
    int tipo_token = TK_DESCONOCIDO;
    VECTOR direccion;
    double magnitud, yaw, pitch, roll;
    MATRIZ_4x4 R;

    if ( strcmp(cad, "velocidad_angular") == 0 ) {
      tipo_token = Sabiondo->siguiente_token(cad);
      ESPERO(TK_NUMERO, "un numero (magnitud de rotacion)");
      magnitud = DEG2RAD(atof(cad));  // OJO: Y este signo que?
      tipo_token = Sabiondo->siguiente_token(cad);
      ESPERO(TK_VECTOR_INICIO, "el inicio de un VECTOR");
      direccion.x = atof(&(cad[1]));
      tipo_token = Sabiondo->siguiente_token(cad);
      ESPERO(TK_NUMERO, "un numero (dato 2 de un VECTOR)");
      direccion.y = atof(cad);
      tipo_token = Sabiondo->siguiente_token(cad);
      ESPERO(TK_VECTOR_FIN, "el final de un VECTOR");
      cad[strlen(cad) - 1] = '\0';
      direccion.z = atof(cad);
      direccion.normalizar();
      _velocidad_angular = direccion * magnitud;
    }
    else if ( strcmp(cad, "orientacion") == 0 ) {
      tipo_token = Sabiondo->siguiente_token(cad);
      ESPERO(TK_NUMERO, "un numero (magnitud de rotacion)");
      magnitud = DEG2RAD(atof(cad));  // OJO: Y este signo que?
      tipo_token = Sabiondo->siguiente_token(cad);
      ESPERO(TK_VECTOR_INICIO, "el inicio de un VECTOR");
      direccion.x = atof(&(cad[1]));
      tipo_token = Sabiondo->siguiente_token(cad);
      ESPERO(TK_NUMERO, "un numero (dato 2 de un VECTOR)");
      direccion.y = atof(cad);
      tipo_token = Sabiondo->siguiente_token(cad);
      ESPERO(TK_VECTOR_FIN, "el final de un VECTOR");
      cad[strlen(cad) - 1] = '\0';
      direccion.z = atof(cad);
      direccion.normalizar();
      _orientacion.importar_angulo_eje(magnitud, direccion);
    }
    else if ( strcmp(cad, "orientacion_euler") == 0 ) {
      tipo_token = Sabiondo->siguiente_token(cad);
      ESPERO(TK_VECTOR_INICIO, "el inicio de un VECTOR");
      yaw = DEG2RAD(atof(&(cad[1])));
      tipo_token = Sabiondo->siguiente_token(cad);
      ESPERO(TK_NUMERO, "un numero (dato 2 de un VECTOR)");
      pitch = DEG2RAD(atof(cad));
      tipo_token = Sabiondo->siguiente_token(cad);
      ESPERO(TK_VECTOR_FIN, "el final de un VECTOR");
      cad[strlen(cad) - 1] = '\0';
      roll = DEG2RAD(atof(cad));
      R.rotacion_angulos_euler(yaw, pitch, roll);
      _orientacion = R.exportar_quaternion();
    }

    return TRUE;
}

void
COSA_RIGIDA::resolver_tensor_de_inercia(void)
{
    // OJO: En este momento esto es un machete! Siempre se tiene el tensor de
    //      inercia de un bloque paralelogramo de masa constante!
    double x0, y0, z0; // El bloque es de x0 * y0 * z0 unidades

    x0 = 1;
    y0 = 0.5;
    z0 = 0.2;

    tensor_de_inercia_0.identidad();
    tensor_de_inercia_0.M[0][0] = ((y0*y0)+(z0*z0)) * (_masa/12.0);
    tensor_de_inercia_0.M[1][1] = ((x0*x0)+(z0*z0)) * (_masa/12.0);
    tensor_de_inercia_0.M[2][2] = ((x0*x0)+(y0*y0)) * (_masa/12.0);
    tensor_de_inercia_0 = tensor_de_inercia_0;

  //printf("El tensor de inercia relativo a las coordenadas del objeto es:\n");
  //tensor_de_inercia_0.imprimir();
}

void
COSA_RIGIDA::grabar(FILE *fd)
{
    //-----------------------------------------------------------------------
    fprintf(fd, "COSA_RIGIDA \"%s\"\n", nombre());
    grabar_basico(fd);

    //-----------------------------------------------------------------------
    char n0[200];
    char n1[200];
    char n2[200];
    char n3[200];
    double yaw, pitch, roll, m;
    MATRIZ_4x4 R;

    R.importar_quaternion(orientacion_absoluta());
    R.exportar_angulos_euler(&yaw, &pitch, &roll);
    sprintf(n1, "%f", yaw);    simplifique_real(n1);
    sprintf(n2, "%f", pitch);    simplifique_real(n2);
    sprintf(n3, "%f", roll);    simplifique_real(n3);
    fprintf(fd, "    orientacion_euler <%s, %s, %s>\n", n1, n2, n3);

    m = _velocidad_angular.norma();
    sprintf(n0, "%f", RAD2DEG(m));    simplifique_real(n0);
    sprintf(n1, "%f", _velocidad_angular.x/m);    simplifique_real(n1);
    sprintf(n2, "%f", _velocidad_angular.y/m);    simplifique_real(n2);
    sprintf(n3, "%f", _velocidad_angular.z/m);    simplifique_real(n3);
    fprintf(fd, "    velocidad_angular %s:<%s, %s, %s>\n", n0, n1, n2, n3);

    if ( Geometria ) Geometria->pintar_aqz(fd);

    //-----------------------------------------------------------------------
    fprintf(fd, "}\n\n");
}

BOOLEAN
COSA_RIGIDA::leer(TOKENIZADOR *Sabiondo)
{
    char cad[1000];
    int tipo_token = TK_DESCONOCIDO, pos;
    BOOLEAN con_mi_color = FALSE;

    //- Ejecute el parser especifico de la COSA_RIGIDA -----------------
    pos = 1;
    while ( tipo_token != TK_CERRAR) {
        tipo_token = Sabiondo->siguiente_token(cad);
        switch ( pos ) {
            case 1:
              ESPERO(TK_CADENA, "una cadena");
              if ( strlen(cad) > MAX_CAD-1 ) cad[MAX_CAD-1] = '\0'; 
              des_comille(cad);
              set_nombre(cad);
              pos++;
              break;
            case 2:  ESPERO(TK_ABRIR, "\"{\"");  pos++; break;
            default:
              if ( tipo_token == TK_CERRAR ) break;
              ESPERO(TK_IDENTIFICADOR, "un identificador (3)");
              if ( strcmp(cad, "espacio") == 0 ) {
                  tipo_token = Sabiondo->siguiente_token(cad);
                  ESPERO(TK_CADENA, "una cadena");
                  // OJO: Es responsabilidad del UNIVERSO asignarme luego
                  // mi ESPACIO
                  //printf("  - espacio: %s\n", cad); 
                }
                else if ( strcmp(cad, "geometria") == 0 ) {
                  Geometria = leer_geometria(Sabiondo);
                }
                else if ( 
                  !leer_basico(Sabiondo, cad, &con_mi_color) ||
                  !leer_rigida(Sabiondo, cad) 
                ) return FALSE;
              ;
              break;
        }
    }

    //- Postprocesamiento de datos -------------------------------------
    if ( !con_mi_color ) _color = Material->difusa();

    // Tensor de inercia inicial, calculado a partir de la geometria.
    if ( !Geometria ) {
        fprintf(stderr, "<COSA_RIGIDA> ERROR FATAL: Sin una geometria no "
            "puede calcularse un tensor de inercia!\n");
        fflush(stderr);
        return FALSE;
    }

    resolver_tensor_de_inercia();
    return TRUE;
}

void
COSA_RIGIDA::tick(void)
/*
Esto es un ridiculo metodo de prueba, debe ser eliminado! (o mejor,
reemplazado por un esquema de evaluadores de funcion...)
*/
{
    static double t = 0;

    t+=0.05;

    _posicion.x = 0.5 + sin(t)/2;
    _posicion.y = 0.5 + cos(2*t)/2;
    _posicion.z = 0.5 - cos(3*t)/2;
}

QUATERNION
COSA_RIGIDA::orientacion_absoluta(void)
{
    QUATERNION resultado = _orientacion;

    if ( Padre ) {
        resultado = Padre->orientacion_absoluta() * resultado;
    }
    resultado.normalizar();
    return resultado;
}

void
COSA_RIGIDA::set_orientacion_absoluta(QUATERNION q)
{
    if ( Padre ) {
        fprintf(stderr, "<COSA_RIGIDA> - ERROR: \n"
         "set_orientacion_absoluta() no implementada para cosas con padre.\n");
        fflush(stderr);
        exit(1);
    }
    _orientacion = q;
}

VECTOR
COSA_RIGIDA::posicion_absoluta(VECTOR p)
{
    // MUCHO OJO! No se ha implementado! Debe utilizar la _orientacion, la
    // _posicion y la transformacion geometrica del padre si lo hay!...

    // OJO: No es mejor usar directamente el quaternion para aplicar 
    //      la rotacion?
    VECTOR p_pos(0, 0, 0), resultado;
    void *interface1, *interface2;
    MATRIZ_4x4 R, p_ori;
    int tipo1, tipo2;

    R.importar_quaternion(_orientacion);

    if ( Padre && Padre->consultar_variable("posicion",  tipo1, &interface1) &&
        Padre->consultar_variable("orientacion_absoluta",tipo2,&interface2) ) {
         if ( tipo1 == T_VECTOR && tipo2 == T_QUATERNION ) {
             p_pos = (*(VECTOR *)interface1);
             p_ori.importar_quaternion((*(QUATERNION *)interface2));
         }
         resultado = p_pos + p_ori*_posicion + R*p;
      }
      else {
        resultado = _posicion + R*p;
    }

    return resultado;
}

VECTOR
COSA_RIGIDA::velocidad_angular_absoluta(void)
{
    // OJO: Esto esta incompleto! Que pasa cuando soy parte de un solido
    //      articulado? (cuando tengo padre?)

    return _velocidad_angular;
}

void
COSA_RIGIDA::set_velocidad_angular_absoluta(VECTOR omega)
{
    // OJO: Esto esta incompleto! Que pasa cuando soy parte de un solido
    //      articulado? (cuando tengo padre?)

    _velocidad_angular = omega;
}

MATRIZ_4x4
COSA_RIGIDA::tensor_de_inercia(void)
{
    MATRIZ_4x4 I_t, R_t;

    // OJO: Si es cierto? Que pasa cuando tengo un padre?
    R_t.importar_quaternion(_orientacion);
    I_t = R_t * (tensor_de_inercia_0 * R_t);
    return I_t;
}

void
COSA_RIGIDA::minmax(VECTOR *min, VECTOR *max)
{
    if ( !Geometria ) return;

    //- Calcule las esquinas del objeto envolvente en coordenadas locales ---
    VECTOR mmi, mmx;
    VECTOR esquinas[8];

    Geometria->minmax(&mmi, &mmx);

    GENERAR_ESQUINAS(esquinas);

    //- Calcule el min-max usando las esquinas en coordenadas globales ------
    int i;

    min->x = INFINITO;
    min->y = INFINITO;
    min->z = INFINITO;
    max->x = -INFINITO;
    max->y = -INFINITO;
    max->z = -INFINITO;

    for ( i = 0; i < 8; i++ ) esquinas[i] = posicion_absoluta(esquinas[i]);
    for ( i = 0; i < 8; i++ ) {
        if ( esquinas[i].x < min->x ) min->x = esquinas[i].x;
        if ( esquinas[i].y < min->y ) min->y = esquinas[i].y;
        if ( esquinas[i].z < min->z ) min->z = esquinas[i].z;
        if ( esquinas[i].x > max->x ) max->x = esquinas[i].x;
        if ( esquinas[i].y > max->y ) max->y = esquinas[i].y;
        if ( esquinas[i].z > max->z ) max->z = esquinas[i].z;
    }
}

double
COSA_RIGIDA::interseccion(RAYO *Rayo, VECTOR *Punto, VECTOR *Normal)
{
    if ( !Geometria ) {
        fprintf(stderr, "<COSA_RIGIDA> ERROR: No para herencia!\n");
        fflush(stderr);
        return 0;
    }

    //- OJO: Esto deberia precalcularse! -----------------------------------
    MATRIZ_4x4 R, R_i;
    VECTOR po;
    VECTOR n(0, 0, 0);

    R.importar_quaternion(orientacion_absoluta());
    R_i = R.inversa();
    po = posicion_absoluta(n);

    //- OJO: Esto deberia optimizarce! -------------------------------------
    double t;
    RAYO mi_rayo;

    // Pre-acomodo el rayito para la operacion centrada en el origen
    mi_rayo.origen = Rayo->origen - po;
    mi_rayo.origen = R_i * mi_rayo.origen;
    mi_rayo.direccion = R_i * Rayo->direccion;
    mi_rayo.direccion.normalizar(); // Esto es necesario?

    // Ejecuto la operacion
    t = Geometria->interseccion(&mi_rayo, *Punto, *Normal);

    // Retorno los resultados a coordenadas globales
    (*Punto)  = R * (*Punto);
    (*Punto)  = (*Punto) + po;
    (*Normal) = R * (*Normal);
    return t;
}


#ifdef GL_ENABLED
void
COSA_RIGIDA::pintar_gl(CALIDAD_VISUAL *Calidad, CAMARA *Camara)
{
    int i;

    //- Calcule las esquinas del paralelogramo envolvente -------------------
    VECTOR mmi(0, 0, 0), mmx(1, 1, 1);
    VECTOR esquinas[8];

    if ( Geometria ) Geometria->minmax(&mmi, &mmx);
    GENERAR_ESQUINAS(esquinas);
    for ( i = 0; i < 8; i++ ) esquinas[i] = posicion_absoluta(esquinas[i]);

    //- Solo si el paralelepipedo envolvente de esta COSA se ve se pinta... -
    if ( !Camara->minmax_visible(esquinas) ) return;

    //- Inicializacion geometrica (OJO!) ------------------------------------
    VECTOR centro_de_masa(0, 0, 0);
    MATRIZ_4x4 R;
    VECTOR p = posicion_absoluta(centro_de_masa);

    glPushMatrix();  glLoadIdentity();
    glTranslatef((float)p.x, (float)p.y, (float)p.z);
    R.importar_quaternion(orientacion_absoluta());
    R.cargar_gl();

    //- Pinte el objeto -----------------------------------------------------
    static COLOR backup_color;

    if ( Geometria ) {
        backup_color = Material->difusa();
        Material->set_difusa(_color);
        Geometria->pintar_gl(Calidad, Material, Camara);
        Material->set_difusa(backup_color);
      }
      else {
        // Esto es por si algo salio mal con la geometria
        glBegin(GL_QUADS);
            glVertex2f(1.0/2.0, 1.0/2.0);
            glVertex2f(-1.0/2.0, 1.0/2.0);
            glVertex2f(-1.0/2.0, -1.0/2.0);
            glVertex2f(1.0/2.0, -1.0/2.0);
        glEnd();
    }

    //- Finalizacion --------------------------------------------------------
    glPopMatrix();
}
#endif // GL_ENABLED

void
COSA_RIGIDA::pintar_povray(FILE *fd)
{
    if ( !Geometria ) {
         fprintf(fd, 
           "// <COSA_RIGIDA> - ERROR: No hay geometria!\n");
         return;
    }

    MATRIZ_4x4 R;
    VECTOR centro_de_masa(0, 0, 0);
    VECTOR p;
    COLOR backup_color;

    p = posicion_absoluta(centro_de_masa);
    R.importar_quaternion(orientacion_absoluta());

    Geometria->pintar_povray(fd);
    R.inversa().activar_povray(fd); // Porque POV piensa en mano izquierda...
    fprintf(fd, "    translate <%f, %f, %f>\n", p.x, p.y, p.z);

    backup_color = Material->difusa();
    Material->set_difusa(_color);
    Material->activar_povray(fd);
    Material->set_difusa(backup_color);

    fprintf(fd, "}\n");
}

void
COSA_RIGIDA::actualizar(double delta_t)
{
    SOLVER_EULER solver(this);

    solver.ejecutar_solucion(delta_t);

    //- Machete para implementar el drag rotacional! ------------------------
    VECTOR omega;
    double factor;

    omega = _velocidad_angular;

    if ( omega.norma() > 2*EPSILON && _ultimo_Kd > EPSILON ) {
    factor = (_ultimo_Kd * delta_t) / 10;
    if ( factor < 0 ) {
        factor = 0;
    }
    if ( factor > 1 ) {
        factor = 1;
    }

    omega = omega * (1 - factor);
        _velocidad_angular = omega;
    }

}

void
COSA_RIGIDA::aplicar_drag(double Kd)
{
    //- Viscosidad lineal --------------------------------------------------
    VECTOR centro_de_masa(0,0,0);

    // OJO: Estas posiciones deben estar bien pensadas!
    anexar_fuerza(velocidad_absoluta() * Kd,
      posicion_absoluta(centro_de_masa));

    //- Viscosidad rotacional ----------------------------------------------
    // Macheteado en COSA_RIGIDA::actualizar ...
//#define ESTRATEGIA_FALLIDA
//#define DEBUG_ROTFAL
#ifdef ESTRATEGIA_FALLIDA
    // Esto es un machete para manejar el torque... mmm OJO: Seguir pensando
    // en la situacion del torque. La implementacion funciono, pero annade
    // rotacion un poco mal, haciendo que el objeto se de la vuelta un
    // poco hacia donde no deberia...
    VECTOR omega;
    VECTOR origen(0, 0, 0);
    MATRIZ_4x4 R;
    VECTOR fuerza_de_torque_positiva;
    VECTOR fuerza_de_torque_negativa;
    VECTOR i(1, 0, 0);
    VECTOR binormal;

    omega = _velocidad_angular;

    if ( omega.norma() > 10*EPSILON ) {
    omega.normalizar();
    R.rotacion_punto_punto(origen, omega);

    fuerza_de_torque_positiva = R * i;
        // OJO: Esto es una constante empirica! -> toca derivarla y mejorarla.
        //      Ademas... en la vida real depende de la geometria del objeto!
    fuerza_de_torque_positiva = fuerza_de_torque_positiva * Kd * 0.5;

    fuerza_de_torque_negativa = fuerza_de_torque_positiva * -1;

    binormal = fuerza_de_torque_positiva.producto_cruz(omega);
        binormal.normalizar();

        anexar_fuerza(fuerza_de_torque_positiva,
                      posicion_absoluta(centro_de_masa + binormal*0.001));
        anexar_fuerza(fuerza_de_torque_negativa,
                      posicion_absoluta(centro_de_masa - binormal*0.001));

      #ifdef DEBUG_ROTFAL
    //*****************************************************************
    clrscr();

    printf("omega = ");
    (_velocidad_angular*1000).imprimir();
    printf("\n");

    printf("F+ = ");
    (fuerza_de_torque_positiva * 1000).imprimir();
    printf("\n");

    printf("F- = ");
    (fuerza_de_torque_negativa * 1000).imprimir();
    printf("\n");

    printf("Bn = ");
    binormal.imprimir();
    printf("\n");

    fflush(stdout);
    //*****************************************************************
      #endif
    }
#endif

}

//= SERVICIOS DE ECUACION DIFERENCIAL! ======================================

int
COSA_RIGIDA::ODE_numero_de_variables_de_estado(void)
{
    if ( _estoy_fijo ) return 0;
    return 13;
}

void
COSA_RIGIDA::ODE_reportar_variables_de_estado(double *Datos)
{
    if ( _estoy_fijo ) return;

    Datos[0] = _posicion.x;
    Datos[1] = _posicion.y; 
    Datos[2] = _posicion.z;
    Datos[3] = _velocidad.x;
    Datos[4] = _velocidad.y;
    Datos[5] = _velocidad.z;
    Datos[6] = _orientacion.direccion.x;
    Datos[7] = _orientacion.direccion.y;
    Datos[8] = _orientacion.direccion.z;
    Datos[9] = _orientacion.magnitud;
    Datos[10] = _velocidad_angular.x;
    Datos[11] = _velocidad_angular.y;
    Datos[12] = _velocidad_angular.z;
}

void
COSA_RIGIDA::ODE_actualizar_variables_de_estado(double *Datos)
{
    if ( _estoy_fijo ) return;
    _posicion.x = Datos[0];
    _posicion.y = Datos[1];
    _posicion.z = Datos[2];
    _velocidad.x = Datos[3];
    _velocidad.y = Datos[4];
    _velocidad.z = Datos[5];
    _orientacion.direccion.x = Datos[6];
    _orientacion.direccion.y = Datos[7];
    _orientacion.direccion.z = Datos[8];
    _orientacion.magnitud = Datos[9];
    _velocidad_angular.x = Datos[10];
    _velocidad_angular.y = Datos[11];
    _velocidad_angular.z = Datos[12];

    // IMPORTANTE: Esto mantiene a raya el error numerico, al asegurar
    // que las matrices de rotacion siempre sean ortonormales.
    _orientacion.normalizar();
}

void
COSA_RIGIDA::ODE_calcular_la_primera_derivada(double *Datos)
{
    //- 1. CALCULE LA CONTRIBUCION TOTAL DE LAS FUERZAS SOBRE EL CUERPO -----
    int i;
    VECTOR fuerza_total(0, 0, 0);
    VECTOR torque_total(0, 0, 0);  // Cambio en el momento angular
    VECTOR r_i;

    for ( i = 0; i < lista_fuerzas_externas.tam(); i++ ) {
        fuerza_total = fuerza_total + lista_fuerzas_externas[i]->fuerza;
        r_i = lista_fuerzas_externas[i]->punto - _posicion;
        torque_total = torque_total + 
                         r_i.producto_cruz(lista_fuerzas_externas[i]->fuerza);
    }
    limpiar_fuerzas();

    // OJO: Los cuerpos fijos no se mueven (i.e. no se afectan por el SOLVER)
    if ( _estoy_fijo ) return;

    //- 2. ACTUALICE LOS DATOS LINEALES -------------------------------------

    // 2.1. X' = V
    Datos[0] = _velocidad.x;
    Datos[1] = _velocidad.y;
    Datos[2] = _velocidad.z;

    // 2.2. V' = F/m
    Datos[3] = fuerza_total.x / _masa;
    Datos[4] = fuerza_total.y / _masa;
    Datos[5] = fuerza_total.z / _masa;

    //- 3. ACTUALICE LOS DATOS ROTACIONALES ---------------------------------

    // 3.1. Orientacion usando el metodo de los quaternions: q' = (1/2)omega*q
    QUATERNION omega, q_prima;

    omega.importar_quaternion_puro(_velocidad_angular);
    q_prima = omega*_orientacion;
    q_prima = q_prima * 0.5;

    // 3.1. Orientacion usando el metodo de las matrices de rotacion...
  #ifdef NONONO // Alternativo...
    MATRIZ_4x4 R_prima, omega_asterisco, R;
    QUATERNION q_prima;

    R.importar_quaternion(_orientacion);
    omega_asterisco.importar_asterisco(_velocidad_angular);
    R_prima = omega_asterisco * R;
    q_prima = R_prima.exportar_quaternion();
    _orientacion = _orientacion + q_prima;
  #endif

    Datos[6] = q_prima.direccion.x; // 3.1.1. Haga efectivos los cambios
    Datos[7] = q_prima.direccion.y;
    Datos[8] = q_prima.direccion.z;
    Datos[9] = q_prima.magnitud;

    // 3.2. Actualizacion de la velocidad rotacional
    // OJO: Es necesario hacer varias pruebas para validar el funcionamiento
    //      de la contribucion rotacional del tensor de inercia!
    MATRIZ_4x4 R_t;       // Matriz de rotacion para la COSA en t
    MATRIZ_4x4 I_t;       // Tensor de inercia evaluado en t
    VECTOR delta_omega_t; // Cambio en la velocidad angular

    // No es mejor usar el metodo `tensor_de_innercia()` ?
    R_t.importar_quaternion(_orientacion);
    I_t = R_t * (tensor_de_inercia_0 * R_t);
    delta_omega_t = (I_t.inversa())*torque_total;

    Datos[10] = delta_omega_t.x;
    Datos[11] = delta_omega_t.y;
    Datos[12] = delta_omega_t.z;
}

//===========================================================================
//= EOF                                                                     =
//===========================================================================

---

Este archivo HTML ha sido generado automáticamente a partir del código fuente AQUYNZA. NO LO EDITE. Para mayor información contacte al autor.