Beispiel #1
0
IoObject *IoDrawStuff_dsDrawTriangle(IoDrawStuff *self, IoObject *locals, IoMessage *m)
{
    float *pos;
    float *R;
    float *v0;
    float *v1;
    float *v2;
    int solid;
    //IoSeq_assertIsVector(self, locals, m);
    {
    IoSeq *other = IoMessage_locals_vectorArgAt_(m, locals, 0);
    pos = IoSeq_floatPointerOfLength_(other, 3);
    
    other = IoMessage_locals_vectorArgAt_(m, locals, 1);
    R = IoSeq_floatPointerOfLength_(other, 12);
    
    other = IoMessage_locals_vectorArgAt_(m, locals, 1);
    v0 = IoSeq_floatPointerOfLength_(other, 3);
    other = IoMessage_locals_vectorArgAt_(m, locals, 2);
    v1 = IoSeq_floatPointerOfLength_(other, 3);
    other = IoMessage_locals_vectorArgAt_(m, locals, 3);
    v2 = IoSeq_floatPointerOfLength_(other, 3);

    solid = IoMessage_locals_intArgAt_(m, locals, 4);
    }
    dsDrawTriangle(pos, R, v0, v1, v2, solid);
    return self;
}
Beispiel #2
0
void drawGeom(dGeomID g)
{
    int gclass = dGeomGetClass(g);
    const dReal *pos = dGeomGetPosition(g);
    const dReal *rot = dGeomGetRotation(g);

    switch (gclass) {
        case dSphereClass:
            dsSetColorAlpha(0, 0.75, 0.5, 0.5);
            dsSetTexture (DS_CHECKERED);
            dsDrawSphere(pos, rot, dGeomSphereGetRadius(g));
            break;
        case dBoxClass:
        {
            dVector3 lengths;
            dsSetColorAlpha(1, 1, 0, 0.5);
            dsSetTexture (DS_WOOD);
            dGeomBoxGetLengths(g, lengths);
            dsDrawBox(pos, rot, lengths);
            break;
        }
        case dTriMeshClass: 
        {
            int numi = dGeomTriMeshGetTriangleCount(g);

            for (int i=0; i<numi; ++i) {
                dVector3 v0, v1, v2;
                dGeomTriMeshGetTriangle(g, i, &v0, &v1, &v2);

                dsSetTexture (DS_WOOD);

                dsSetDrawMode(DS_WIREFRAME);
                dsSetColorAlpha(0, 0, 0, 1.0);
                dsDrawTriangle(pos, rot, v0, v1, v2, true);

                dsSetDrawMode(DS_POLYFILL);
                dsSetColorAlpha(1, 1, 0, 0.5);
                dsDrawTriangle(pos, rot, v0, v1, v2, true);
            }
            break;
        }
        
        default:
        {}
    }
}
Beispiel #3
0
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldStepFast (world,0.05, 1);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }

  /*{
    for (int i = 1; i < IndexCount; i++) {
      dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
    }
  }*/

  {const dReal* Pos = dGeomGetPosition(TriMesh);
  const dReal* Rot = dGeomGetRotation(TriMesh);

  {for (int i = 0; i < IndexCount / 3; i++){
    const dVector3& v0 = Vertices[Indices[i * 3 + 0]];
	const dVector3& v1 = Vertices[Indices[i * 3 + 1]];
	const dVector3& v2 = Vertices[Indices[i * 3 + 2]];
	dsDrawTriangle(Pos, Rot, (dReal*)&v0, (dReal*)&v1, (dReal*)&v2, 0);
  }}}

  if (Ray){
	  dVector3 Origin, Direction;
	  dGeomRayGet(Ray, Origin, Direction);
	  
	  dReal Length = dGeomRayGetLength(Ray);
	  
	  dVector3 End;
	  End[0] = Origin[0] + (Direction[0] * Length);
	  End[1] = Origin[1] + (Direction[1] * Length);
	  End[2] = Origin[2] + (Direction[2] * Length);
	  End[3] = Origin[3] + (Direction[3] * Length);
	  
	  dsDrawLine(Origin, End);
  }
}
Beispiel #4
0
static void simLoop (int pause)
{
  double simstep = 0.001; // 1ms simulation steps
  double dt = dsElapsedTime();

  int nrofsteps = (int) ceilf(dt/simstep);
//  fprintf(stderr, "dt=%f, nr of steps = %d\n", dt, nrofsteps);

  for (int i=0; i<nrofsteps && !pause; i++)
  {
    dSpaceCollide (space,0,&nearCallback);
    dWorldQuickStep (world, simstep);
    dJointGroupEmpty (contactgroup);
  }

  dsSetColor (1,1,1);
  const dReal *SPos = dBodyGetPosition(sphbody);
  const dReal *SRot = dBodyGetRotation(sphbody);
  float spos[3] = {SPos[0], SPos[1], SPos[2]};
  float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
  dsDrawSphere
  (
    spos, 
    srot, 
    RADIUS
  );

  // draw world trimesh
  dsSetColor(0.4,0.7,0.9);
  dsSetTexture (DS_NONE);

  const dReal* Pos = dGeomGetPosition(world_mesh);
  //dIASSERT(dVALIDVEC3(Pos));
  float pos[3] = { Pos[0], Pos[1], Pos[2] };

  const dReal* Rot = dGeomGetRotation(world_mesh);
  //dIASSERT(dVALIDMAT3(Rot));
  float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };

  int numi = sizeof(world_indices)  / sizeof(int);

  for (int i=0; i<numi/3; i++)
  {
    int i0 = world_indices[i*3+0];
    int i1 = world_indices[i*3+1];
    int i2 = world_indices[i*3+2];
    float *v0 = world_vertices+i0*3;
    float *v1 = world_vertices+i1*3;
    float *v2 = world_vertices+i2*3;
    dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
  }
}
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);


