/***********************************************************************/
/* gravedad_colision.cpp.  Juan Gonzalez. Mayo 2006                    */
/*---------------------------------------------------------------------*/
/* LICENCIA GPL                                                        */
/*---------------------------------------------------------------------*/
/* Ejemplo "hola mundo" del motor fisico ODE (Open Dynamics Engine)    */
/* Es similar al ejemplo gravedad, pero se ha anadido deteccion de     */
/* colisiones. Se crea un mundo virtual, se crea un suelo infinitao,   */
/* se situa una "caja" a una cierta altura y se comienza la simulacion */
/* La caja caera hasta que choque con el suelo. La salida del programa */
/* es la posicion de la caja en los diferentes instantes de tiempo     */
/*---------------------------------------------------------------------*/
/* Este programa es para consola, no hay representacion grafica en 3D  */
/*---------------------------------------------------------------------*/
/* Los datos devueltos se pueden visualizar con Octave/Matlab          */
/* Ejemplo de uso:                                                     */
/*   $ gravedad_colision > func.m                                      */
/*   $ octave func.m                                                   */
/***********************************************************************/

#include <ode/ode.h>

/*********************************************/
/* Algunas constantes usadas en el programa  */
/*********************************************/

//-- Numero maximo de puntos de contacto. Es para la deteccion de 
//-- colisiones.
#define MAX_CONTACTS 4

//-- Numero de instantes que queremos simular. Este valor se puede
//-- cambiar
#define TICKS   200

//-- Estructura que representa los objetos del universo
//-- Hay dos tipos de elementos:
//--    * Cuerpos (bodys): Tienen la informacion sobre posicion,
//--      orientacion, velocidad lineal y velocidad de rotacion
//--    * Elementos geometricos: Determinal la forma y se usan para
//--      las colisiones
//-- Un objeto esta formado por el cuerpo y su geometria (o la 
//--  composicion de varias geometrias.
struct MyObject {
  dBodyID body;       // Cuerpo
  dGeomID geom;       // Elemento geometrico
};

//*******************************************
// VARIABLES GLOBALES DEL PROGRAMA
//*******************************************

//-- Identificador para el mundo
static dWorldID world;

//-- Identificador del espacio (para colisiones)
//-- Para detectar las colisiones hay que crear un espacio con los
//-- elementos que pueden colisionar.
static dSpaceID space;

//-- Identificador del grupo de articulaciones de los puntos de contactos
//-- Cuando hay una colision, se crean puntos de contacto entre las 
//-- superficies y actuan como articulaciones: los objetos rotaran con
//-- respecto a estos puntos de contacto.
static dJointGroupID contactgroup;

//-- El objeto que situamos en el mundo: la caja
static MyObject obj;

//-- Contador de cucantos tics de simulacion quedan
static int ticks = TICKS;

/***************************************************************************/
/*  CODIGO                                                                 */
/***************************************************************************/

/********************************************************************/
/* Crear la "caja"                                                  */
/* Se define la "caja" usando la API de ODE y se asocia al "mundo"  */
/********************************************************************/
void Crear_objeto()
{
  dMass m;
  
  //-- Crear el Cuerpo y asociarlo al mundo
  obj.body = dBodyCreate (world);

  //-- Establecer la posicion inicial. Se pasan las coordenadas x,y,z
  //-- En este ejemplo el objeto esta en el origen, a una altura de 
  //-- 4 unidades
  dBodySetPosition(obj.body, 0,0,4);

  //-- Establecer la rotacion
  //-- Cada objeto puede estar rotado. Se crea una matriz de
  //-- rotacion pasandole como parametros el vector que hace de
  //-- eje de giro y el angulo que se rota. En este ejemplo se pasa
  //-- el eje z y se rota '0' grados con respecto a el.
  //-- Probar este mismo ejemplo con los parametros (R,0,0,1,45)
  dMatrix3 R;
  dRFromAxisAndAngle(R,0,0,1,0);
  dBodySetRotation (obj.body,R);
  
  //-- Establecer la masa del cuerpo
  //-- Hay que especificar la masa total y las dimensiones
  //-- del cubo. Como masa se toma 0.5 y las dimensiones de la caja
  //-- son 0.5 x 0.5 x 0.1
  dMassSetBoxTotal (&m,0.5,0.5,0.5,0.1);
  dBodySetMass (obj.body,&m);

  //-- Crear la geometria: un cubo que se anadira al espacio para detectar
  //-- las colisiones. Las dimensiones son 0.5 x 0.5 x 0.1
  obj.geom = dCreateBox (space,0.5,0.5,0.1);

  //-- Asociar el cuerpo con la geometria
  dGeomSetBody (obj.geom,obj.body);
}

/****************************************************************/
/* Funcion de retrollamada invocada por dSpaceCollide cuando    */
/* dos objetos del espacio estan a punto de colisionar          */
/* El ODE permite que los usuarios avanzados puedan implementar */
/* su propia rutina de colision. Para los usuarios no expertos  */
/* que simplemente quieren una colision estandar, esta es la    */
/* rutina que SIEMPRE deberan usar. No es solo valida para este */
/* ejemplo, vale para cualquiera. Pero no esta incluida en la   */
/* libreria del ODE para que se pueda adaptar a otras           */
/* necesidades.
/****************************************************************/
static void nearCallback (void *data, dGeomID o1, dGeomID o2)   
{
  int i;
 
  //-- Obtener los cuerpos asociados
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);

  //-- Si ya estan conectados por una articulacion, terminar
  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
 
  //-- Crear los puntos de contacto, que en realidad son articulaciones
  //-- Para mas informacion consultar la documentacion de ODE
  //-- Aqui se puede especificar el tipo de superficies, los
  //-- coeficientes de rozamiento (mu), etc...
  dContact contact[MAX_CONTACTS]; 
  for (i=0; i<MAX_CONTACTS; i++) {
    contact[i].surface.mode = dContactBounce | dContactSoftCFM;
    contact[i].surface.mu = dInfinity;
    contact[i].surface.mu2 = 0;
    contact[i].surface.bounce = 0.1;
    contact[i].surface.bounce_vel = 0.1;
    contact[i].surface.soft_cfm = 0.01;
  }

  //-- Obtener los puntos de contacto
  int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom, sizeof(dContact));

  //-- Si hay al menos algun punto de contacto...
  if (numc!=0) {
    
    //-- Para cada punto de contacto crear una articulacion
    for (i=0; i<numc; i++) {

      //-- Crear articulacion y meterla en el grupo contactgroup
      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);

      //-- Establecer la artiulacion entre los dos cuerpos
      dJointAttach (c,b1,b2);
    }
  }
}

