//Auxiliary function to print info on a space void OdeInit::printInfoOnSpace(dSpaceID my_space,const std::string & my_space_name) { int num_geoms = 0; dGeomID geom_temp; int geom_class_temp = 0; dReal aabb[6]; dBodyID body_temp; num_geoms = dSpaceGetNumGeoms(my_space); printf("\nSpace: %s: ID: %p. sublevel: %d, nr. geoms: %d. \n",my_space_name.c_str(),my_space,dSpaceGetSublevel(my_space),num_geoms); for (int i=0;i<=(num_geoms-1);i++){ geom_temp = dSpaceGetGeom (my_space, i); geom_class_temp = dGeomGetClass(geom_temp); printGeomClassAndNr(geom_class_temp,i); if (!dGeomIsSpace(geom_temp)){ if (dGeomGetBody(geom_temp)!=NULL){ // i.e. a placeable geom printf(" ID: %p, Coordinates:",geom_temp); ICubSim::printPositionOfGeom(geom_temp); dGeomGetAABB(geom_temp,aabb); printf(" Bounding box coordinates: %f-%f,%f-%f,%f-%f\n:",aabb[0],aabb[1],aabb[2],aabb[3],aabb[4],aabb[5]); if (geom_class_temp==8){ // trimesh body_temp = dGeomGetBody(geom_temp); printf(" Coordinates of associated body are:"); ICubSim::printPositionOfBody(body_temp); } } else { dGeomGetAABB(geom_temp,aabb); printf(" ID: %p; bounding box coordinates: %f-%f,%f-%f,%f-%f\n:",geom_temp,aabb[0],aabb[1],aabb[2],aabb[3],aabb[4],aabb[5]); } } } }
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); } }
void PhysicsGeom::getAABB(Real32 aabb[6]) const { dReal t[6]; dGeomGetAABB(_GeomID, t); aabb[0] = t[0]; aabb[1] = t[1]; aabb[2] = t[2]; aabb[3] = t[3]; aabb[4] = t[4]; aabb[5] = t[5]; }
void BoxObstacle::getAABB(double& minX, double& maxX, double& minY, double& maxY, double& minZ, double& maxZ) { dReal aabb[6]; dGeomGetAABB(boxGeom_, aabb); minX = aabb[0]; maxX = aabb[1]; minY = aabb[2]; maxY = aabb[3]; minZ = aabb[4]; maxZ = aabb[5]; }
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 Object::showAABB() { dReal aabb[6]; dGeomGetAABB (iGeom,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); glMatrixMode(GL_MODELVIEW); glPushMatrix(); glTranslatef(bbpos[0],bbpos[1],bbpos[2]); Graphics::cube(bbsides[0],bbsides[1],bbsides[0]); glPopMatrix(); }
void CShape::GetAABB(bbox3& AABB) const { if (ODEGeomID) { dReal Box[6]; dGeomGetAABB(ODEGeomID, Box); AABB.vmin.x = Box[0]; AABB.vmax.x = Box[1]; AABB.vmin.y = Box[2]; AABB.vmax.y = Box[3]; AABB.vmin.z = Box[4]; AABB.vmax.z = Box[5]; //???transform? } else { AABB.vmin.x = AABB.vmin.y = AABB.vmin.z = AABB.vmax.x = AABB.vmax.y = AABB.vmax.z = 0.f; } }
static void dRayMotionsAABB (dxGeom *geom, dReal aabb[6]) { dxRayMotions *c = (dxRayMotions*) dGeomGetClassData(geom); dGeomGetAABB(c->ray,aabb); ///dInfiniteAABB(geom,aabb); }
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); } }
void ODEGeom::getGeomAABB(dReal aabb[6]) { dGeomGetAABB(geom,aabb); }
void ompl::control::OpenDEStateSpace::setDefaultBounds() { // limit all velocities to 1 m/s, 1 rad/s, respectively base::RealVectorBounds bounds1(3); bounds1.setLow(-1); bounds1.setHigh(1); setLinearVelocityBounds(bounds1); setAngularVelocityBounds(bounds1); // find the bounding box that contains all geoms included in the collision spaces double mX, mY, mZ, MX, MY, MZ; mX = mY = mZ = std::numeric_limits<double>::infinity(); MX = MY = MZ = -std::numeric_limits<double>::infinity(); bool found = false; std::queue<dSpaceID> spaces; for (auto &collisionSpace : env_->collisionSpaces_) spaces.push(collisionSpace); while (!spaces.empty()) { dSpaceID space = spaces.front(); spaces.pop(); int n = dSpaceGetNumGeoms(space); for (int j = 0; j < n; ++j) { dGeomID geom = dSpaceGetGeom(space, j); if (dGeomIsSpace(geom) != 0) spaces.push((dSpaceID)geom); else { bool valid = true; dReal aabb[6]; dGeomGetAABB(geom, aabb); // things like planes are infinite; we want to ignore those for (double k : aabb) if (fabs(k) >= std::numeric_limits<dReal>::max()) { valid = false; break; } if (valid) { found = true; if (aabb[0] < mX) mX = aabb[0]; if (aabb[1] > MX) MX = aabb[1]; if (aabb[2] < mY) mY = aabb[2]; if (aabb[3] > MY) MY = aabb[3]; if (aabb[4] < mZ) mZ = aabb[4]; if (aabb[5] > MZ) MZ = aabb[5]; } } } } if (found) { double dx = MX - mX; double dy = MY - mY; double dz = MZ - mZ; double dM = std::max(dx, std::max(dy, dz)); // add 10% in each dimension + 1% of the max dimension dx = dx / 10.0 + dM / 100.0; dy = dy / 10.0 + dM / 100.0; dz = dz / 10.0 + dM / 100.0; bounds1.low[0] = mX - dx; bounds1.high[0] = MX + dx; bounds1.low[1] = mY - dy; bounds1.high[1] = MY + dy; bounds1.low[2] = mZ - dz; bounds1.high[2] = MZ + dz; setVolumeBounds(bounds1); } }
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 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); } }
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); } }