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;
}
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:
{}
}
}
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);
}
}
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);
}}}
}
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);
}
}
// 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);
}
}
