colision.C

Ir a la documentación de este archivo.

//===========================================================================
//= colision.cc                                               Julio de 1999 =
//=-------------------------------------------------------------------------=
//= Declaraciones de las operaciones utilitarias para la deteccion general  =
//= de colisiones.                                                          =
//=-------------------------------------------------------------------------=
//= 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 "core/simul/colision.h"
#include "lista.cc"

//===========================================================================
//= Bromas globales                                                         =
//===========================================================================

static int COLOR_pareja = 0;
static LISTA <CONTACTO *> contactos;

LISTA <MINMAX *> TMP_debug_visual;
static BOOLEAN visual_debug = TRUE;

#define VERIFICAR(p)                                                    \
    if ( !p ) {                                                         \
        fprintf(stderr, "<COLISION> - ERROR: memoria insuficiente!\n"); \
        fflush(stderr);                                                 \
        return;                                                         \
    }

//===========================================================================
//= Clase utilitaria MINMAX                                                 =
//===========================================================================

MINMAX::MINMAX()
{
    Rayito = NULL;
    switch ( COLOR_pareja ) {
      case 0: color.r = 1;  color.g = 1;  color.b = 1; break;
      case 1: color.r = 1;  color.g = 0;  color.b = 0; break;
      case 2: color.r = 0;  color.g = 1;  color.b = 0; break;
      case 3: color.r = 0;  color.g = 0;  color.b = 1; break;
      case 4: color.r = 1;  color.g = 1;  color.b = 0; break;
      case 5: color.r = 0;  color.g = 1;  color.b = 1; break;
      case 6: color.r = 1;  color.g = 0;  color.b = 1; break;
    }
    if ( COLOR_pareja >= 7 ) COLOR_pareja = 0;
}

MINMAX::~MINMAX()
{
    if ( Rayito ) delete Rayito;
}

#ifdef GL_ENABLED
void
MINMAX::pintar_gl(void)
{
    glColor3f(color.r, color.g, color.b);
    pintar_paralelepipedo(min, max);
    if ( Rayito ) {
        Rayito->pintar_gl();
    }
}
#endif

//===========================================================================
//= Clase CONTACTO                                                          =
//===========================================================================

CONTACTO::CONTACTO()
{
    ;
}

CONTACTO::~CONTACTO()
{
    ;
}

//===========================================================================
//= Rutinas de deteccion de colisiones                                      =
//===========================================================================

#define SWAP(a, b) temp = (a); (a) = (b); (b) = temp;
#define TA A->tipo_de_cosa()
#define TB B->tipo_de_cosa()

static void
detector_colision_puntual_rigida(COSA *A, COSA *B,
    LISTA <CONTACTO *> *Colisiones, double udt, BOOLEAN visual_debug,
    MINMAX *m1, MINMAX *m2)
{
    CONTACTO *Contacto = NULL;
    VECTOR nulo(0, 0, 0);
    VECTOR pos_actual, pos_anterior, punto_choque, normal_choque;
    RAYO rayo;
    double t;

    pos_actual = A->posicion_absoluta(nulo);
    pos_anterior = pos_actual - A->velocidad_absoluta()*udt;
    rayo.origen = pos_anterior;
    rayo.direccion = pos_actual - pos_anterior;
    if ( visual_debug ) {
        m1->Rayito = new RAYO();
        *(m1->Rayito) = rayo;
    }
    rayo.direccion.normalizar();

    t = B->interseccion(&rayo, &punto_choque, &normal_choque);
    punto_choque = punto_choque - rayo.direccion * (EPSILON * 4);  // OJO!
    if ( t < EPSILON || t > DISTANCIA_V(pos_actual, pos_anterior) ) {
        //printf("[falsa alarma](%.2f - %.2f)\n", t, 
        //    DISTANCIA_V(pos_actual, pos_anterior));
        //fflush(stdout);
      return;
    }
    else {
        //printf("[COLISION MASA_PUNTUAL/COSA_RIGIDA](%.2f - %.2f)\n", t,
        //    DISTANCIA_V(pos_actual, pos_anterior));
        //fflush(stdout);
        if ( visual_debug ) {
            m2->Rayito = new RAYO();
            m2->Rayito->origen = punto_choque;
            m2->Rayito->direccion = normal_choque * (0.3);
            ((MASA_PUNTUAL *)A)->en_colision = TRUE;
        }
        A->set_posicion_absoluta(punto_choque); // OJO: Esto es medio-machete!
        Contacto = new CONTACTO;
        VERIFICAR(Contacto);
        Contacto->A = A;
        Contacto->B = B;
        Contacto->p = punto_choque;
        // OJO: se supone que las dos normales son de igual direccion?
        Contacto->n_a = normal_choque * -1.0;
        Contacto->n_b = normal_choque;
        Contacto->t = t;
        Colisiones->anx(Contacto);
    }
}

