static void simLoop (int pause)
{
int i;
if (!pause) {
static double angle = 0;
angle += 0.05;
dBodyAddForce (body[NUM-1],0,0,1.5*(sin(angle)+1.0));
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
/* remove all contact joints */
dJointGroupEmpty (contactgroup);
}
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (i=0; i<NUM; i++) dsDrawSphere (dBodyGetPosition(body[i]),
dBodyGetRotation(body[i]),RADIUS);
}
static void simLoop (int pause)
{
if (!pause) {
static double angle = 0;
angle += 0.05;
body[NUM-1].addForce (0,0,1.5*(sin(angle)+1.0));
space.collide (0,&nearCallback);
world.step (0.05);
// remove all contact joints
contactgroup.empty();
}
dReal sides[3] = {SIDE,SIDE,SIDE};
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<NUM; i++)
dsDrawBox (body[i].getPosition(),body[i].getRotation(),sides);
}
static void simLoop( int pause )
{
dsSetColor( 0,0,2 );
dSpaceCollide( space,0,&nearCallback );
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 );
}
drawGeom( obj[i].geom[j],0,0,show_aabb );
}
}
}
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
int color_changed = 0;
if (i==selected) {
dsSetColor (0,0.7,1);
color_changed = 1;
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
color_changed = 1;
}
for (int j=0; j < GPB; j++) drawGeom (obj[i].geom[j],0,0);
if (color_changed) dsSetColor (1,1,0);
}
{for (int i = 0; i < RayCount; i++){
dVector3 Origin, Direction;
dGeomRayGet(Rays[i], Origin, Direction);
dReal Length = dGeomRayGetLength(Rays[i]);
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);
}}
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
space->collide(0,&nearCallback);
if (!pause)
//world->quickStep(0.02);
world->step(0.02);
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (*world,f,"X");
fclose (f);
}
write_world = false;
}
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetTexture (DS_WOOD);
dsSetColor (1,0.5f,0);
dsDrawCylinder(top1->getPosition(),
top1->getRotation(),
toplength, topradius);
dsDrawCapsule(top1->getPosition(),
top1->getRotation(),
pinlength, pinradius);
dsSetColor (0.5f,1,0);
dsDrawCylinder(top2->getPosition(),
top2->getRotation(),
toplength, topradius);
dsDrawCapsule(top2->getPosition(),
top2->getRotation(),
pinlength, pinradius);
}
void ode::drawLines()
{
dsSetTexture (DS_WOOD);
//dWorldQuickStep (world,0.05);
dsSetColor(1,0,0);
//const dReal *temp1=dGeomGetPosition(lineStrips[0]);
for (int i=0; i< contourX.size(); i++)
{
float tempX[3];
tempX[0]=contourX[i][0];
tempX[0]=contourX[i][1];
tempX[0]=contourX[i][2];
float tempY[3];
tempY[0]=contourY[i][0];
tempY[0]=contourY[i][1];
tempY[0]=contourY[i][2];
//dsDrawLine(tempX,tempY);
}
flag=false;
}
void desenharRobos()
{
dsSetTexture(DS_WOOD);
for(int j=0; j < ROBOTS; j++)
{
dReal sides[2][3] = {{LENGTH,WIDTH,HEIGHT}, {LENGTH/2,WIDTH,HEIGHT}};
for(int i=0; i < 2; i++)
{
//desenhar chassi
if (j<3)
dsSetColor(0.5,1,0);
else
dsSetColor(0.5,0,1);
//dsSetColor(0,0.5,1);
dsDrawBox(dBodyGetPosition(robot[j].body[i]), dBodyGetRotation(robot[j].body[i]), sides[i]);
//desenhar roda
dsSetColor (0,0,0);
//dsSetColor(1,0.5,1);
dsDrawCylinder(dBodyGetPosition(robot[j].wheel[i]),dBodyGetRotation(robot[j].wheel[i]),wTHICK,RADIUS);
}
}
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
dAASSERT(terrainY);
dAASSERT(terrainZ);
dsSetColor (0,1,0);
dsDrawTerrainY(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainY),dGeomGetPosition(terrainY));
dsDrawTerrainZ(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainZ),dGeomGetPosition(terrainZ));
if (show_aabb)
{
dReal aabb[6];
dGeomGetAABB (terrainY,aabb);
dVector3 bbpos;
int i;
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);
dGeomGetAABB (terrainZ,aabb);
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dsDrawBox (bbpos,RI,bbsides);
}
dsSetColor (1,1,0);
// 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);
}
}
}
// simulation loop
static void simLoop (int pause)
{
const dReal *rot;
dVector3 ax;
dReal l=0;
switch (joint->getType() )
{
case dJointTypeSlider :
( (dSliderJoint *) joint)->getAxis (ax);
l = ( (dSliderJoint *) joint)->getPosition();
break;
case dJointTypePiston :
( (dPistonJoint *) joint)->getAxis (ax);
l = ( (dPistonJoint *) joint)->getPosition();
break;
default:
{} // keep the compiler happy
}
if (!pause)
{
double simstep = 0.01; // 1ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf (dt/simstep);
if (!nrofsteps)
nrofsteps = 1;
for (int i=0; i<nrofsteps && !pause; i++)
{
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
update();
dReal radius, length;
dsSetTexture (DS_WOOD);
drawBox (geom[BODY2], 1,1,0);
drawBox (geom[RECT], 0,0,1);
if ( geom[BODY1] )
{
const dReal *pos = dGeomGetPosition (geom[BODY1]);
rot = dGeomGetRotation (geom[BODY1]);
dsSetColor (0,0,1);
dGeomCapsuleGetParams (geom[BODY1], &radius, &length);
dsDrawCapsule (pos, rot, length, radius);
}
drawBox (geom[OBS], 1,0,1);
// Draw the prismatic axis
if ( geom[BODY1] )
{
const dReal *pos = dGeomGetPosition (geom[BODY1]);
rot = dGeomGetRotation (geom[BODY2]);
dVector3 p;
p[X] = pos[X] - l*ax[X];
p[Y] = pos[Y] - l*ax[Y];
p[Z] = pos[Z] - l*ax[Z];
dsSetColor (1,0,0);
dsDrawCylinder (p, rot, 3.75, 1.05*AXIS_RADIUS);
}
if (joint->getType() == dJointTypePiston )
{
dVector3 anchor;
dJointGetPistonAnchor(joint->id(), anchor);
// Draw the rotoide axis
rot = dGeomGetRotation (geom[BODY2]);
dsSetColor (1,0.5,0);
dsDrawCylinder (anchor, rot, 4, AXIS_RADIUS);
dsSetColor (0,1,1);
rot = dGeomGetRotation (geom[BODY1]);
dsDrawSphere (anchor, rot, 1.5*RADIUS);
}
}
}
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 float *p = Vertices[Indices[i * 3 + 0]];
const dVector3 v0 = { p[0], p[1], p[2] };
p = Vertices[Indices[i * 3 + 1]];
const dVector3 v1 = { p[0], p[1], p[2] };
p = Vertices[Indices[i * 3 + 2]];
const dVector3 v2 = { p[0], p[1], p[2] };
dsDrawTriangle(Pos, Rot, v0, v1, 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);
}
}
static void simLoop (int pause)
{
int i, j;
dsSetTexture (DS_WOOD);
if (!pause) {
#ifdef BOX
dBodyAddForce(body[bodies-1],lspeed,0,0);
#endif
for (j = 0; j < joints; j++)
{
dReal curturn = dJointGetHinge2Angle1 (joint[j]);
//dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamFMax,dInfinity);
dJointSetHinge2Param(joint[j],dParamVel2,speed);
dJointSetHinge2Param(joint[j],dParamFMax2,FMAX);
dBodyEnable(dJointGetBody(joint[j],0));
dBodyEnable(dJointGetBody(joint[j],1));
}
if (doFast)
{
dSpaceCollide (space,0,&nearCallback);
#if defined(QUICKSTEP)
dWorldQuickStep (world,0.05);
#elif defined(STEPFAST)
dWorldStepFast1 (world,0.05,ITERS);
#endif
dJointGroupEmpty (contactgroup);
}
else
{
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
dJointGroupEmpty (contactgroup);
}
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
{
bool disable = true;
const dReal *lvel = dBodyGetLinearVel(b);
dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2];
if (lspeed > DISABLE_THRESHOLD)
disable = false;
const dReal *avel = dBodyGetAngularVel(b);
dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2];
if (aspeed > DISABLE_THRESHOLD)
disable = false;
if (disable)
wb_stepsdis[i]++;
else
wb_stepsdis[i] = 0;
if (wb_stepsdis[i] > DISABLE_STEPS)
{
dBodyDisable(b);
dsSetColor(0.5,0.5,1);
}
else
dsSetColor(1,1,1);
}
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
else
{
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
dsSetColor(1,1,1);
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
dsSetColor (0,1,1);
dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
for (i = 0; i < boxes; i++)
dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides);
dsSetColor (1,1,1);
for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]),
dGeomGetRotation(sphere[i]),RADIUS);
// draw the cannon
dsSetColor (1,1,0);
dMatrix3 R2,R3,R4;
dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
dMultiply0 (R4,R2,R3,3,3,3);
dReal cpos[3] = {CANNON_X,CANNON_Y,1};
dReal csides[3] = {2,2,2};
dsDrawBox (cpos,R2,csides);
for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2];
dsDrawCylinder (cpos,R4,3,0.5);
// draw the cannon ball
dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body),
CANNON_BALL_RADIUS);
}
void drawRobot_Nleg()
{
#ifndef NOVIZ
if(visualize){
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
dReal r,length;
dVector3 sides;
// Drawing of body
// Drawing of body
dsSetTexture(1);
dsSetColor(2.5,2.5,2.5);
dGeomBoxGetLengths(torso[segment].geom,sides);
dsDrawBox(dBodyGetPosition(torso[segment].body),
dBodyGetRotation(torso[segment].body),sides);
// Drawing oflegs
dsSetTexture(0);
dsSetColor(1.3,0.0,0.0);
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++ ) {
dGeomCapsuleGetParams(leg[segment][i][j].geom, &r,&length);
if (j== 0) dsDrawCapsule(dBodyGetPosition(leg[segment][i][j].body),
dBodyGetRotation(leg[segment][i][j].body),0.5*length,1.2*r);
else dsDrawCapsule(dBodyGetPosition(leg[segment][i][j].body),
dBodyGetRotation(leg[segment][i][j].body),length,r);
}
}
}
// int middleBodySegment = BODY_SEGMENTS/2;
// cerr <<dBodyGetPosition(torso.body)[0]<<" " <<dBodyGetPosition(torso.body)[1]<< " " << dBodyGetPosition(torso.body)[2]<<endl;
//FROM SIDE
// hpr[0] = 90.0f;
// hpr[1] = 0.0f;//-1.2f;
// hpr[2] = 0.0f; // point of view[m]
// xyz[0] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[0];
// xyz[1] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[1]-ceil(log2(BODY_SEGMENTS));
// xyz[2] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[2]+0.2f; // gaze[°]
//FROM BACK
// hpr[0] = 1.0f;
// hpr[1] = 0.0f;//-1.2f;
// hpr[2] = 0.0f; // point of view[m]
// xyz[0] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[0]-ceil(log2(BODY_SEGMENTS));
// xyz[1] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[1];
// xyz[2] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[2]+0.2f; // gaze[°]
//FROM BELOW and BACK
hpr[0] = 1.0f;
hpr[1] = -1.2f;
hpr[2] = 0.5f; // point of view[m]
xyz[0] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[0]-BODY_SEGMENTS/2.0f;
xyz[1] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[1];
xyz[2] = 0.1; // gaze[°]
dsSetViewpoint(xyz,hpr);
}
#endif
}
/*** Drawing of robot ***/
void drawRobot()
{
#ifndef NOVIZ
if(visualize){
dReal r,length;
dVector3 sides;
// Drawing of body
dsSetTexture(1);
dsSetColor(2.5,2.5,2.5);
dGeomBoxGetLengths(torso[0].geom,sides);
dsDrawBox(dBodyGetPosition(torso[0].body),
dBodyGetRotation(torso[0].body),sides);
// Drawing oflegs
dsSetTexture(0);
dsSetColor(1.3,0.0,0.0);
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++ ) {
dGeomCapsuleGetParams(leg[0][i][j].geom, &r,&length);
if (j== 0) dsDrawCapsule(dBodyGetPosition(leg[0][i][j].body),
dBodyGetRotation(leg[0][i][j].body),0.5*length,1.2*r);
else dsDrawCapsule(dBodyGetPosition(leg[0][i][j].body),
dBodyGetRotation(leg[0][i][j].body),length,r);
}
}
// hpr[0] = 0.0f;
// hpr[1] = -20.0f;
// hpr[2] = 0.0f; // point of view[m]
// //cerr <<dBodyGetPosition(torso[0].body)[0]<<" " <<dBodyGetPosition(torso[0].body)[1]<< " " << dBodyGetPosition(torso[0].body)[2]<<endl;
// xyz[0] = dBodyGetPosition(torso[0].body)[0]-.75f;
// xyz[1] = dBodyGetPosition(torso[0].body)[1];
// xyz[2] = dBodyGetPosition(torso[0].body)[2]+0.05f; // gaze[°]
hpr[0] = 1.0f;
hpr[1] = -1.2f;
hpr[2] = 0.5f; // point of view[m]
// hpr[0] = dBodyGetRotation(torso[0].body)[0];
// hpr[1] = dBodyGetRotation(torso[0].body)[1];
// hpr[2] = dBodyGetRotation(torso[0].body)[2];
//cerr <<dBodyGetPosition(torso[0].body)[0]<<" " <<dBodyGetPosition(torso[0].body)[1]<< " " << dBodyGetPosition(torso[0].body)[2]<<endl;
xyz[0] = dBodyGetPosition(torso[0].body)[0]-.75f;
xyz[1] = dBodyGetPosition(torso[0].body)[1];
xyz[2] = 0.1; //dBodyGetPosition(torso[0].body)[2]-0.15f; // gaze[°]
//overrides to look from the other side of the robot
//hpr[0] = -180.0f; //slides view to the right
//xyz[0] = dBodyGetPosition(torso[0].body)[0]+.75f;
// hpr[1] = -30.2f; //slides down/up
// hpr[2] = 20.5f; // point of view[m]
//different override for other corner
xyz[0] = dBodyGetPosition(torso[0].body)[0]+.75f;
xyz[1] = dBodyGetPosition(torso[0].body)[1]-.50;
hpr[0] = -220.0f; //slides view to the right
dsSetViewpoint(xyz,hpr);
}
#endif
}
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);
}
}
// simulation loop
static void simLoop (int pause)
{
static bool todo = false;
if ( todo ) { // DEBUG
static int cnt = 0;
++cnt;
if (cnt == 5)
command ( 'q' );
if (cnt == 10)
dsStop();
}
if (!pause) {
double simstep = 0.01; // 10ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf (dt/simstep);
if (!nrofsteps)
nrofsteps = 1;
for (int i=0; i<nrofsteps && !pause; i++) {
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
update();
dReal radius, length;
dsSetTexture (DS_WOOD);
drawBox (geom[W], 1,1,0);
drawBox (geom[EXT], 0,1,0);
dVector3 anchorPos;
dReal ang1 = 0;
dReal ang2 = 0;
dVector3 axisP, axisR1, axisR2;
if ( dJointTypePU == type ) {
dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
ang1 = pu->getAngle1();
ang2 = pu->getAngle2();
pu->getAxis1 (axisR1);
pu->getAxis2 (axisR2);
pu->getAxisP (axisP);
dJointGetPUAnchor (pu->id(), anchorPos);
}
else if ( dJointTypePR == type ) {
dPRJoint *pr = dynamic_cast<dPRJoint *> (joint);
pr->getAxis1 (axisP);
pr->getAxis2 (axisR1);
dJointGetPRAnchor (pr->id(), anchorPos);
}
// Draw the axisR
if ( geom[INT] ) {
dsSetColor (1,0,1);
dVector3 l;
dGeomBoxGetLengths (geom[INT], l);
const dReal *rotBox = dGeomGetRotation (geom[W]);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i];
dsDrawBox (pos, rotBox, l);
}
dsSetTexture (DS_CHECKERED);
if ( geom[AXIS1] ) {
dQuaternion q, qAng;
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dGeomGetQuaternion (geom[AXIS1], q);
dQuaternion qq;
dQMultiply1 (qq, qAng, q);
dMatrix3 R;
dQtoR (qq,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS1]), &radius, &length);
dsSetColor (1,0,0);
dsDrawCylinder (anchorPos, R, length, radius);
}
if ( dJointTypePU == type && geom[AXIS2] ) {
//dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
dQuaternion q, qAng, qq, qq1;
dGeomGetQuaternion (geom[AXIS2], q);
dQFromAxisAndAngle (qAng, 0, 1, 0, ang2);
dQMultiply1 (qq, qAng, q);
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dQMultiply1 (qq1, qAng, qq);
dMatrix3 R;
dQtoR (qq1,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS2]), &radius, &length);
dsSetColor (0,0,1);
dsDrawCylinder (anchorPos, R, length, radius);
}
dsSetTexture (DS_WOOD);
// Draw the anchor
if ( geom[ANCHOR] ) {
dsSetColor (1,1,1);
dVector3 l;
dGeomBoxGetLengths (geom[ANCHOR], l);
const dReal *rotBox = dGeomGetRotation (geom[D]);
const dReal *posBox = dGeomGetPosition (geom[D]);
dVector3 e;
for (int i=0; i<3; ++i)
e[i] = posBox[i] - anchorPos[i];
dNormalize3 (e);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i];
dsDrawBox (pos, rotBox, l);
}
drawBox (geom[D], 1,1,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
}
}
IoObject *IoDrawStuff_dsSetTexture(IoDrawStuff *self, IoObject *locals, IoMessage *m)
{
int texture_number = IoMessage_locals_intArgAt_(m, locals, 0);
dsSetTexture(texture_number);
return self;
}
void nearCallback(void *, dGeomID a, dGeomID b)
{
const unsigned max_contacts = 8;
dContact contacts[max_contacts];
if (!dGeomGetBody(a) && !dGeomGetBody(b))
return; // don't handle static geom collisions
int n = dCollide(a, b, max_contacts, &contacts[0].geom, sizeof(dContact));
//clog << "got " << n << " contacts" << endl;
/* Simple contact merging:
* If we have contacts that are too close with the same normal, keep only
* the one with maximum depth.
* The epsilon that defines what "too close" means can be a heuristic.
*/
int new_n = 0;
dReal epsilon = 1e-1; // default
/* If we know one of the geoms is a sphere, we can base the epsilon on the
* sphere's radius.
*/
dGeomID s = 0;
if ((dGeomGetClass(a) == dSphereClass && (s = a)) ||
(dGeomGetClass(b) == dSphereClass && (s = b))) {
epsilon = dGeomSphereGetRadius(s) * 0.3;
}
for (int i=0; i<n; ++i) {
// this block draws the contact points before merging, in red
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(1, 0, 0);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.008);
// let's offset the line a bit to avoid drawing overlap issues
float xyzf[3], hprf[3];
dsGetViewpoint(xyzf, hprf);
dVector3 xyz = {dReal(xyzf[0]), dReal(xyzf[1]), dReal(xyzf[2])};
dVector3 v;
dSubtractVectors3(v, contacts[i].geom.pos, xyz);
dVector3 c;
dCalcVectorCross3(c, v, contacts[i].geom.pos);
dSafeNormalize3(c);
dVector3 pos1;
dAddScaledVectors3(pos1, contacts[i].geom.pos, c, 1, 0.005);
dVector3 pos2;
dAddScaledVectors3(pos2, pos1, contacts[i].geom.normal, 1, 0.05);
dsDrawLine(pos1, pos2);
// end of contacts drawing code
int closest_point = i;
for (int j=0; j<new_n; ++j) {
dReal alignment = dCalcVectorDot3(contacts[i].geom.normal, contacts[j].geom.normal);
if (alignment > 0.99 // about 8 degrees of difference
&&
dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) {
// they are too close
closest_point = j;
//clog << "found close points: " << j << " and " << i << endl;
break;
}
}
if (closest_point != i) {
// we discard one of the points
if (contacts[i].geom.depth > contacts[closest_point].geom.depth)
// the new point is deeper, copy it over closest_point
contacts[closest_point] = contacts[i];
} else
contacts[new_n++] = contacts[i]; // the point is preserved
}
//clog << "reduced from " << n << " to " << new_n << endl;
n = new_n;
for (int i=0; i<n; ++i) {
contacts[i].surface.mode = dContactBounce | dContactApprox1 | dContactSoftERP;
contacts[i].surface.mu = 10;
contacts[i].surface.bounce = 0.2;
contacts[i].surface.bounce_vel = 0;
contacts[i].surface.soft_erp = 1e-3;
//clog << "depth: " << contacts[i].geom.depth << endl;
dJointID contact = dJointCreateContact(world, contact_group, &contacts[i]);
dJointAttach(contact, dGeomGetBody(a), dGeomGetBody(b));
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(0, 0, 1);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.01);
dsSetColor(0, 1, 0);
dVector3 pos2;
dAddScaledVectors3(pos2, contacts[i].geom.pos, contacts[i].geom.normal, 1, 0.1);
dsDrawLine(contacts[i].geom.pos, pos2);
}
//clog << "----" << endl;
}
static void drawGeom(dGeomID geomID)
{
// printf("1%lu", (uint64_t)geomID);
int gclass = dGeomGetClass(geomID);
// printf("2\n");
const dReal *pos = NULL;
const dReal *rot = NULL;
bool canDrawJoints = false;
ODEUserObject* userObj = (ODEUserObject*)dGeomGetData(geomID);
if (nullptr != userObj) {
dsSetColorAlpha(1, 1, 1, 1);
// printf("%f %f %f %f\n", userObj->colorVec[0], userObj->colorVec[1], userObj->colorVec[2], userObj->colorVec[3]);
dsSetTexture (userObj->textureNum);
dsSetColorAlpha(userObj->m_colorVec[0], userObj->m_colorVec[1], userObj->m_colorVec[2], userObj->m_colorVec[3]);
} else {
dsSetColorAlpha(1, 1, 0, 1);
dsSetTexture (DS_WOOD);
}
switch (gclass) {
case dSphereClass:
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dsDrawSphere(pos, rot, dGeomSphereGetRadius(geomID));
}
canDrawJoints = true;
break;
case dBoxClass:
{
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dVector3 lengths;
dGeomBoxGetLengths(geomID, lengths);
dsDrawBox(pos, rot, lengths);
}
canDrawJoints = true;
break;
}
case dCylinderClass:
{
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dReal length;
dReal radius;
dGeomCylinderGetParams(geomID, &radius, &length);
dsDrawCylinder(pos, rot, length, radius);
}
canDrawJoints = true;
break;
}
default:
break;
}
// printf("class: %d\n", gclass);
if (canDrawJoints) {
#ifdef DRAW_JOINTS_TOO
dBodyID body = dGeomGetBody(geomID);
int numJoints = dBodyGetNumJoints(body);
for (int i = 0; i < numJoints; ++i)
{
dJointID joint = dBodyGetJoint(body, i);
int jointClass = dJointGetType(joint);
switch (jointClass)
{
case dJointTypeHinge:
{
dVector3 a11;
dBodyID body1 = dJointGetBody(joint, 0);
dBodyID body2 = dJointGetBody(joint, 1);
if (body1 && body2) {
const dReal* bodyPos1 = dBodyGetPosition(body1);
const dReal* bodyPos2 = dBodyGetPosition(body2);
dJointGetHingeAnchor(joint, a11);
dsSetColor(1, 0, 0);
dsDrawLine(a11, bodyPos1);
dsDrawLine(a11, bodyPos2);
dsSetColor(0, 1, 0);
dsDrawLine(bodyPos1, bodyPos2);
}
}
}
}
#endif
}
}
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);
}}}
}
void simLoop (int pause)
{
int contactcount;
const dReal ss[3] = {0.02,0.02,0.02};
dContactGeom contacts[8];
if(geoms==convex)
contactcount = dCollideConvexConvex(geoms[0],geoms[1],8,contacts,sizeof(dContactGeom));
else
contactcount = dCollideBoxBox(geoms[0],geoms[1],8,contacts,sizeof(dContactGeom));
//fprintf(stdout,"Contact Count %d\n",contactcount);
const dReal* pos;
const dReal* R;
dsSetTexture (DS_WOOD);
pos = dGeomGetPosition (geoms[0]);
R = dGeomGetRotation (geoms[0]);
dsSetColor (0.6f,0.6f,1);
dsSetDrawMode(drawmode);
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
dsSetDrawMode(DS_POLYFILL);
pos = dGeomGetPosition (geoms[1]);
R = dGeomGetRotation (geoms[1]);
dsSetColor (0.4f,1,1);
dsSetDrawMode(drawmode);
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
dsSetDrawMode(DS_POLYFILL);
/*if (show_contacts) */
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColor (1.0f,0,0);
for(int i=0;i<contactcount;++i)
{
if(DumpInfo)
{
//DumpInfo=false;
fprintf(stdout,"Contact %d Normal %f,%f,%f Depth %f Pos %f %f %f ",
i,
contacts[i].normal[0],
contacts[i].normal[1],
contacts[i].normal[2],
contacts[i].depth,
contacts[i].pos[0],
contacts[i].pos[1],
contacts[i].pos[2]);
if(contacts[i].g1==geoms[0])
{
fprintf(stdout,"Geoms 1 2\n");
}
else
{
fprintf(stdout,"Geoms 2 1\n");
}
}
dsDrawBox (contacts[i].pos,RI,ss);
}
if(DumpInfo)
DumpInfo=false;
}
