/***********************************************************************/
/* gravedad.cpp.  Juan Gonzalez. Mayo 2006                             */
/*---------------------------------------------------------------------*/
/* LICENCIA GPL                                                        */
/*---------------------------------------------------------------------*/
/* Ejemplo "hola mundo" del motor fisico ODE (Open Dynamics Engine)    */
/* Es un programa para consola, no utiliza representacion en 3D        */
/* Se crea un mundo virtual y se situa en el una "caja" de una cierta  */
/* masa y a una cierta altura                                          */
/* Como salida del programa se obtiene una funcion que indica como     */
/* varia la altura de la caja con el tiempo, es decir, como cae        */
/* En el mundo virtual creado NO HAY SUELO, por lo que no hay ninguna  */
/* colision. El objeto caera infinitamente...                          */
/*---------------------------------------------------------------------*/
/* Los datos devueltos se pueden visualizar con Octave/Matlab          */
/* Ejemplo de uso:                                                     */
/*   $ gravedad > func.m                                               */
/*   $ octave func.m                                                   */
/***********************************************************************/

#include <ode/ode.h>

/*********************************************/
/* Algunas constantes usadas en el programa  */
/*********************************************/
//-- Numero de instantes que queremos simular. Este valor se puede
//-- cambiar
#define TICKS   200

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

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

//-- El objeto que vamos a colocar. Sera una caja
static dBodyID body;

//-- 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
  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(body, 0,0,4);
  
  //-- 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. Se pueden poner las que se quiera
  dMassSetBoxTotal (&m,0.5,0.5,0.5,0.1);
  dBodySetMass (body,&m);
}


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

  /********************************************************************/
  /* Primero se crea el mundo (esto es como ser Dios). El mundo es    */
  /* un "contenedor" de objetos                                       */
  /* El mundo no sabe nada de como se dibujan los objetos             */
  /* En este ejemplo el mundo solo tiene 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. Normalmente siempre se
  //-- activara
  dWorldSetAutoDisableFlag (world,1);
  
  //-- 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!!!!  */
  /********************************/
  //-- Este es el bucle principal. 

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

  //-- Realizar un paso de simulacion. Se especifica que resolucion
  //-- en unidades de tiempo se quiere para la simulacion
    dWorldStep(world,0.01);

    //-- 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(body);
    printf ("%f,",pos[2]);
}

  //-- Simulacion finalizada:
  //-- Imprimir la ultima posicion e
  //-- Imprimir comandos octave para sacar grafica
  pos=dBodyGetPosition(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 mundo con todos sus objetos (apocalipsis?)
  dWorldDestroy (world);

  return 0;
}