IoObject *IoDrawStuff_dsSetDrawMode(IoDrawStuff *self, IoObject *locals, IoMessage *m)
{
int mode;
mode = IoMessage_locals_intArgAt_(m, locals, 0);
dsSetDrawMode(mode);
return self;
}
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:
{}
}
}
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;
}
