Vehiculos con ODE (2ª parte)

Muy Buenas!! en la entrada anterior comentamos las modificaciones que le hicimos a un ejemplo de ODE para poder formar distintos tipos de vehiculos. Sin embargo, no pusimos codigo fuente alguno. Supongo que más de uno se quedaria con las ganas :*(, así que en esta entrega os vamos a exponer todo el codigo del ejemplo.

Antes de nada te recuerdo que esto es codigo exclusivamente ODE, no tiene nada de Irrlicht. Ahora mismo estamos trabajando en la integracion de este ejemplo con nuestra jerarquía de clases, de modo que todo el codigo que aquí verás quede hermosamente encapsulado en unas pocas clases. Bueno, vamos allá.

Para empezar, tenemos los includes de ode y drawstuff

#include "ode.h"
#include "drawstuff.h"
#include "texturepath.h"

A continuacion vemos la rista de definiciones que tuvimos que hacer para especificar todas las caracteristicas del vehiculo, tales como dimensiones globales de la carroceria, numero de ruedas, numero de cajas que componen el chasis, tracción, etc

#define LENGTH 1.75    // chassis length
#define WIDTH 0.7    // chassis width
#define HEIGHT 0.25    // chassis height
#define STARTZ 0.5    // starting height of chassis
#define CMASS 1.50        // chassis mass
#define WMASS 0.084    // wheel mass

#define NUM_WHEELS 6
#define NUM_DIRECTIONWHEELS 2
#define NUM_CHASSISBODIES 1
#define NUM_CHASSISGEOMS 3
#define NUM_BODIES (NUM_WHEELS + NUM_CHASSISBODIES)

//#define TRACCION_INICAL 0                     //  -   - Delantera
#define TRACCION_INICAL NUM_DIRECTIONWHEELS     //  |   |     - Trasera
//#define TRACCION_FINAL NUM_DIRECTIONWHEELS    //  |   -     |
#define TRACCION_FINAL NUM_WHEELS               //  - Total   -

Para especificar las dimensiones de cada caja y ruedas tenemos arrays de arrays, tanto para las dimensiones como para el posicionamiento relativo, radios y grosores de las ruedas, etc.

//Configuracion de las cajas para la Cabeza tractora de un Camion
dReal ChassisDim[3][3] =
{
  {LENGTH,WIDTH,HEIGHT},
  {LENGTH*0.5,WIDTH*0.5,HEIGHT},
  {LENGTH*0.25,WIDTH*0.9,1.5*HEIGHT},
};

dReal ChassisPos[3][3] =
{
  {0,0,0},
  {0.55, 0,  HEIGHT + HEIGHT*0.5 -0.1},
  {0.25, 0,0.45}
};

dReal WheelsPos[6][3] =
{
  {    0.35*LENGTH,  0.45*WIDTH, STARTZ -0.5*HEIGHT},
  {    0.35*LENGTH, -0.45*WIDTH, STARTZ -0.5*HEIGHT},
  {   -0.225*LENGTH,  0.4*WIDTH, STARTZ -0.5*HEIGHT},
  {   -0.225*LENGTH, -0.4*WIDTH, STARTZ -0.5*HEIGHT},
  {  -0.4*LENGTH,   0.4*WIDTH,  STARTZ -0.5*HEIGHT},
  {  -0.4*LENGTH,  -0.4*WIDTH,  STARTZ -0.5*HEIGHT}
};

#define RADIUS 0
#define GROSOR 1
dReal WheelsSize[6][2] =
{    //RADIUS GROSOR
  { 0.14,  0.1},
  { 0.14,  0.1},
  { 0.14,  0.2},
  { 0.14,  0.2},
  { 0.14,  0.2},
  { 0.14,  0.2},
};

Como ves, todas las medidas van en función de los defines WIDTH, HEIGHT Y LENGTH. Esto no quiere decir que tenga que ser estrictamente asi. De hecho, todos estos valores serán leidos de fichero.

A continuación tenemos el chorro de constantes mágicas de ODE para los contact joints de colisión y los joints hinge2 para las ruedas.

//Contantes para los ContactJoints
#define MAX_CONTACT_JOINTS 20
#define CONTACTJOIN_MU dInfinity
#define CONTACTJOIN_SLIP1 0.1
#define CONTACTJOIN_SLIP2 0.1
#define CONTACTJOIN_SOFT_ERP 0.5
#define CONTACTJOIN_SOFT_CFM 0.3

//Configuracion del Hinge2 de las ruedas
#define RUEDAS_MOTRICES_ParamFMax2 0.1
#define RUEDAS_FRENO_PROGRESIVO 0.01
#define STEER_MAX 1.0
#define HINGE2_DIRECTIONWHEELS_MAXDIFF_ANGLE 0.1
#define HINGE2_DIRECTIONWHEELS_MAXDIFF_SCALE 10.0
#define HINGE2_DIRECTIONWHEELS_ParamFMax 0.2
#define HINGE2_DIRECTIONWHEELS_ParamLoStop -0.75
#define HINGE2_DIRECTIONWHEELS_ParamHiStop 0.75
#define HINGE2_DIRECTIONWHEELS_ParamFudgeFactor 0.1
#define HINGE2_ParamSuspensionERP 0.4
#define HINGE2_ParamSuspensionCFM 0.8
#define HINGE2_FIXEDWHEELS_ParamLoStop 0
#define HINGE2_FIXEDWHEELS_ParamHiStop 0


//ODE World constants
#define ODE_STEP 0.05
#define ODE_GRAVITY -0.5

Hasta aqui los defines y arrays. En el siguiente bloque puedes ver la definición del mundo y el espacio raiz, el array de bodys y joints, etc., así como las dos variables controladas por el usuario, la velocidad y direccion del vehiculo.

// dynamics and collision objects (chassis, 3 wheels, environment)
static dWorldID world;
static dSpaceID space;
static dBodyID body[NUM_BODIES];
static dJointID joint[NUM_WHEELS];    // joint[0] and [1] are the front wheels
static dJointGroupID contactgroup;
static dGeomID ground;
static dSpaceID car_space;
static dGeomID box[NUM_CHASSISGEOMS];
static dGeomID sphere[NUM_WHEELS];
static dGeomID ground_box;



// things that the user controls

static dReal speed=0,steer=0;    // user commands

Veamos ahora la función llamada por la simulación de ODE para gestion de las colisiones. Esta función es bastante similar a los otros ejemplos vistos de ODE.

// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
int i,n;

bool o1isSpace = dGeomIsSpace (o1);
bool o2isSpace = dGeomIsSpace (o1);
dSpaceID s1 = dGeomGetSpace (o1);
dSpaceID s2 = dGeomGetSpace (o2);
if(!o1isSpace && !o2isSpace && s1 == s2)
    return;

//    only collide things with the ground
//    int g1 = (o1 == ground || o1 == ground_box);
//    int g2 = (o2 == ground || o2 == ground_box);
//    if (!(g1 ^ g2)) return;

dContact contact[MAX_CONTACT_JOINTS];
n = dCollide (o1,o2,MAX_CONTACT_JOINTS,&contact[0].geom,sizeof(dContact));
if (n > 0) {
  for (i=0; i<n; i++) {
    contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
  dContactSoftERP | dContactSoftCFM | dContactApprox1;
    contact[i].surface.mu = CONTACTJOIN_MU;
    contact[i].surface.slip1 = CONTACTJOIN_SLIP1;
    contact[i].surface.slip2 = CONTACTJOIN_SLIP2;
    contact[i].surface.soft_erp = CONTACTJOIN_SOFT_ERP;
    contact[i].surface.soft_cfm = CONTACTJOIN_SOFT_CFM;
    dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
    dJointAttach (c,
          dGeomGetBody(contact[i].geom.g1),
          dGeomGetBody(contact[i].geom.g2));
  }
}
}

