/***********************************************************************/
/* gravedad_colision-3D.cpp. Juan Gonzalez. Mayo 2006 */
/*---------------------------------------------------------------------*/
/* LICENCIA GPL */
/* Programa derivado del ejemplo test_boxstack.cpp que viene con las */
/* fuentes del ODE. Por Russell L. Smith */
/*---------------------------------------------------------------------*/
/* Ejemplo "hola mundo" del motor fisico ODE (Open Dynamics Engine) */
/* Es similar al programa gravedad_colision, pero se ha anadido */
/* representacion en 3D, de manera que se puede ver el objeto */
/* cayendo. */
/* Se ha realizado a partir del ejemplo test_boxstack.cpp que viene */
/* en el directorio ode/test de las fuentes del ODE. */
/* Se ha simplificado al maximo y se han anadido comentarios para */
/* que se entienda. */
/*---------------------------------------------------------------------*/
/* Es un ejemplo "hola_mundo" que incluye representacion en 3D. */
/* Se coloca una "caja" a una altura, y al comenzar la simulacion */
/* se puede ver como cae. */
/* Los unicos objetos que hay en el mundo son la caja y un plano */
/* que hace de suelo */
/***********************************************************************/
#include <ode/ode.h>
#include <unistd.h>
#include "drawstuff.h"
//***********************************************************************
// Sobre la libreria DRAWSTUFF.
//-----------------------------------------------------------------------
// Esta libreria no es parte del motor fisico ODE. Se utiliza para
// hacer mas facil el dibujo en 3D
// Inicialmente se dibuja una reticula (grid) que tiene 9 puntos.
// El punto central (amarillo) es el origen (0,0,0).
// El rojo indica la direccion positiva de las Xs
// El azul la direccion positiva de las Ys
// Estos puntos estan separados una distancia de 1
//************************************************************************
/*********************************************/
/* Algunas constantes usadas en el programa */
/*********************************************/
// Numero maximo de puntos de contacto. Es para la deteccion de
// colisiones
#define MAX_CONTACTS 4
//-- 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;
/***************************************************************************/
/* 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,3);
//-- 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 contactos
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);
}
}
}
//**************************************************************
//-- Funcion de retrollamada de comienzo de la simulacion
//-- Se utiliza principalmente para establecer el punto de vista
//-- inicial (posicion de la camara en el universo)
//**************************************************************
static void start()
{
//-- Esta es la posicion de la camara. Se pasan las coordenadas x,y,z
static float xyz[3] = {-6.0f,0.3f,2.0f};
//-- Esta es la orientacion de la camara.
//-- Las componentes son Pan, Tilt (inclinacion) y Roll
//-- El valor (0,0,0) indica que la camara esta apuntando en
//-- direccion positiva de las X, paralelamente al suelo.
static float hpr[3] = {0.0f,-19.0f,0.0f};
//-- Establecer la posicion de la camara
dsSetViewpoint (xyz,hpr);
}
/**************************************************/
/* FUNCION DE RETROLLAMADA. */
/* Se invoca cuando se pulsa una tecla */
/**************************************************/
static void command (int cmd)
{
//-- En este ejemplo no se usan las teclas
printf ("Tecla pulsada: %c\n",cmd);
}
/**************************/
/* DIBUJAR una geometria */
/**************************/
void drawGeom (dGeomID g)
{
int i;
const dReal *pos;
const dReal *R;
dVector3 sides;
//-- Leer la posicion y orintacion de la geometria
pos = dGeomGetPosition (g);
R = dGeomGetRotation (g);
//-- Obtener las tres componentes: altura, ancho, largo de la caja
dGeomBoxGetLengths (g,sides);
//-- Dibujar!
dsDrawBoxD (pos,R,sides);
}
/***************************************************/
/* FUNCION DE RETROLLAMADA */
/* Simulacion de un paso */
/* Se invoca a esta funcion cada vez que hay que */
/* simular un nuevo paso */
/* El argumento pause es 1 si la simulacicon esta */
/* en modo "pause". En ese caso solo hay que */
/* dibujar el universo tal y como esta */
/***************************************************/
static void simLoop (int pause)
{
if (!pause) {
//-- 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
dJointGroupEmpty (contactgroup);
}
//-- Dibujar el objeto
//-- Establecer el color
dsSetColor (1,1,0);
//-- Especificar la textura
dsSetTexture (DS_WOOD);
//-- Dibujar
drawGeom (obj.geom);
//-- Pequena pausa para no cargar tanto la CPU
//-- Si la simulacion va lenta se puede eliminar
usleep(100);
}
/*******************/
/* MAIN */
/*******************/
int main (int argc, char **argv)
{
/*-------------------------------------------------------------*/
/* Establecer parametros y las funciones de retrollamadas */
/* de la libreria de dibujo */
/*-------------------------------------------------------------*/
dsFunctions fn;
//-- Retrollamada de comienzo de la simulacion
fn.start = &start;
//-- Retrollamada de un paso en la simulacion
fn.step = &simLoop;
//-- Retrollamada para tecla pulsada
fn.command = &command;
//-- Otros
fn.version = DS_VERSION;
fn.stop = 0;
fn.path_to_textures = "./textures";
/********************************************************************/
/* 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,-3.0);
//-- 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();
/********************************/
/** COMENZAR LA SIMULACION!!!! */
/********************************/
//-- Este es el bucle principal. Llamara a las funciones de
//-- retrollamada especificadas
//-- Se pasan como parametros las dimensiones de la ventana
dsSimulationLoop (argc,argv,400,300,&fn);
/************************/
/* 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
dWorldDestroy (world);
return 0;
}