static void
detector_colision_tela_rigida(COSA *A, COSA *B,
    LISTA <CONTACTO *> *Colisiones, double udt, BOOLEAN visual_debug,
    MINMAX *m1, MINMAX *m2)
{
    MASA_PUNTUAL *Mi;
    TELA *T = (TELA *)A;
    int i;

    for ( i = 0; i < T->num_masas(); i++ ) {
        Mi = T->masa_puntual(i);
        detector_colision_puntual_rigida(Mi, B, Colisiones, udt,
            visual_debug, m1, m2);
    }
}

static void
detectar_par_colision(COSA *A, COSA *B, LISTA <CONTACTO *> *Colisiones,
                      double udt, BOOLEAN visual_debug)
{
    //- Si las dos cosas son fijas no importa la colision! ------------------
    if ( A->estoy_fijo() && B->estoy_fijo() ) return;

    //- Pre-chequeo de condicion de la interseccion de min-max'es -----------
    MINMAX *m1, *m2;

    m1 = new MINMAX;
    A->minmax(&(m1->min), &(m1->max));
    m2 = new MINMAX;
    B->minmax(&(m2->min), &(m2->max));

    if ( ((m1->max.x >= m2->min.x) && (m1->min.x <= m2->max.x)) &&
         ((m1->max.y >= m2->min.y) && (m1->min.y <= m2->max.y)) &&
         ((m1->max.z >= m2->min.z) && (m1->min.z <= m2->max.z))     ) {
        if ( visual_debug ) {
            TMP_debug_visual.anx(m1); // Los min-max'es se intersectan
            TMP_debug_visual.anx(m2);
          }
          else {
            delete m1;
            delete m2;
        }
        COLOR_pareja++;
      }
      else {
        delete m1;                // Los min-max'es NO se intersectan
        delete m2;
        return;
    }

    //- Analisis detallado de colision --------------------------------------
    //- Aqui se analiza la pareja teniendo en cuenta el tipo de geometria   -
    //- de cada COSA.  Cada tipo de pareja posee un sistema de deteccion de -
    //- colisiones particular, y si no existe un sistema de deteccion para  -
    //- una pareja de tipos dada, la colision no se detectan (los objetos   -
    //- eventualmente se interpenetraran).                                  -

    //printf("[%s, %s]", A->nombre(), B->nombre());  fflush(stdout);

    // Preparacion: notese que la solucion de colision para cosas de dos
    // tipos A y B, es reciproca a la de los tipos B y A, por lo tanto, aqui
    // se verifica un "orden canonico".
    COSA *temp;

    if ( (TB == CC_MASA_PUNTUAL && TA == CC_COSA_RIGIDA) ||
         (TB == CC_TELA && TA == CC_COSA_RIGIDA)
       ) {
        SWAP(A, B);
    }

    // Procesamiento central
    if ( TA == CC_MASA_PUNTUAL && TB == CC_MASA_PUNTUAL ) {
        // En principio las masas infinitesimales no se estrellan...
        return;
      }
      else if ( TA == CC_MASA_PUNTUAL && TB == CC_COSA_RIGIDA ) {
        detector_colision_puntual_rigida(A, B, Colisiones, udt, visual_debug,
                                         m1, m2);
      }
      else if ( TA == CC_TELA && TB == CC_COSA_RIGIDA ) {
        detector_colision_tela_rigida(A, B, Colisiones, udt, visual_debug,
                                         m1, m2);
      }
    ;
}

