static void simLoop (int pause) { dsSetColor (0,0,2); dSpaceCollide (space,0,&nearCallback); if (!pause) dWorldQuickStep (world,0.05); 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); 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.8,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) dWorldQuickStep (world,0.02); if (write_world) { FILE *f = fopen ("state.dif","wt"); if (f) { dWorldExportDIF (world,f,"X"); fclose (f); } write_world = 0; } if (doFeedback) { if (fbnum>MAX_FEEDBACKNUM) printf("joint feedback buffer overflow!\n"); else { dVector3 sum = {0, 0, 0}; printf("\n"); for (int i=0; i<fbnum; i++) { dReal* f = feedbacks[i].first?feedbacks[i].fb.f1:feedbacks[i].fb.f2; printf("%f %f %f\n", f[0], f[1], f[2]); sum[0] += f[0]; sum[1] += f[1]; sum[2] += f[2]; } printf("Sum: %f %f %f\n", sum[0], sum[1], sum[2]); dMass m; dBodyGetMass(obj[selected].body, &m); printf("Object G=%f\n", GRAVITY*m.mass); } doFeedback = 0; fbnum = 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.8,0); } else { dsSetColor (1,1,0); } drawGeom (obj[i].geom[j],0,0,show_aabb); } } }
static void simLoop (int pause) { const dReal stepsize = 0.02; dsSetColor (0,0,2); dSpaceCollide (space,0,&nearCallback); if (!pause) { if (mov_type == 1) moveplat_1(stepsize); else moveplat_2(stepsize); dGeomSetPosition(platform, platpos[0], platpos[1], platpos[2]); updatecam(); dWorldQuickStep (world,stepsize); //dWorldStep (world,stepsize); } 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); dsSetColor (1,1,0); dsSetTexture (DS_WOOD); for (int i=0; i<num; i++) { for (int j=0; j < GPB; j++) { if (! dBodyIsEnabled (obj[i].body)) { dsSetColor (1,0.8,0); } else { dsSetColor (1,1,0); } drawGeom (obj[i].geom[j],0,0,show_aabb); } } dsSetColor (1,0,0); drawGeom (platform,0,0,show_aabb); //usleep(5000); }
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); }
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); } }