Si observas el principio de la funcion veras que se realiza un pequeño testeo. "Si ninguno de los dos geoms son espacios y, ambos pertenecen al mismo espacio, no se realizará tratamiento de colision". Esto es para que el chasis de un coche no colisione con sus ruedas, pero que, por ejemplo dos coches puedan colisionar entre si, y por supuesto, un coche pueda interactuar con el suelo y otros objetos del mundo, como la rampa.

A continuación vemos las funciones llamadas por DrawStuff para la inicialización y el manejo del teclado. Estas funciones no interesan dado que en irrlicht son totalmente distintas.

// start simulation - set viewpoint

static void start()
{
dAllocateODEDataForThread(dAllocateMaskAll);

static float xyz[3] = {2.8317f,-3.9817f,3.8000f};
static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("Press:\t'a' to increase speed.\n"
    "\t'z' to decrease speed.\n"
    "\t',' to steer left.\n"
    "\t'.' to steer right.\n"
    "\t' ' to reset speed and steering.\n"
    "\t'1' to save the current state to 'state.dif'.\n");
}


// called when a key pressed

static void command (int cmd)
{
  switch (cmd)
  {
      case 'w': case 'W': speed += 0.3; break;
      case 's': case 'S': speed -= 0.3; break;
      case 'a': case 'A': steer -= 0.5; break;
      case 'd': case 'D': steer += 0.5; break;
      case ' ': speed = 0; steer = 0;   break;
      case '1':
      {
          FILE *f = fopen ("state.dif","wt");
          if (f)
          {
              dWorldExportDIF (world,f,"");
              fclose (f);
          }
      }
  }
}