#if 1
  // What is this for??? - Bram
  if (!pause) 
  {
    for (int i=0; i<num; i++)
      for (int j=0; j < GPB; j++)
        if (obj[i].geom[j])
          if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass)
            setCurrentTransform(obj[i].geom[j]);
 
    setCurrentTransform(TriMesh1);
    setCurrentTransform(TriMesh2);
  }
#endif

  //if (!pause) dWorldStep (world,0.05);
  if (!pause) dWorldQuickStep (world,0.05);

  for (int j = 0; j < dSpaceGetNumGeoms(space); j++){
	  dSpaceGetGeom(space, j);
  }

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (obj[i].geom[j]) {
        if (i==selected) {
          dsSetColor (0,0.7,1);
        }
        else if (! dBodyIsEnabled (obj[i].body)) {
          dsSetColor (1,0,0);
        }
        else {
          dsSetColor (1,1,0);
        }
      
        if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass) {
          dTriIndex* Indices = (dTriIndex*)::Indices;

          // assume all trimeshes are drawn as bunnies
          const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
          const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);
        
          for (int ii = 0; ii < IndexCount / 3; ii++) {
            const dReal v[9] = { // explicit conversion from float to dReal
              Vertices[Indices[ii * 3 + 0] * 3 + 0],
              Vertices[Indices[ii * 3 + 0] * 3 + 1],
              Vertices[Indices[ii * 3 + 0] * 3 + 2],
              Vertices[Indices[ii * 3 + 1] * 3 + 0],
              Vertices[Indices[ii * 3 + 1] * 3 + 1],
              Vertices[Indices[ii * 3 + 1] * 3 + 2],
              Vertices[Indices[ii * 3 + 2] * 3 + 0],
              Vertices[Indices[ii * 3 + 2] * 3 + 1],
              Vertices[Indices[ii * 3 + 2] * 3 + 2]
            };
            dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
          }

          // tell the tri-tri collider the current transform of the trimesh --
          // this is fairly important for good results.
          
		  // Fill in the (4x4) matrix.
		  dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );

		  p_matrix[ 0 ] = Rot[ 0 ];	p_matrix[ 1 ] = Rot[ 1 ];	p_matrix[ 2 ] = Rot[ 2 ];	p_matrix[ 3 ] = 0;
		  p_matrix[ 4 ] = Rot[ 4 ];	p_matrix[ 5 ] = Rot[ 5 ];	p_matrix[ 6 ] = Rot[ 6 ];	p_matrix[ 7 ] = 0;
		  p_matrix[ 8 ] = Rot[ 8 ];	p_matrix[ 9 ] = Rot[ 9 ];	p_matrix[10 ] = Rot[10 ];	p_matrix[11 ] = 0;
		  p_matrix[12 ] = Pos[ 0 ];	p_matrix[13 ] = Pos[ 1 ];	p_matrix[14 ] = Pos[ 2 ];	p_matrix[15 ] = 1;

		  // Flip to other matrix.
		  obj[i].last_matrix_index = !obj[i].last_matrix_index;

		  dGeomTriMeshSetLastTransform( obj[i].geom[j], 
			  *(dMatrix4*)( obj[i].matrix_dblbuff + obj[i].last_matrix_index * 16 ) );
  
        } else {
          drawGeom (obj[i].geom[j],0,0,show_aabb);
        }
      }
    }
  }

  dTriIndex* Indices = (dTriIndex*)::Indices;

  {const dReal* Pos = dGeomGetPosition(TriMesh1);
  const dReal* Rot = dGeomGetRotation(TriMesh1);

  {for (int i = 0; i < IndexCount / 3; i++){
    const dReal v[9] = { // explicit conversion from float to dReal
      Vertices[Indices[i * 3 + 0] * 3 + 0],
      Vertices[Indices[i * 3 + 0] * 3 + 1],
      Vertices[Indices[i * 3 + 0] * 3 + 2],
      Vertices[Indices[i * 3 + 1] * 3 + 0],
      Vertices[Indices[i * 3 + 1] * 3 + 1],
      Vertices[Indices[i * 3 + 1] * 3 + 2],
      Vertices[Indices[i * 3 + 2] * 3 + 0],
      Vertices[Indices[i * 3 + 2] * 3 + 1],
      Vertices[Indices[i * 3 + 2] * 3 + 2]
    };
    dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 0);
  }}}

  {const dReal* Pos = dGeomGetPosition(TriMesh2);
  const dReal* Rot = dGeomGetRotation(TriMesh2);

  {for (int i = 0; i < IndexCount / 3; i++){
    const dReal v[9] = { // explicit conversion from float to dReal
      Vertices[Indices[i * 3 + 0] * 3 + 0],
      Vertices[Indices[i * 3 + 0] * 3 + 1],
      Vertices[Indices[i * 3 + 0] * 3 + 2],
      Vertices[Indices[i * 3 + 1] * 3 + 0],
      Vertices[Indices[i * 3 + 1] * 3 + 1],
      Vertices[Indices[i * 3 + 1] * 3 + 2],
      Vertices[Indices[i * 3 + 2] * 3 + 0],
      Vertices[Indices[i * 3 + 2] * 3 + 1],
      Vertices[Indices[i * 3 + 2] * 3 + 2]
    };
    dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
  }}}
}
Beispiel #6
0
static void simLoop (int pause)
{
  double simstep = 0.005; // 5ms simulation steps
  double dt = dsElapsedTime();
  int nrofsteps = (int) ceilf(dt/simstep);
  for (int i=0; i<nrofsteps && !pause; i++)
  {
    dSpaceCollide (space,0,&nearCallback);
    dWorldQuickStep (world, simstep);
    dJointGroupEmpty (contactgroup);
  }

  dsSetColor (1,1,1);
#ifdef BOX
  const dReal *BPos = dBodyGetPosition(boxbody);
  const dReal *BRot = dBodyGetRotation(boxbody);
  float bpos[3] = {BPos[0], BPos[1], BPos[2]};
  float brot[12] = { BRot[0], BRot[1], BRot[2], BRot[3], BRot[4], BRot[5], BRot[6], BRot[7], BRot[8], BRot[9], BRot[10], BRot[11] };
  float sides[3] = {BOXSZ, BOXSZ, BOXSZ};
  dsDrawBox
  (
    bpos, 
    brot, 
    sides
  ); // single precision
#endif
#ifdef CYL
  const dReal *CPos = dGeomGetPosition(cylgeom);
  const dReal *CRot = dGeomGetRotation(cylgeom);
  float cpos[3] = {CPos[0], CPos[1], CPos[2]};
  float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] };
  dsDrawCylinder
  ( 
//    dBodyGetPosition(cylbody),
//    dBodyGetRotation(cylbody),
    cpos,
    crot,
    WHEELW,
    RADIUS
  ); // single precision
