void WorldPhysics::drawGL(GLuint floortex, GLuint woodtex, GLuint bulldozertex) {
glBindTexture(GL_TEXTURE_2D,woodtex);
for (int i=0;i<items.size();i++) {
items[i]->render();
}
glBindTexture(GL_TEXTURE_2D,bulldozertex);
drawBulldozer();
const dReal* p = dGeomGetPosition( gheight );
const dReal* r = 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 )
{
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( i, j );
a[ 2 ] = oz + ( j ) * HFIELD_DSAMP;
b[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
b[ 1 ] = heightfield_callback( i + 1, j );
b[ 2 ] = oz + ( j ) * HFIELD_DSAMP;
c[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
c[ 1 ] = heightfield_callback( i, j + 1 );
c[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;
d[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
d[ 1 ] = heightfield_callback( i + 1, j + 1 );
d[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;
if (
(a[0]>-50 || b[0]>-50 || c[0]>-50 || d[0]>-50) &&
(a[0]<50 || b[0]<50 || c[0]<50 || d[0]<50) &&
(a[2]>-50 || b[2]>-50 || c[2]>-50 || d[2]>-50) &&
(a[2]<50 || b[2]<50 || c[2]<50 || d[2]<50)
) {
glPushMatrix();
float m[16];
m[ 0] = r[ 0];m[ 1] = r[ 4];m[ 2] = r[ 8];m[ 3] = 0;
m[ 4] = r[ 1];m[ 5] = r[ 5];m[ 6] = r[ 9];m[ 7] = 0;
m[ 8] = r[ 2];m[ 9] = r[ 6];m[10] = r[10];m[11] = 0;
m[12] = p[ 0];m[13] = p[ 1];m[14] = p[ 2];m[15] = 1;
glMultMatrixf(m);
///////////////
#ifdef o
glBegin(GL_LINES);
glColor3f(0.0, 1.0, 0.0);
glVertex3f(a[0],a[1],a[2]);
glVertex3f(a[0]-normal(0,a[0]-b[0],a[1]-b[1],a[2]-b[2],a[0]-d[0],a[1]-d[1],a[2]-d[2]),
a[1]-normal(1,a[0]-b[0],a[1]-b[1],a[2]-b[2],a[0]-d[0],a[1]-d[1],a[2]-d[2]),
a[2]-normal(2,a[0]-b[0],a[1]-b[1],a[2]-b[2],a[0]-d[0],a[1]-d[1],a[2]-d[2])
);
glEnd();
///////////////
///////////////
glBegin(GL_LINES);
glColor3f(1.0, 0.0, 0.0);
glVertex3f(a[0],a[1],a[2]);
glVertex3f(a[0]+normal(0,a[0]-c[0],a[1]-c[1],a[2]-c[2],a[0]-d[0],a[1]-d[1],a[2]-d[2]),
a[1]+normal(1,a[0]-c[0],a[1]-c[1],a[2]-c[2],a[0]-d[0],a[1]-d[1],a[2]-d[2]),
a[2]+normal(2,a[0]-c[0],a[1]-c[1],a[2]-c[2],a[0]-d[0],a[1]-d[1],a[2]-d[2])
);
glEnd();
///////////////
#endif
glColor3f(0.5, 0.5, 0.5);
glBindTexture(GL_TEXTURE_2D,floortex);
glBegin(GL_TRIANGLES);
const GLfloat amb[]={0.3,0.3,0.3,1.0};
const GLfloat dif[]={0.7,0.7,0.7,1.0};
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT,amb);
glMaterialfv(GL_FRONT_AND_BACK,GL_DIFFUSE,dif);
glNormal3f(-normal(0,a[0]-b[0],a[1]-b[1],a[2]-b[2],a[0]-d[0],a[1]-d[1],a[2]-d[2]),
-normal(1,a[0]-b[0],a[1]-b[1],a[2]-b[2],a[0]-d[0],a[1]-d[1],a[2]-d[2]),
-normal(2,a[0]-b[0],a[1]-b[1],a[2]-b[2],a[0]-d[0],a[1]-d[1],a[2]-d[2])
);
//glNormal3f(0,0,1);
//glColor3f(0.5, 0.5, 0.5);
glTexCoord2d(0, 0);
glVertex3f(a[0],a[1],a[2]);
//glVertex3f(b[0], b[1], b[2]);
glTexCoord2d(1,0);
glVertex3f(b[0],b[1],b[2]);
//glVertex3f(c[0], c[1], c[2]);
glTexCoord2d(1, 1);
glVertex3f(d[0],d[1],d[2]);
//glVertex3f(a[0], a[1], a[2]);
//glTexCoord2d(0, 1);
//glVertex3f(c[0],c[1],c[2]);
//glVertex3f(d[0], d[1], d[2]);
glNormal3f(normal(0,a[0]-c[0],a[1]-c[1],a[2]-c[2],a[0]-d[0],a[1]-d[1],a[2]-d[2]),
normal(1,a[0]-c[0],a[1]-c[1],a[2]-c[2],a[0]-d[0],a[1]-d[1],a[2]-d[2]),
normal(2,a[0]-c[0],a[1]-c[1],a[2]-c[2],a[0]-d[0],a[1]-d[1],a[2]-d[2])
);
glTexCoord2d(0, 0);
glVertex3f(a[0],a[1],a[2]);
//glVertex3f(b[0], b[1], b[2]);
glTexCoord2d(0,1);
glVertex3f(c[0],c[1],c[2]);
//glVertex3f(c[0], c[1], c[2]);
glTexCoord2d(1, 1);
glVertex3f(d[0],d[1],d[2]);
glEnd();
glPopMatrix();
//dsDrawTriangle( pReal, RReal, a, c, b, 1 );
//dsDrawTriangle( pReal, RReal, b, c, d, 1 );
}
}
}
}
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);
}
}