//===========================================================================
//= CLASE MANEJADOR_CONFLICTOS_COLISION                                     =
//===========================================================================

MANEJADOR_CONFLICTOS_COLISION::MANEJADOR_CONFLICTOS_COLISION()
{
    ;
}

MANEJADOR_CONFLICTOS_COLISION::~MANEJADOR_CONFLICTOS_COLISION()
{
    ;
}

//= Front-end de colisiones basado en el concepto de "conflicto" ============

void
MANEJADOR_CONFLICTOS_COLISION::detectar_conflictos(LISTA <COSA *> *Cosas,
                                                   double udt)
{
    int i, j;

    //- Si se tiene una lista de chequeo visual, borrela --------------------
    for ( i = 0; i < TMP_debug_visual.tam(); i++ ) delete TMP_debug_visual[i];
    TMP_debug_visual.elim();

    //- Version inocente de verificacion de colision entre parejas ----------
    COLOR_pareja = 1;

    for ( i = 0; i < Cosas->tam()-1; i++ ) {
        for ( j = i+1; j < Cosas->tam(); j++ ) {
            detectar_par_colision((*Cosas)[i],(*Cosas)[j], &contactos,
                udt,visual_debug);
        }
    }
}

//= Rutinas de respuesta a colisiones =======================================

void
MANEJADOR_CONFLICTOS_COLISION::resolver_conflictos(void)
{
    int i;

    //- Algunas variables facilitan la notacion matematica... --------------
    COSA *A, *B;                                  // Temporales utilitarias
    CONTACTO *Contacto;                           //
    VECTOR centro_de_masa(0, 0, 0);               //
    double numerator, term1, term2, term3, term4; //
    VECTOR p;      // coordenadas globales del punto de contacto
    VECTOR n;      // normal a B en el punto de contacto
    VECTOR xa;     // posicion absoluta del centro de masa A
    VECTOR xb;     // posicion absoluta del centro de masa B
    VECTOR pa;     // coordenadas del punto de contacto en el sistema A
    VECTOR pb;     // coordenadas del punto de contacto en el sistema B
    VECTOR dot_pa; // velocidad puntual del punto de contacto en la cosa A
    VECTOR dot_pb; // velocidad puntual del punto de contacto en la cosa B
    double vrel;   // criterio de "en choque", "en contacto" o "separando".
    VECTOR J;      // es un vector en la direccion `n`, con magnitud `j`
    double j;      // magnitud de `J`
    VECTOR torque_impulsivo_a; // Torque impulsivo aplicado a A
    VECTOR torque_impulsivo_b; // Torque impulsivo aplicado a B
    double epsilon; // Coeficiente de restitucion absoluto para la colision
    MATRIZ_4x4 Ia_i; // Inversa del tensor de inercia de la cosa A
    MATRIZ_4x4 Ib_i; // Inversa del tensor de inercia de la cosa B

    //- Ciclo principal: el proceso se repite para cada pareja de COSAs ----
    for ( i = 0; i < contactos.tam(); i++ ) {
        // Esto esta aqui para aumentar la eficiencia...
        Contacto = contactos[i];
        A = Contacto->A;
        B = Contacto->B;

        //- 1. Calcule las velocidades puntuales en el lugar de colision ----
        p = Contacto->p;
        xa = A->posicion_absoluta(centro_de_masa);
        dot_pa = A->velocidad_absoluta() + 
            (A->velocidad_angular_absoluta().producto_cruz(p - xa));

        xb = B->posicion_absoluta(centro_de_masa);
        dot_pb = B->velocidad_absoluta() + 
            (B->velocidad_angular_absoluta().producto_cruz(p - xb));

        //- 2. Determine si la colision es un choque, un contacto o ninguno -
        // OJO: Aqui sobraria una normal en la representacion de CONTACTO,
        //      porque la idea es que n_a y n_b tienen la misma direccion!
        n = Contacto->n_b;
        n.normalizar();  // Por siaca'
        vrel = n.producto_punto(dot_pa - dot_pb);
        if ( vrel < -EPSILON ) {
            // 2.1. Las cosas se estan estrellando! 

            // Aqui se "retrocede" en el tiempo a cada pareja de cosas hasta
            // el instante en que supuestamente se habian chocado antes
            // de seguir... OJO!: ESTA IDEA TRAE PROBLEMAS!
            //A->actualizar(-Contacto->t);
            //B->actualizar(-Contacto->t);

            // Notese que `epsilon` modela una perdida de energia cinetica
            // en la colision, debida por ejemplo a que los objetos suenan o
            // se calientan
            epsilon = A->ke() * B->ke();  // Heuristica particular de AQUYNZA!

            // Se calcula `J`
            if ( A->estoy_fijo() ) Ia_i = Ia_i * 0;
            else Ia_i = (A->tensor_de_inercia()).inversa();
            if ( B->estoy_fijo() ) Ib_i = Ib_i * 0;
            else Ib_i = (B->tensor_de_inercia()).inversa();
            numerator = -((1 + epsilon) * vrel);
            if ( A->estoy_fijo() ) term1 = 0;
            else term1 = 1/A->masa();
            if ( B->estoy_fijo() ) term2 = 0;
            else term2 = 1/B->masa();

            pa = p - xa;
            pb = p - xb;

            term3 = n.producto_punto(
                      (Ia_i * (pa.producto_cruz(n))).producto_cruz(pa)
                    );
            term4 = n.producto_punto(
                      (Ib_i * (pb.producto_cruz(n))).producto_cruz(pb)
                    );
            j = numerator / (term1 + term2 + term3 + term4);
            J = n * j;

            // Se calcula el `torque impulsivo` 
            torque_impulsivo_a = pa.producto_cruz(J);
            torque_impulsivo_b = pb.producto_cruz(J*(-1));

            // Se aplican `J` y `torque_impulsivo`
            if ( !A->estoy_fijo() ) {
                A->set_velocidad_absoluta(
                    A->velocidad_absoluta() + J * (1/A->masa()));
                A->set_velocidad_angular_absoluta(
                    A->velocidad_angular_absoluta() + Ia_i*torque_impulsivo_a);
            }
            if ( !B->estoy_fijo() ) {
                B->set_velocidad_absoluta(
                    B->velocidad_absoluta() - J * (1/B->masa()));
                B->set_velocidad_angular_absoluta(
                    B->velocidad_angular_absoluta() + Ib_i*torque_impulsivo_b);
            }

            // Aqui se avanza de nuevo en el tiempo a cada pareja de cosas 
            // hasta el instante despues de chocarse
            // OJO!: ESTA IDEA TRAE PROBLEMAS!
            //A->actualizar(Contacto->t);
            //B->actualizar(Contacto->t);
          }
          else if ( vrel > EPSILON ) {
            // 2.2. Las cosas se estan alejando!
          }
          else {
            // 2.3. Las cosas estan en contacto!
          }
        ;

        //-------------------------------------------------------------------
        //printf("Colision #%d:[%s, %s]\n", i+1,
        //    A->nombre(), B->nombre());
        //fflush(stdout);
    }

    //- Al final del proceso elimine la lista de contactos -----------------
    for ( i = 0; i < contactos.tam(); i++ ) delete contactos[i];
    contactos.elim();
}

//===========================================================================
//= 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.