#endif

  // draw world trimesh
  dsSetColor(0.7,0.7,0.4);
  dsSetTexture (DS_NONE);

  const dReal* Pos = dGeomGetPosition(world_mesh);
  float pos[3] = { Pos[0], Pos[1], Pos[2] };

  const dReal* Rot = dGeomGetRotation(world_mesh);
  float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };

  int numi = sizeof(world_indices)  / sizeof(dTriIndex);

  for (int i=0; i<numi/3; i++)
  {
    int i0 = world_indices[i*3+0];
    int i1 = world_indices[i*3+1];
    int i2 = world_indices[i*3+2];
    float *v0 = world_vertices+i0*3;
    float *v1 = world_vertices+i1*3;
    float *v2 = world_vertices+i2*3;
    dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
  }
}
static void simLoop (int pause)
{
  int i,j;
  
  dsSetColor (0,0,2);
  
  dSpaceCollide (space,0,&nearCallback);
  
  //if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldQuickStep (world,0.05);
  if (!pause) dWorldStepFast1 (world,0.05, 5);


  if (write_world) {
    FILE *f = fopen ("state.dif","wt");
    if (f) {
      dWorldExportDIF (world,f,"X");
      fclose (f);
    }
    write_world = 0;
  }

  // remove all contact joints
  dJointGroupEmpty (contactgroup);



	const dReal* pReal = dGeomGetPosition( gheight );

	const dReal* RReal = dGeomGetRotation( gheight );

	//
	// Draw Heightfield
	//

	// Set ox and oz to zero for DHEIGHTFIELD_CORNER_ORIGIN mode.
	int ox = (int) ( -HFIELD_WIDTH/2 );
	int oz = (int) ( -HFIELD_DEPTH/2 );

//	for ( int tx = -1; tx < 2; ++tx )
//	for ( int tz = -1; tz < 2; ++tz )
	{
		dsSetColorAlpha (0.5,1,0.5,0.5);
		dsSetTexture( DS_WOOD );

		for ( int i = 0; i < HFIELD_WSTEP - 1; ++i )
		for ( int j = 0; j < HFIELD_DSTEP - 1; ++j )
		{
			dReal a[3], b[3], c[3], d[3];

			a[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
			a[ 1 ] = heightfield_callback( NULL, i, j );
			a[ 2 ] = oz + ( j ) * HFIELD_DSAMP;

			b[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
			b[ 1 ] = heightfield_callback( NULL, i + 1, j );
			b[ 2 ] = oz + ( j ) * HFIELD_DSAMP;

			c[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
			c[ 1 ] = heightfield_callback( NULL, i, j + 1 );
			c[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;

			d[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
			d[ 1 ] = heightfield_callback( NULL, i + 1, j + 1 );
			d[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;

			dsDrawTriangle( pReal, RReal, a, c, b, 1 );
			dsDrawTriangle( pReal, RReal, b, c, d, 1 );
		}
	}





	dsSetColor (1,1,0);
	dsSetTexture (DS_WOOD);
	for (i=0; i<num; i++)
	{
		for (j=0; j < GPB; j++)
		{
			if (i==selected)
			{
				dsSetColor (0,0.7,1);
			}
			else if (! dBodyIsEnabled (obj[i].body))
			{
				dsSetColor (1,0.8,0);
			}
			else 
			{
				dsSetColor (1,1,0);
			}

			if ( obj[i].geom[j] && dGeomGetClass(obj[i].geom[j]) == dTriMeshClass )
			{
				dTriIndex* Indices = (dTriIndex*)::Indices;

				// assume all trimeshes are drawn as bunnies
				const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
				const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);

				for (int ii = 0; ii < IndexCount / 3; ii++)
				{
					const dReal v[9] = { // explicit conversion from float to dReal
						Vertices[Indices[ii * 3 + 0] * 3 + 0],
							Vertices[Indices[ii * 3 + 0] * 3 + 1],
							Vertices[Indices[ii * 3 + 0] * 3 + 2],
							Vertices[Indices[ii * 3 + 1] * 3 + 0],
							Vertices[Indices[ii * 3 + 1] * 3 + 1],
							Vertices[Indices[ii * 3 + 1] * 3 + 2],
							Vertices[Indices[ii * 3 + 2] * 3 + 0],
							Vertices[Indices[ii * 3 + 2] * 3 + 1],
							Vertices[Indices[ii * 3 + 2] * 3 + 2]
					};
					dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
				}

				// tell the tri-tri collider the current transform of the trimesh --
				// this is fairly important for good results.

				// Fill in the (4x4) matrix.
				dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );

				p_matrix[ 0 ] = Rot[ 0 ];	p_matrix[ 1 ] = Rot[ 1 ];	p_matrix[ 2 ] = Rot[ 2 ];	p_matrix[ 3 ] = 0;
				p_matrix[ 4 ] = Rot[ 4 ];	p_matrix[ 5 ] = Rot[ 5 ];	p_matrix[ 6 ] = Rot[ 6 ];	p_matrix[ 7 ] = 0;
				p_matrix[ 8 ] = Rot[ 8 ];	p_matrix[ 9 ] = Rot[ 9 ];	p_matrix[10 ] = Rot[10 ];	p_matrix[11 ] = 0;
				p_matrix[12 ] = Pos[ 0 ];	p_matrix[13 ] = Pos[ 1 ];	p_matrix[14 ] = Pos[ 2 ];	p_matrix[15 ] = 1;

				// Flip to other matrix.
				obj[i].last_matrix_index = !obj[i].last_matrix_index;

				// Apply the 'other' matrix which is the oldest.
				dGeomTriMeshSetLastTransform( obj[i].geom[j], 
					*(dMatrix4*)( obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 ) ) );
			}
			else
			{
				drawGeom (obj[i].geom[j],0,0,show_aabb);
			}
		}
	}

	if ( show_aabb )
	{
		// draw the bounding box for this geom
		dReal aabb[6];
		dGeomGetAABB (gheight,aabb);
		dVector3 bbpos;
		for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
		dVector3 bbsides;
		for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
		dMatrix3 RI;
		dRSetIdentity (RI);
		dsSetColorAlpha (1,0,0,0.5);
		dsDrawBox (bbpos,RI,bbsides);
	}
}
Beispiel #8
0
// copied from an OpenDE demo program
//todo:pass trimesh as argument to this function
void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb, Tmesh tm)
{
    int i;

    if (!g) return;

    if (dGeomIsSpace(g))
    {
        displaySpace((dSpaceID)g);
        return;
    }
    int type = dGeomGetClass (g);
    if (type == dBoxClass)
    {
        if (!pos) pos = dGeomGetPosition (g);
        if (!R) R = dGeomGetRotation (g);

        dVector3 sides;
        dGeomBoxGetLengths (g,sides);
        dsDrawBox (pos,R,sides);
    }
    else if (type == dSphereClass)
    {

        if (!pos) pos = dGeomGetPosition (g);
        if (!R) R = dGeomGetRotation (g);
        dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
    }
    else if (type == dCapsuleClass)
    {
        if (!pos) pos = dGeomGetPosition (g);
        if (!R) R = dGeomGetRotation (g);
        dReal radius,length;
        dGeomCapsuleGetParams (g,&radius,&length);
        dsDrawCapsule (pos,R,length,radius);
    }
    else if (type == dCylinderClass)
    {

        if (!pos) pos = dGeomGetPosition (g);
        if (!R) R = dGeomGetRotation (g);
        dReal radius,length;
        dGeomCylinderGetParams (g,&radius,&length);
        dsDrawCylinder (pos,R,length,radius);
    }

    else if (type == dTriMeshClass)
    {
        //dTriIndex* Indices = DISP.tmd[i].indices;

        const dReal* Pos = dGeomGetPosition(g);
        const dReal* Rot = dGeomGetRotation(g);

        for (int ii = 0; ii < (tm.indexSize/3); ii++)
        {

            const dReal v[9] = { // explicit conversion from float to dReal
                                 tm.vertices[tm.indices[ii * 3 + 0] * 3 + 0],
                                 tm.vertices[tm.indices[ii * 3 + 0] * 3 + 1],
                                 tm.vertices[tm.indices[ii * 3 + 0] * 3 + 2],
                                 tm.vertices[tm.indices[ii * 3 + 1] * 3 + 0],
                                 tm.vertices[tm.indices[ii * 3 + 1] * 3 + 1],
                                 tm.vertices[tm.indices[ii * 3 + 1] * 3 + 2],
                                 tm.vertices[tm.indices[ii * 3 + 2] * 3 + 0],
                                 tm.vertices[tm.indices[ii * 3 + 2] * 3 + 1],
                                 tm.vertices[tm.indices[ii * 3 + 2] * 3 + 2]
                               };
            dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
        }
        //std::cout<<"done once"<<std::endl;
    }
    else
        if (type == dRayClass)
        {
            dVector3 Origin, Direction;
            dGeomRayGet(g, Origin, Direction);

            dReal Length = dGeomRayGetLength(g);

            dVector3 End;
            End[0] = Origin[0] + (Direction[0] * Length);
            End[1] = Origin[1] + (Direction[1] * Length);
            End[2] = Origin[2] + (Direction[2] * Length);
            End[3] = Origin[3] + (Direction[3] * Length);
            double *ori = new double[3];
            double *end = new double[4];
            ori[0]=Origin[0];
            ori[1]=Origin[1];
            ori[2]=Origin[2];
            end[0]=End[0];
            end[1]=End[1];
            end[2]=End[2];
            end[3]=End[3];


            dsDrawLine(ori, end);
        }
        else
            show_aabb = 0;

    if (show_aabb)
    {
        // draw the bounding box for this geom
        dReal aabb[6];
        dGeomGetAABB (g,aabb);
        dVector3 bbpos;
        for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
        dVector3 bbsides;
        for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
        dMatrix3 RI;
        dRSetIdentity (RI);
        dsSetColorAlpha (1,0,0,0.5);
        dsDrawBox (bbpos,RI,bbsides);
    }
}