Veamos la funcion empleada por DrawStuff para el bucle de simulacion, simLoop.

static void simLoop (int pause)
{

Esta función tiene chicha. Por un lado, modificamos o actualizamos la velocidad de giro de las ruedas motrices, la dirección de las ruedas directrices (nótese que una rueda puede ser motriz y directriz a la vez, como en los 4x4, turismos de tracción delantera, etc.)

    if (!pause)
  {
      // motor
      for(int i=TRACCION_INICAL; i<TRACCION_FINAL; ++i) //ahora mismo es 4x4
      {
          dJointSetHinge2Param (joint[i],dParamVel2,-speed);
          dJointSetHinge2Param (joint[i],dParamFMax2,RUEDAS_MOTRICES_ParamFMax2);
      }
      if        (speed > 0)    speed -= RUEDAS_FRENO_PROGRESIVO;
      else if    (speed < 0)    speed += RUEDAS_FRENO_PROGRESIVO;

      // steering
      for(int i=0; i<NUM_DIRECTIONWHEELS; ++i)
      {
          dReal v = steer - dJointGetHinge2Angle1 (joint[i]);
          if (v > HINGE2_DIRECTIONWHEELS_MAXDIFF_ANGLE)    v = HINGE2_DIRECTIONWHEELS_MAXDIFF_ANGLE;
          if (v < -HINGE2_DIRECTIONWHEELS_MAXDIFF_ANGLE)    v = -HINGE2_DIRECTIONWHEELS_MAXDIFF_ANGLE;
          v *= HINGE2_DIRECTIONWHEELS_MAXDIFF_SCALE;
          dJointSetHinge2Param (joint[i],dParamVel,v);
          dJointSetHinge2Param (joint[i],dParamFMax,HINGE2_DIRECTIONWHEELS_ParamFMax);
          dJointSetHinge2Param (joint[i],dParamLoStop,HINGE2_DIRECTIONWHEELS_ParamLoStop);
          dJointSetHinge2Param (joint[i],dParamHiStop,HINGE2_DIRECTIONWHEELS_ParamHiStop);
          dJointSetHinge2Param (joint[i],dParamFudgeFactor,HINGE2_DIRECTIONWHEELS_ParamFudgeFactor);
      }
      if (steer >  STEER_MAX)    steer =  STEER_MAX;
      if (steer < -STEER_MAX)    steer = -STEER_MAX;
            
      dSpaceCollide (space,0,&nearCallback);
      dWorldStep (world,ODE_STEP);
    
      // remove all contact joints
      dJointGroupEmpty (contactgroup);
  }

Y por otro es la encargada de pintar en pantalla el entorno, con funciones y objetos de DrawStuff. Te vuelvo a recordar que esta parte cambiará bastante cuando pasemos esto a irrlicht.

    dsSetColor (0,0.5,1);
  dsSetTexture (DS_WOOD);

  for (int i=0; i<NUM_CHASSISGEOMS; i++)
      dsDrawBox (dGeomGetPosition(box[i]),dBodyGetRotation(body[0]),ChassisDim[i]);
  dsSetColor (0.33,0.33,0.33);
  for (int i=1; i<=NUM_WHEELS; i++)
      dsDrawCylinder (dBodyGetPosition(body[i]),dBodyGetRotation(body[i]),WheelsSize[i-1][GROSOR],WheelsSize[i-1][RADIUS]);

  dVector3 ss;
  dGeomBoxGetLengths (ground_box,ss);
  dsSetColor (1,1,1);
  dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
}

Pues bien, ahora viene cuando la matan y él está herido, aqui tenemos el main, encargado, en este ejemplo, de inicalizar ODE, DrawStuff, crear los geoms, bodys, joints y toda la pesca. Para empezar tenemos la creación e inicialización de DrawStuff, que no nos interesa, y la de ODE.

int main (int argc, char **argv)
{
  dMass m;

  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
  if(argc==2)
  {
      fn.path_to_textures = argv[1];
  }

  // Inicializacion ODE

  dInitODE2(0);
  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,ODE_GRAVITY);
  ground = dCreatePlane (space,0,0,1,0);