/************************************************************************/
/* Realizar un paso de simulacion. Primero se comprueba colisiones. Si  */
/* hay alguna, se crean automaticamente (por medio de la funcion        */
/* nearCallback, puntos de contactos que en realidad son articulaciones */
/* Despues se realiza un paso de la simulacion. Los objetos rotaran en  */
/* contacto rotaran sobre los nuevos puntos de contacto creados.        */
/* Finalmente se eliminan.                                              */
/************************************************************************/
static void simLoop()
{
  //-- Deteccion de colisiones. Determinar que pares de elementos geometricos
  //-- estan a punto de colisionar. Se llama a la funcion de retrollamada
  //-- nearCallback, pasando como argumento los dos elementos.
  dSpaceCollide (space,0,&nearCallback);

  //-- Realizar un paso de simulacion
  dWorldStep(world,0.01);

  // Eliminar todos los puntos de contacto (articulaciones) creados.
  dJointGroupEmpty (contactgroup);
}

/*******************/
/*     MAIN        */
/*******************/
int main (int argc, char **argv)
{
  const dReal *pos;

  /********************************************************************/
  /* Crear el mundo. Es un contenedor de todos los objetos a simular  */
  /* El mundo no sabe nada de como se dibujan los objetos             */
  /* En este ejemplo el mundo solo tiene un suelo y una "caja"        */
  /********************************************************************/
  //-- Crear mundo
  world = dWorldCreate();
  
  //-- Establecer la gravedad (gravedad terrestres: -9.81)
  dWorldSetGravity (world,0,0,-9.81);

  //-- Establecer parametro CFM
  //-- Normalmente se deja siempre a este valor
  dWorldSetCFM (world,1e-5);
  
  //-- Establecer el modo auto-disabled por defecto
  //-- Cualquier objeto que se encuentre en reposo se deshabilitara
  //-- y no consumira recursos en la simulacion
  dWorldSetAutoDisableFlag (world,1);
  
  //-- Otros parametros... (consular documentacion)
  //-- En principio siempre tendran esos valores
  dWorldSetContactMaxCorrectingVel (world,0.1);
  dWorldSetContactSurfaceLayer (world,0.001);
  
  //-- Crear un espacio. Los espacios contienen los elementos geometricos
  //-- sobre las que se quiere comprobar si hay colision o no
  //-- Se utilizan espacios para que la simulacion sea mas rapida 
  space = dHashSpaceCreate (0);
  
  //-- Crear un grupo de articulaciones
  //-- Se utiliza para almacenar los puntos de contacto en una colision
  contactgroup = dJointGroupCreate (0);
  
  //-- Crear un plano infinito y meterlo en el espacio
  //-- El plano se determina por su ecuacion del tipo:
  //-- a*x + b*y + c*z = d, donde (a,b,c) es un vector unitario
  //-- normal a su superficie
  //-- En este ejemplo se crea el plano con vector (0,0,1), es
  //-- decir, el plano: z=0
  //-- Este plano sera para nosotros el "suelo"
  dCreatePlane (space,0,0,1,0);
  
  //-- Inicializar los objetos del universo a cero
  memset (&obj,0,sizeof(obj));

  //-- Crear la "Caja" y ponerla en el "mundo"
  Crear_objeto();

  //-- Salida para Octave:
  //-- La matriz z es la que se ira rellenando con los valores de la altura
  //-- de la caja
  printf ("z=[");

  /********************************/
  /** COMENZAR LA SIMULACION!!!!  */
  /********************************/
  //-- Se haran tantos pasos de simulacion como se indican en la 
  //-- constante TICKS
  for (ticks=TICKS; ticks>0; ticks--) {

    //-- Realizar un paso de la simulacion, comprobando colisiones
    simLoop();

    //-- Leer la altura del objeto e imprimirla
    //-- Pos es el vector de posicion, que tiene 3 componentes:
    //-- pos[0] --> x;  pos[1]--> y; pos[2] --> z
    //-- Solo nos interesa la altura a la que esta la caja (pos[2])
    pos=dBodyGetPosition(obj.body);
    printf ("%f,",pos[2]);
}

  //-- Simulacion finalizada:
  //-- Imprimir la ultima posicion e
  //-- Imprimir comandos octave para sacar grafica
  pos=dBodyGetPosition(obj.body);
  printf ("%f];\n",pos[2]);
  printf ("t=0:1:%d;\n",TICKS);
  printf ("plot(t,z);\n");
  printf ("grid on;\n");
  printf ("pause;\n");

  /************************/
  /* FIN DE LA SIMULACION */
  /************************/

  //-- Destruir el grupo de articulaciones
  dJointGroupDestroy (contactgroup);

  //-- Destruir el espacio de colisiones
  dSpaceDestroy (space);

  //-- Destruir el mundo con todos sus objetos (apocalipsis?)
  dWorldDestroy (world);

  return 0;
}