// simulation loop
static void simLoop (int pause)
{
const dReal *pos1,*R1,*pos2,*R2,*pos3,*R3;
//@a sphere
dsSetColor(1,0,0); // set color (red, green, blue�j a value is between 0 and 1
dsSetSphereQuality(3); // set sphere quality. 3 is enough
pos1 = dBodyGetPosition(sphere.body); // get a body position
R1 = dBodyGetRotation(sphere.body); // get a body rotation matrix
dsDrawSphere(pos1,R1,radius); // draw a sphere
// a cylinder
dsSetColorAlpha (0,1,0,1);
pos2 = dBodyGetPosition(cylinder.body);
R2 = dBodyGetRotation(cylinder.body);
dsDrawCylinder(pos2,R2,length,radius); // draw a cylinder
// a capsule
dsSetColorAlpha (1,1,1,1);
pos2 = dBodyGetPosition(capsule.body);
R2 = dBodyGetRotation(capsule.body);
dsDrawCapsule(pos2,R2,length,radius); // draw a capsule
// a box
dsSetColorAlpha (0,0,1,1);
pos3 = dBodyGetPosition(box.body);
R3 = dBodyGetRotation(box.body);
dsDrawBox(pos3,R3,sides); // draw a box
// a ray
dReal posA[3] = {0, 5, 0}, posB[3]= {0, 5, 1.9};
dsDrawLine(posA,posB); // draw a ray
}
void vmWishboneCar::drawLinkage(vm::WheelLoc loc)
{
// select wheel
vmWishbone *snow;
switch (loc) {
case vm::WheelLoc::FR:
snow= &frSuspension;
break;
case vm::WheelLoc::FL:
snow= &flSuspension;
break;
case vm::WheelLoc::RR:
snow= &rrSuspension;
break;
case vm::WheelLoc::RL:
snow= &rlSuspension;
break;
default:
break;
}
// setup linkage
const dReal *pos1,*R1,*pos2,*R2,*pos3,*R3;
const dReal *pos4,*R4,*pos5,*R5;
dsSetColorAlpha(0.0,0.0,1.0, 0.5);
pos1 = dBodyGetPosition(snow->uplink.body);
R1 = dBodyGetRotation(snow->uplink.body);
dsDrawCylinderD(pos1,R1,snow->uplink.width,snow->uplink.radius);
dsSetColor(0.0,1.0,0.0);
pos2 = dBodyGetPosition(snow->hlink.body);
R2 = dBodyGetRotation(snow->hlink.body);
dsDrawCylinderD(pos2,R2,snow->hlink.width,snow->hlink.radius);
dsSetColorAlpha(1.0,0.0,1.0, 0.5);
pos3 = dBodyGetPosition(snow->lowlink.body);
R3 = dBodyGetRotation(snow->lowlink.body);
dsDrawCylinderD(pos3,R3,snow->lowlink.width,snow->lowlink.radius);
// setup spring and damper
dsSetColorAlpha(0.1,0.7,1.0, 0.5);
pos4= dBodyGetPosition(snow->upstrut.body);
R4= dBodyGetRotation(snow->upstrut.body);
dsDrawCylinderD(pos4,R4,snow->upstrut.width,snow->upstrut.radius);
dsSetColorAlpha(1.0,0.7,1.0, 0.5);
pos5= dBodyGetPosition(snow->lowstrut.body);
R5= dBodyGetRotation(snow->lowstrut.body);
dsDrawCylinderD(pos5,R5,snow->lowstrut.width,snow->lowstrut.radius);
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!draw_geom){
return;
}
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
/*
// cylinder option not yet implemented
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
*/
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
static void simLoop (int pause)
{
dSpaceCollide (space,0,&nearCallback);
if (!pause)
{
dWorldQuickStep (world, 0.01); // 100 Hz
}
dJointGroupEmpty (contactgroup);
dsSetColorAlpha (1,1,0,0.5);
const dReal *CPos = dBodyGetPosition(cylbody);
const dReal *CRot = dBodyGetRotation(cylbody);
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
(
cpos,
crot,
CYLLENGTH,
CYLRADIUS
); // single precision
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,
SPHERERADIUS
); // single precision
}
//===========================================================================================
//! \brief 描画関数
void draw_world( void )
//===========================================================================================
{
int i;
dsSetColor (0,0.5,1);
dsSetTexture (DS_WOOD);
dReal rad, len;
dReal sides[4];
dBox *blink;
dCapsule *clink;
dsSetColorAlpha (1.0, 0.0, 0.0, 0.6);
i=0; blink=&LinkBase; blink->getLengths(sides); dsDrawBox (body[i].getPosition(),body[i].getRotation(),sides);
dsSetColorAlpha (0.0, 0.5, 1.0, 0.6);
i=1; clink=&Link1; clink->getParams(&rad, &len); dsDrawCappedCylinder (body[i].getPosition(),body[i].getRotation(),len,rad);
i=2; clink=&Link2; clink->getParams(&rad, &len); dsDrawCappedCylinder (body[i].getPosition(),body[i].getRotation(),len,rad);
}
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 DrawBox()
{
for(int i = 0; i < BOX_NUM; i++)
{
dsSetColorAlpha(0.3, 0.2, 0.1, 1.0);
dsSetTexture(DS_WOOD);
dsDrawBoxD(dBodyGetPosition(box[i].body),dBodyGetRotation(box[i].body), boxSize[i]);
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2);
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = 0;
contact[i].surface.mu = 50.0; // was: dInfinity
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
if (show_contacts)
{
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.12,0.12,0.12};
dsSetColorAlpha (0,0,1,0.5);
dsDrawBox (contact[i].geom.pos,RI,ss);
dReal *pos = contact[i].geom.pos;
dReal depth = contact[i].geom.depth;
dReal *norm = contact[i].geom.normal;
dReal endp[3] = {pos[0]+depth*norm[0], pos[1]+depth*norm[1], pos[2]+depth*norm[2]};
dsSetColorAlpha (1,1,1,1);
dsDrawLine (contact[i].geom.pos, endp);
}
}
}
}
IoObject *IoDrawStuff_dsSetColorAlpha(IoDrawStuff *self, IoObject *locals, IoMessage *m)
{
float red = IoMessage_locals_intArgAt_(m, locals, 0);
float green = IoMessage_locals_intArgAt_(m, locals, 1);
float blue = IoMessage_locals_intArgAt_(m, locals, 2);
float alpha = IoMessage_locals_intArgAt_(m, locals, 3);
dsSetColorAlpha(red, green, blue, alpha);
return self;
}
static void simLoop (int pause)
{
const dReal *pos1,*R1;
dsSetColorAlpha (1,1,1,1);
dSpaceCollide (space,0,&nearCallback);
dJointGroupEmpty (contactgroup);
dWorldStep(world,0.01);
pos1 = dBodyGetPosition(capsule);
R1 = dBodyGetRotation(capsule);
dsDrawCapsule(pos1,R1,length,radius); // draw a capsule
}
void vmWishboneCar::simDraw()
{
// draw suspension linkages
drawLinkage(vm::FR);
drawLinkage(vm::FL);
drawLinkage(vm::RR);
drawLinkage(vm::RL);
// draw wheels
dsSetColorAlpha(1.0,1.0,0.0,0.5);
dsDrawCylinderD(dBodyGetPosition(frWheel.body),dBodyGetRotation(frWheel.body),frWheel.length,frWheel.radius);
dsDrawCylinderD(dBodyGetPosition(flWheel.body),dBodyGetRotation(frWheel.body),flWheel.length,flWheel.radius);
dsDrawCylinderD(dBodyGetPosition(rrWheel.body),dBodyGetRotation(rrWheel.body),rrWheel.length,rrWheel.radius);
dsDrawCylinderD(dBodyGetPosition(rlWheel.body),dBodyGetRotation(rlWheel.body),rlWheel.length,rlWheel.radius);
// draw chassis
dsSetColorAlpha(1.0,0.0,0.0,0.5); // red
dsDrawBoxD(dBodyGetPosition(chassis.body),dBodyGetRotation(chassis.body)
,chassis.sides);
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
} else if (type == dConvexClass) {
//dVector3 sides={0.50,0.50,0.50};
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
}
/*
// cylinder option not yet implemented
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
*/
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
void draw_all_objects (dSpaceID space)
{
int i, j;
draw_all_objects_called = 1;
if (!graphical_test) return;
int n = dSpaceGetNumGeoms (space);
// draw all contact points
dsSetColor (0,1,1);
dSpaceCollide (space,0,&nearCallback);
// draw all rays
for (i=0; i<n; i++) {
dGeomID g = dSpaceGetGeom (space,i);
if (dGeomGetClass (g) == dRayClass) {
dsSetColor (1,1,1);
dVector3 origin,dir;
dGeomRayGet (g,origin,dir);
origin[2] += Z_OFFSET;
dReal length = dGeomRayGetLength (g);
for (j=0; j<3; j++) dir[j] = dir[j]*length + origin[j];
dsDrawLine (origin,dir);
dsSetColor (0,0,1);
dsDrawSphere (origin,dGeomGetRotation(g),0.01);
}
}
// draw all other objects
for (i=0; i<n; i++) {
dGeomID g = dSpaceGetGeom (space,i);
dVector3 pos;
if (dGeomGetClass (g) != dPlaneClass) {
memcpy (pos,dGeomGetPosition(g),sizeof(pos));
pos[2] += Z_OFFSET;
}
switch (dGeomGetClass (g)) {
case dSphereClass: {
dsSetColorAlpha (1,0,0,0.8);
dReal radius = dGeomSphereGetRadius (g);
dsDrawSphere (pos,dGeomGetRotation(g),radius);
break;
}
case dBoxClass: {
dsSetColorAlpha (1,1,0,0.8);
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,dGeomGetRotation(g),sides);
break;
}
case dCapsuleClass: {
dsSetColorAlpha (0,1,0,0.8);
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,dGeomGetRotation(g),length,radius);
break;
}
case dCylinderClass: {
dsSetColorAlpha (0,1,0,0.8);
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,dGeomGetRotation(g),length,radius);
break;
}
case dPlaneClass: {
dVector4 n;
dMatrix3 R,sides;
dVector3 pos2;
dGeomPlaneGetParams (g,n);
dRFromZAxis (R,n[0],n[1],n[2]);
for (j=0; j<3; j++) pos[j] = n[j]*n[3];
pos[2] += Z_OFFSET;
sides[0] = 2;
sides[1] = 2;
sides[2] = 0.001;
dsSetColor (1,0,1);
for (j=0; j<3; j++) pos2[j] = pos[j] + 0.1*n[j];
dsDrawLine (pos,pos2);
dsSetColorAlpha (1,0,1,0.8);
dsDrawBox (pos,R,sides);
break;
}
}
}
}
void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
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
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);
}
}
// 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);
}
}
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);
}
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
int i;
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
//<---- Convex Object
else if (type == dConvexClass)
{
#if 0
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
#else
dsDrawConvex(pos,R,
Sphere_planes,
Sphere_planecount,
Sphere_points,
Sphere_pointcount,
Sphere_polygons);
#endif
}
//----> Convex Object
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
if (show_body) {
dBodyID body = dGeomGetBody(g);
if (body) {
const dReal *bodypos = dBodyGetPosition (body);
const dReal *bodyr = dBodyGetRotation (body);
dReal bodySides[3] = { 0.1, 0.1, 0.1 };
dsSetColorAlpha(0,1,0,1);
dsDrawBox(bodypos,bodyr,bodySides);
}
}
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);
}
}
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);
}
}
}
// copied from an OpenDE demo program
void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
int i;
if (g == nullptr) return;
if (dGeomIsSpace(g) != 0)
{
displaySpace((dSpaceID)g);
return;
}
int type = dGeomGetClass (g);
if (type == dBoxClass)
{
if (pos == nullptr) pos = dGeomGetPosition (g);
if (R == nullptr) R = dGeomGetRotation (g);
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass)
{
if (pos == nullptr) pos = dGeomGetPosition (g);
if (R == nullptr) R = dGeomGetRotation (g);
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass)
{
if (pos == nullptr) pos = dGeomGetPosition (g);
if (R == nullptr) R = dGeomGetRotation (g);
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
else if (type == dCylinderClass)
{
if (pos == nullptr) pos = dGeomGetPosition (g);
if (R == nullptr) R = dGeomGetRotation (g);
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else if (type == dGeomTransformClass)
{
if (pos == nullptr) pos = dGeomGetPosition (g);
if (R == nullptr) R = dGeomGetRotation (g);
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
else
show_aabb = 0;
if (show_aabb != 0)
{
// 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);
}
}
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
}
}