Seguimos con la creación del chasis como un conjunto de cajas, en las que iterativamente vamos sumando las masa de cada caja y la traslada a la posición dada por el array de arrays que vimos al principio.

    dMass m2;
  dMassSetZero (&m);
  body[0] = dBodyCreate (world);
  dBodySetPosition (body[0],0,0,STARTZ);

  for (int k=0; k<NUM_CHASSISGEOMS; k++)
  {
      dMassSetZero (&m2);
      box[k] = dCreateBox (0,ChassisDim[k][0],ChassisDim[k][1],ChassisDim[k][2]);
      dMassSetBox (&m2,1,ChassisDim[k][0],ChassisDim[k][1],ChassisDim[k][2]);
      dMassTranslate (&m2,ChassisPos[k][0],ChassisPos[k][1],ChassisPos[k][2]);
      dMassAdd (&m,&m2);

      dGeomSetBody (box[k],body[0]);
      dGeomSetOffsetPosition(box[k],
              ChassisPos[k][0]-m.c[0],
              ChassisPos[k][1]-m.c[1],
              ChassisPos[k][2]-m.c[2]);
  }
//    for (int k=0; k<NUM_CHASSISGEOMS; k++)
//    {
//        dGeomSetBody (box[k],body[0]);
//        dGeomSetOffsetPosition(box[k],
//                ChassisPos[k][0]-m.c[0],
//                ChassisPos[k][1]-m.c[1],
//                ChassisPos[k][2]-m.c[2]);
//    }
  dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
  dMassAdjust (&m,CMASS);
  dBodySetMass (body[0],&m);

Seguimos con las ruedas. En este caso es mas simple (y no) puesto que cada rueda es un objeto simple, y no compuesto, como el chasis. Lo dificil puede estar en la rotación aplicada a las ruedas.

    // wheel bodies
  for (int i=1; i<=NUM_WHEELS; i++)
  {
      body[i] = dBodyCreate (world);
      dQuaternion q;
      dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
      dBodySetQuaternion (body[i],q);
      dMassSetCylinder (&m, 1, 1,WheelsSize[i-1][RADIUS], WheelsSize[i-1][GROSOR]);
      dMassAdjust (&m,WMASS);
      dBodySetMass (body[i],&m);
      sphere[i-1] = dCreateCylinder (0, WheelsSize[i-1][RADIUS], WheelsSize[i-1][GROSOR]);

      dGeomSetBody (sphere[i-1],body[i]);
      dBodySetPosition (body[i], WheelsPos[i-1][0],WheelsPos[i-1][1],WheelsPos[i-1][2]);
  }

He aquí la parte importante del ejemplo, la creación de los joints de tipo hinge2, la configuración de la suspensión, simulada con los parametros de ERP y CFM y, la configuración de restricción de las ruedas traseras para que no puedan girar libremente como las de delante.

    // front and back wheel hinges
  for (int i=0; i<NUM_WHEELS; i++)
  {
      joint[i] = dJointCreateHinge2 (world,0);
      dJointAttach (joint[i],body[0],body[i+1]);
      const dReal *a = dBodyGetPosition (body[i+1]);
      dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
      dJointSetHinge2Axis1 (joint[i],0,0,1);
      dJointSetHinge2Axis2 (joint[i],0,1,0);
  }

  // set joint suspension
  for (int i=0; i<NUM_WHEELS; i++)
  {
      dJointSetHinge2Param (joint[i],dParamSuspensionERP,HINGE2_ParamSuspensionERP);
      dJointSetHinge2Param (joint[i],dParamSuspensionCFM,HINGE2_ParamSuspensionCFM);
  }
  // lock back wheels along the steering axis
  for (int i=NUM_DIRECTIONWHEELS; i<NUM_WHEELS; i++)
  {
      dJointSetHinge2Param (joint[i],dParamLoStop,HINGE2_FIXEDWHEELS_ParamLoStop);
      dJointSetHinge2Param (joint[i],dParamHiStop,HINGE2_FIXEDWHEELS_ParamHiStop);
  }

Llegamos al último paso de la creación e inicialización. Creamos el espacio de colisiones del coche, carspace, le agregamos los geoms del chasis y las ruedas. Creamos también la rampa de salto como una caja inclinada.

    // create car space and add it to the top level space
  car_space = dSimpleSpaceCreate (space);
  dSpaceSetCleanup (car_space,0);
  for(int i=0; i<NUM_CHASSISGEOMS; ++i)
      dSpaceAdd (car_space,box[i]);
  for(int i=0; i<NUM_WHEELS; ++i)
      dSpaceAdd (car_space,sphere[i]);

  // environment
  ground_box = dCreateBox (space,2,1.5,1);
  dMatrix3 R;
  dRFromAxisAndAngle (R,0,1,0,-0.15);
  dGeomSetPosition (ground_box,2,0,-0.34);
  dGeomSetRotation (ground_box,R);

Inicializado todo el entorno, es hora de lanzar la simulacion con la llamada pertinente a la funcion de DrawStuff. Éste hara las llamadas oportunas a start, simLoop, y command. Obviamente, esta parte cambiará radicalmente en Irrlicht.

    dsSimulationLoop (argc,argv,352,288,&fn);

Para terminar, debemos dejar la cocina limpia y aseada, así que destruimos el chasis, las ruedas, el espacio global, el mundo y finalizamos ODE.

    //Destruccion del vehiculo
  for(int i=0; i<NUM_CHASSISGEOMS; ++i)
      dGeomDestroy (box[i]);
  for(int i=0; i<NUM_WHEELS; ++i)
      dGeomDestroy (sphere[i]);

  //destruccion de ode
  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}

Hasta aquí todo el codigo del demo_buggy.cpp modificado. Si de verdad quieres compilar el codigo fuente bájate el paquetón de ODE aquí y reemplaza el ode-0.10.0\ode\demo\demo_buggy.cpp por el nuestro. Para compilarlo tendrás que ejecutar ./configure && make. Lamento no haber puesto en el zip un triste makefile ni las librerias.

En la próxima entrega abordaremos el tema de la integración de los vehiculos ODE e Irrlicht.

Sigue leyendo >

Vehiculos con ODE

Muy buenas otra vez, estos dias hemos estado bastante enmarronados comprendiendo la forma de trabajar que tiene ODE. Hace unos dias nos cogimos un par de ejemplos de ODE, entre ellos el demo_buggy (un pequeño cochecito de 3 ruedas que salta una rampa) para, a partir de él, llegar a construir la parte fisica de los vehiculos.

From Menus con Irr...

Podríamos utilizar el modelo 3D de cualquier coche y utilizarlo como malla de colisiones pero ODE desaconseja su uso debido a las perdidas de rendimiento (enorme), precisión y sobretodo estabilidad. Además, piénsalo detenidamente, cualquier modelo 3D de un vehiculo, personaje, etc. tiene cientos o miles de poligonos. Sin embargo, una caja como carroceria solo son 12 triangulicos.

Inicialmente pensamos en construir los vehiculos como una simple caja con 4 ruedas conectadas, pero empezamos a pensar en los camiones (mas ruedas), monovolumenes, tractores, autobuses, y, decidimos usar mas de una caja a fin de poder ajustar la forma fisica a la del modelo visual, pero aumentando minimamente la complejidad del modelo fisico.

El ejemplo de ODE, demo_buggy tiene tres ruedas, así que, evidentemente, lo primero que hicimos fue colocarle una cuarta y posicionarlas como es debido.

Lo siguiente que vimos fue que, el modelo fisico de las ruedas eran esferas, para mejorar el rendimiento. El problema que presentaba esto era que al pasar junto a un obstaculo, las esferas tropiezan con éste, con lo que las cambiamos por cilindros.

Despues fue el turno de las cajas. Para ello construimos un objeto compuesto por varias cajas. Esta parte nos llevó un poco más de tiempo puesto que el código de ejemplo de demo_boxstack de ODE es un poco críptico.

Ya solo restaba sustituir valores fijos por defines, matrices de medidas para las cajas, radios y anchura de las ruedas, etc. Tambien hemos dispuesto variables para cambiar facilmente el numero de ejes asi como de cajas.

Nuestra intención es implementar un editor de vehiculos para, de manera sencilla, poder añadir nuevos vehiculos, esto es, ajustando un modelo fisico a una caroceria. De momento nos entretuvimos en montar tres vehiculos de prueba.

El primero de ellos es un simple turismo a lo manovolumen.

From Menus con Irr...

El siguiente es la flagoneta de los malacatones.

From Menus con Irr...

Y por último, pero no por ello menos importante, como dijo Shia LaBeouf en Transformers: Señores, les presento a mi amigo: Optimus Prime.

From Menus con Irr...

Puedes bajarte los ejecutables aquí y probarlos tú mismo. las teclas de direccion del vehiculo son la cruz formada por las teclas WASD, salvo en el buggy original que sí que son las que indica la ventana del shell.

Ah! se me olvidaba. No os preocupeis por la falta de codigo, en breve lo postearemos ;). Es solo que estos ejemplos son puro codigo ODE y prefiero mostrarlo cuando lo tengamos integrado en el proyecto con Irrlicht.

Sigue leyendo >