bool XmlODESettings::GetSettings(ODESimulator& sim) { string globals="globals"; TiXmlElement* c = e->FirstChildElement(globals); if(c) { printf("Parsing globals...\n"); //parse globals: gravity, etc Vector3 gravity; if(c->QueryValueAttribute("gravity",&gravity)==TIXML_SUCCESS) sim.SetGravity(gravity); double worldERP,worldCFM; if(c->QueryValueAttribute("ERP",&worldERP)==TIXML_SUCCESS) sim.SetERP(worldERP); if(c->QueryValueAttribute("CFM",&worldCFM)==TIXML_SUCCESS) sim.SetCFM(worldCFM); int maxContacts; if(c->QueryValueAttribute("maxContacts",&maxContacts)==TIXML_SUCCESS) sim.GetSettings().maxContacts = maxContacts; int rigidObjectCollisions,robotSelfCollisions,robotRobotCollisions; if(c->QueryValueAttribute("rigidObjectCollisions",&rigidObjectCollisions)==TIXML_SUCCESS) sim.GetSettings().rigidObjectCollisions = rigidObjectCollisions; if(c->QueryValueAttribute("robotSelfCollisions",&robotSelfCollisions)==TIXML_SUCCESS) sim.GetSettings().robotSelfCollisions = robotSelfCollisions; if(c->QueryValueAttribute("robotRobotCollisions",&robotRobotCollisions)==TIXML_SUCCESS) sim.GetSettings().robotRobotCollisions = robotRobotCollisions; } else c=e->FirstChildElement(); while(c!=NULL) { const char* name=c->Value(); printf("Parsing element %s\n",name); if(0 == strcmp(name,"env")) { int index; if(c->QueryValueAttribute("index",&index)==TIXML_SUCCESS) { Assert(index < (int)sim.numEnvs()); TiXmlElement* eg=c->FirstChildElement("geometry"); if(eg) { XmlODEGeometry g(eg); if(!g.Get(*sim.envMesh(index))) { fprintf(stderr,"Error reading environment geometry from XML\n"); return false; } } } else { fprintf(stderr,"Error reading environment index from XML file\n"); return false; } } else if(0 == strcmp(name,"object")) { int index; if(c->QueryValueAttribute("index",&index)==TIXML_SUCCESS) { Assert(index < (int)sim.numObjects()); TiXmlElement* eg=c->FirstChildElement("geometry"); if(eg) { XmlODEGeometry g(eg); if(!g.Get(*sim.object(index)->triMesh())) { fprintf(stderr,"Error reading object geometry from XML\n"); return false; } } } else { fprintf(stderr,"Error reading object index from XML file\n"); return false; } } else if(0 == strcmp(name,"robot")) { int index,bodyIndex=-1; if(c->QueryValueAttribute("index",&index)==TIXML_SUCCESS) { Assert(index < (int)sim.numRobots()); if(c->QueryValueAttribute("body",&bodyIndex)==TIXML_SUCCESS) { } else bodyIndex=-1; ODERobot* robot=sim.robot(index); TiXmlElement* eg=c->FirstChildElement("geometry"); if(eg) { XmlODEGeometry g(eg); if(bodyIndex < 0) { for(size_t i=0;i<robot->robot.links.size();i++) { if(robot->triMesh(i)) g.Get(*robot->triMesh(i)); } } else { Assert(bodyIndex < (int)robot->robot.links.size()); if(robot->triMesh(bodyIndex)) g.Get(*robot->triMesh(bodyIndex)); } } } else { fprintf(stderr,"Error reading robot index from XML file\n"); return false; } } c=c->NextSiblingElement(); } return true; }
void internal_raycastCollisionCallback(void* data, dGeomID o0, dGeomID o1) { if (dGeomIsSpace(o0) || dGeomIsSpace(o1)) { // Colliding a space with either a geom or another space. dSpaceCollide2(o0, o1, data, &internal_raycastCollisionCallback); } else { // Colliding two geoms. // Sometimes we get a case where the ray geom is passed in // as both objects, which is stupid. if (o0 == o1) { return ; } // Get a pointer to the ODESimulator. ODESimulator* sim = (ODESimulator*) data; // Get pointers to the two geoms' GeomData structure. One // of these (the one NOT belonging to the ray geom) // will always be non-NULL. GeomData* geomData0 = ((GeomData*) dGeomGetData(o0)); GeomData* geomData1 = ((GeomData*) dGeomGetData(o1)); // Find the contact group of the collided Solid. unsigned int geomContactGroup = defaults::shape::contactGroup; if (geomData0) { geomContactGroup = geomData0->shape->contactGroup; } else { geomContactGroup = geomData1->shape->contactGroup; } // Check if the two Solids' contact groups generate contacts // when they collide. bool makeContacts = sim->groupsMakeContacts( geomContactGroup, sim->internal_getRayContactGroup()); if (!makeContacts) { return ; } // Now actually test for collision between the two geoms. // This is a fairly expensive operation. dContactGeom contactArray[defaults::ode::maxRaycastContacts]; int numContacts = dCollide(o0, o1, defaults::ode::maxRaycastContacts, contactArray, sizeof(dContactGeom)); if (0 == numContacts) { return ; } else { // These two geoms must be intersecting. We will store // only the closest RaycastResult. int closest = 0; for (int i = 0; i < numContacts; ++i) { if (contactArray[i].depth < contactArray[closest].depth) { closest = i; } } // Only one of the geoms will be part of a Solid we // want to store; the other is the ray. Solid* solid = NULL; if (geomData0) { solid = geomData0->solid; } else { solid = geomData1->solid; } Point3r intersection((real) contactArray[closest].pos[0], (real) contactArray[closest].pos[1], (real) contactArray[closest].pos[2]); Vec3r normal((real) contactArray[closest].normal[0], (real) contactArray[closest].normal[1], (real) contactArray[closest].normal[2]); sim->internal_addRaycastResult(solid, intersection, normal, (real) contactArray[closest].depth); } } }
void internal_collisionCallback(void *data, dGeomID o0, dGeomID o1) { if (dGeomIsSpace(o0) || dGeomIsSpace(o1)) { // Colliding a space with either a geom or another space. dSpaceCollide2(o0, o1, data, &internal_collisionCallback); if (dGeomIsSpace(o0)) { // Colliding all geoms internal to the space. dSpaceCollide((dSpaceID) o0, data, &internal_collisionCallback); } if (dGeomIsSpace(o1)) { // Colliding all geoms internal to the space. dSpaceCollide((dSpaceID) o1, data, &internal_collisionCallback); } } else { // Colliding two geoms. // The following is a set of special cases where we might // want to skip collision detection (but not all are // enforced here for various reasons): // 1. Two static Solids (neither geom has a body) AND // neither Solid has a CollisionEventHandler AND there are // not global handlers: this is enforced. // 2. Two Shapes that are part of the same Solid (they // share a body): this is not enforced because ODE // already takes care of it. // 3. Two sleeping Solids (note: both must have bodies to // check this): this is enforced. (ODE should handle // this, but it doesn't.) // 4. Two Solids connected by a fixed Joint: this is // enforced. // 5. Two Solids connected by a Joint (besides ODE // contact joints, which should never generate more // contacts) with contacts disabled (note: both must have // bodies to check this): this is enforced. // 6. Solid0 is static, Solid1 is dynamic and is sleeping, // static-to-sleeping contacts are ignored by the // Simulator, and neither Solid has a // CollisionEventHandler AND there are no global handlers: // this is enforced. // 7. Solid1 is static, Solid0 is dynamic and is sleeping, // static-to-sleeping contacts are ignored by the // Simulator, and neither Solid has a // CollisionEventHandler AND there are no global handlers: // this is enforced. // 8. The two Solids' contact groups do not generate // contacts when they collide AND neither Solid has a // CollisionEventHandler AND there are no global handlers. // Get the geoms' ODE body IDs. dBodyID o0BodyID = dGeomGetBody(o0); dBodyID o1BodyID = dGeomGetBody(o1); bool solid0Static = (0 == o0BodyID); bool solid1Static = (0 == o1BodyID); // Check if both Solids are dynamic (i.e. have ODE bodies). bool bothHaveBodies = true; if (0 == o0BodyID || 0 == o1BodyID) { bothHaveBodies = false; } // If the two Solids are connected by a common Joint, get // a pointer to that Joint. Joint* commonJoint = NULL; if (bothHaveBodies && dAreConnectedExcluding(o0BodyID, o1BodyID, dJointTypeContact)) { // This will become non-NULL only if there exists an ODE // joint connecting the two bodies. commonJoint = internal_getCommonJoint(o0BodyID, o1BodyID); } // Get pointers to the geoms' GeomData structures. GeomData* geomData0 = (GeomData*) dGeomGetData(o0); GeomData* geomData1 = ((GeomData*) dGeomGetData(o1)); // Get pointers to the geoms' ShapeData structures. const ShapeData* shape0 = geomData0->shape; const ShapeData* shape1 = geomData1->shape; // Get a pointer to the ODESimulator. ODESimulator* sim = (ODESimulator*)data; // Check if the two Solids' contact groups generate contacts // when they collide. bool makeContacts = sim->groupsMakeContacts( shape0->contactGroup, shape1->contactGroup); // Find out whether the Simulator has static-to-sleeping // contacts disabled. bool ignoreStaticSleepingContacts = !sim->areStaticSleepingContactsEnabled(); // Get pointers to the geoms' Solids. Solid* solid0 = geomData0->solid; Solid* solid1 = geomData1->solid; // Get pointers to the two Solids' CollisionEventHandlers. // These will be NULL if the Solids don't use // CollisionEventHandlers. CollisionEventHandler* handler0 = solid0->getCollisionEventHandler(); CollisionEventHandler* handler1 = solid1->getCollisionEventHandler(); bool neitherHasCollisionHandler = !(handler0 || handler1); bool noGlobalCollisionHandlers = sim->getNumGlobalCollisionEventHandlers() == 0; // Now do the actual tests to see if we should return early. // It is important here that we don't call dBodyIsEnabled on // a static body because that crashes ODE. bool case1 = neitherHasCollisionHandler && noGlobalCollisionHandlers && solid0Static && solid1Static; //bool case2= o0BodyID == o1BodyID; bool case3 = bothHaveBodies && !dBodyIsEnabled(o0BodyID) && !dBodyIsEnabled(o1BodyID); bool case4 = commonJoint && commonJoint->getType() == FIXED_JOINT; bool case5 = commonJoint && !commonJoint->areContactsEnabled(); bool case6 = solid0Static && 0 != o1BodyID && !dBodyIsEnabled(o1BodyID) && ignoreStaticSleepingContacts && neitherHasCollisionHandler && noGlobalCollisionHandlers; bool case7 = solid1Static && 0 != o0BodyID && !dBodyIsEnabled(o0BodyID) && ignoreStaticSleepingContacts && neitherHasCollisionHandler && noGlobalCollisionHandlers; bool case8 = !makeContacts && neitherHasCollisionHandler && noGlobalCollisionHandlers; if (case1 || case3 || case4 || case5 || case6 || case7 || case8) { return; } // Now actually test for collision between the two geoms. // This is one of the more expensive operations. dWorldID theWorldID = sim->internal_getWorldID(); dJointGroupID theJointGroupID = sim->internal_getJointGroupID(); dContactGeom contactArray[globals::maxMaxContacts]; int numContacts = dCollide(o0, o1, sim->getMaxContacts(), contactArray, sizeof(dContactGeom)); // If the two objects didn't make any contacts, they weren't // touching, so just return. if (0 == numContacts) { return ; } // If at least one of the Solids has a CollisionEventHandler, // send it a CollisionEvent. if (handler0 || handler1 || !noGlobalCollisionHandlers) { // Call the CollisionEventHandlers. Note that we only // use one contact point per collision: just the first one // in the contact array. The order of the Solids // passed to the event handlers is important: the first // one should be the one whose event handler is // getting called. CollisionEvent e; e.thisSolid = solid0; e.otherSolid = solid1; e.pos[0] = (real) contactArray[0].pos[0]; e.pos[1] = (real) contactArray[0].pos[1]; e.pos[2] = (real) contactArray[0].pos[2]; e.normal[0] = (real) contactArray[0].normal[0]; e.normal[1] = (real) contactArray[0].normal[1]; e.normal[2] = (real) contactArray[0].normal[2]; e.depth = (real) contactArray[0].depth; if (handler0) { handler0->internal_pushCollisionEvent(e); } if (handler1) { // For the other Solid's CollisionEventHandler, we need // to invert the normal and swap the Solid pointers. e.normal *= -1; e.thisSolid = solid1; e.otherSolid = solid0; handler1->internal_pushCollisionEvent(e); } sim->internal_recordCollision(e); } // Old version... //// Early check to save some time. //if (solid0->getCollisionEventHandler() // || solid1->getCollisionEventHandler()) //{ // // Call the event handlers. Note: we only use one // // contact point per collision; just use the first one // // in the contact array. The order of the Solids // // passed to the event handlers is important: the first // // one should be the one whose event handler is // // getting called. // CollisionEvent e; // e.solid0 = solid0; // e.solid1 = solid1; // e.pos[0] = contactArray[0].pos[0]; // e.pos[1] = contactArray[0].pos[1]; // e.pos[2] = contactArray[0].pos[2]; // e.normal[0] = contactArray[0].normal[0]; // e.normal[1] = contactArray[0].normal[1]; // e.normal[2] = contactArray[0].normal[2]; // e.depth = contactArray[0].depth; // EventHandler* eventHandler = // solid0->getCollisionEventHandler(); // if (eventHandler) // { // generateContacts0 = // eventHandler->handleCollisionEvent(e); // } // e.normal *= -1; // Invert normal. // e.solid0 = solid1; // Swap solid pointers. // e.solid1 = solid0; // eventHandler = solid1->getCollisionEventHandler(); // if (eventHandler) // { // generateContacts1 = // eventHandler->handleCollisionEvent(e); // } //} if (makeContacts) { // Invalidate the "freely-spinning" parameters. ((ODESolid*)solid0)->internal_setFreelySpinning(false); ((ODESolid*)solid1)->internal_setFreelySpinning(false); for (int i = 0; i < numContacts; ++i) { const Material* m0 = &(shape0->material); const Material* m1 = &(shape1->material); dContact tempContact; tempContact.surface.mode = dContactBounce | dContactSoftERP; // | dContactSoftCFM; // Average the hardness of the two materials. assert(m0->hardness >= 0 && m0->hardness <= 1 && m1->hardness >= 0 && m1->hardness <= 1); real hardness = (m0->hardness + m1->hardness) * (real) 0.5; // Convert hardness to ERP. As hardness goes from // 0.0 to 1.0, ERP goes from min to max. tempContact.surface.soft_erp = hardness * (defaults::ode::maxERP - defaults::ode::minERP) + defaults::ode::minERP; // Don't use contact CFM anymore. Just let it use // the global value set in the ODE world. //tempContact.surface.soft_cfm = // defaults::ode::minCFM; // As friction goes from 0.0 to 1.0, mu goes from 0.0 // to max, though it is set to dInfinity when // friction == 1.0. assert(m0->friction >= 0 && m0->friction <= 1 && m1->friction >= 0 && m1->friction <= 1); if (1.0 == m0->friction && 1.0 == m1->friction) { tempContact.surface.mu = dInfinity; } else { tempContact.surface.mu = sqrt(m0->friction * m1->friction) * defaults::ode::maxFriction; } // Average the bounciness of the two materials. assert(m0->bounciness >= 0 && m0->bounciness <= 1 && m1->bounciness >= 0 && m1->bounciness <= 1); real bounciness = (m0->bounciness + m1->bounciness) * (real) 0.5; // ODE's bounce parameter, a.k.a. restitution. tempContact.surface.bounce = bounciness; // ODE's bounce_vel parameter is a threshold: // the relative velocity of the two objects must be // greater than this for bouncing to occur at all. tempContact.surface.bounce_vel = defaults::bounceThreshold; // Old way to calculate bounce threshold; threshold // was scaled by the collision bounciness, but this // makes all objects with non-zero bounciness // always bounce. This is undesirable because it // takes a while for bouncing to totally diminish. //tempContact.surface.bounce_vel = // defaults::ode::maxBounceVel - bounciness * // defaults::ode::maxBounceVel; tempContact.geom = contactArray[i]; dJointID contactJoint = dJointCreateContact( theWorldID, theJointGroupID, &tempContact); // Note: the following line of code replaces the // rest of this function which is commented out. // TODO: test this and make sure the mass ratio // issues are unimportant and that we never need // "one-sided" contacts between two Solids. dJointAttach(contactJoint, o0BodyID, o1BodyID); //if (!bothHaveBodies) //{ // // at least one object is static, so just handle // // things like normal // dJointAttach(contactJoint, o0BodyID, o1BodyID); //} //else //{ // // TODO: We probably need to remove the following chunk of // // code. The first case is obsolete since now both sides // // always get contacts, if at all. (There isn't really a // // good reason to have one side use contacts but not the // // other; the side not wanting contacts would appear to be // // static, so just make it static in the first place. On // // the other hand, this may end up being desirable if // // an object should be moved by some objects but not // // others, and the "others" *do* collid with the first // // object... but this situation may never come up. The // // second case, using mass ratios to determine whether two // // objects should collide, might not be an issue. Mass // // ratios might only be a problem when the two objects are // // constantly connected with a Joint. // // Handle one-sided contacts for two cases: 1) only one of // // the above event handlers actually wants contacts generated, // // 2) if the mass ratio is above some threshold, treat it as // // a one-sided collision solution: treat the more massive // // object as static, calculate the collision like normal // // (with the massive one being static), then also add the // // massive object's velocity to the smaller one (velocity // // calculated at the point of collision). // // calculate mass ratio (use mass1 / mass2); if // // the ratio is too high, mass1 is too large; if // // the ratio is too low, mass2 is too large // real massRatio = 0; // dMass mass0, mass1; // dBodyGetMass(o0BodyID, &mass0); // dBodyGetMass(o1BodyID, &mass1); // massRatio = mass0.mass / mass1.mass; // // here we handle all the different collision // // cases: one solid or the other or both may want // // contacts generated; also, the mass ratio may // // be outside the acceptable range // if (true == generateContacts0 && true == // generateContacts1) // { // // both want contacts, neither wants to be // // static // if (massRatio > defaults::ode::maxMassRatio) // { // // ratio is too high - mass0 is too large, // // treat solid0 as static // dBodyEnable(o1BodyID); // dJointAttach(contactJoint, 0, o1BodyID); // } // else if (massRatio < // defaults::ode::minMassRatio) // { // // ratio is too low - mass1 is too large, // // treat solid1 as static // dBodyEnable(o0BodyID); // dJointAttach(contactJoint, o0BodyID, 0); // } // else // { // //ratio is good - no static objects // dJointAttach(contactJoint, o0BodyID, // o1BodyID); // } // } // else if (true == generateContacts0) // { // // solid0 wants contacts, solid1 wants to be // // static // if (massRatio > defaults::ode::maxMassRatio) // { // // ratio is too high - mass0 is too large, // // treat solid0 and solid1 as static // // i.e. don't generate a contact joint // } // else // { // // this block handles two cases which have // // the same result: // // 1. ratio is too low - mass1 is too // // large, treat solid1 as static // // 2. ratio is good - treat solid1 as // // static // dBodyEnable(o0BodyID); // dJointAttach(contactJoint, o0BodyID, 0); // } // } // else //generateContacts1 must be true // { // // solid1 wants contacts, solid0 wants to be // // static // if (massRatio < defaults::ode::minMassRatio) // { // // ratio is too low - mass1 is too large, // // treat solid0 and solid1 as static // // i.e. don't generate a contact joint // } // else // { // // this block handles two cases which have // // the same result: // // 1. ratio is too high - mass0 is too // // large, treat solid0 as static // // 2. ratio is good - treat solid0 as // // static // dBodyEnable(o1BodyID); // dJointAttach(contactJoint, 0, o1BodyID); // } // } //} } } } }
void internal_volumeCollisionCallback(void* data, dGeomID o0, dGeomID o1) { if (dGeomIsSpace(o0) || dGeomIsSpace(o1)) { // Colliding a space with either a geom or another space. dSpaceCollide2(o0, o1, data, &internal_volumeCollisionCallback); } else { // Colliding two geoms. //dBodyID o0BodyID = dGeomGetBody(o0); //dBodyID o1BodyID = dGeomGetBody(o1); //bool bothHaveBodies = true; //if (0 == o0BodyID || 0 == o1BodyID) //{ // bothHaveBodies = false; //} // don't do collision detection for the following case: // two shapes that are part of the same solid (they share a // body) // update-> this is already handled by ODE //if (bothHaveBodies && o0BodyID == o1BodyID) //{ // //don't do collision detection // return; //} // Get a pointer to the ODESimulator. ODESimulator* sim = (ODESimulator*) data; // Get pointers to the two geoms' GeomData structure. Both // of these should always be non-NULL. const GeomData* geomData0 = ((GeomData*) dGeomGetData(o0)); const GeomData* geomData1 = ((GeomData*) dGeomGetData(o1)); assert(geomData0); assert(geomData1); // Get pointers to the geoms' ShapeData structures. const ShapeData* shape0 = geomData0->shape; const ShapeData* shape1 = geomData1->shape; // Check if the two Solids' contact groups generate contacts // when they collide. bool makeContacts = sim->groupsMakeContacts( shape0->contactGroup, shape1->contactGroup); if (!makeContacts) { return ; } // Now actually test for collision between the two geoms. // This is a fairly expensive operation. dContactGeom contactArray[1]; int numContacts = dCollide(o0, o1, 1, contactArray, sizeof(dContactGeom)); if (0 == numContacts) { return ; } else { // These two geoms must be intersecting. // Get pointers to the geoms' Solids. Solid* solid0 = geomData0->solid; Solid* solid1 = geomData1->solid; // Not sure at this point if we can know that o1 is the // volume object, so we'll just call this twice. It // will automatically keep from adding the same Solid // multiple times by using its collision count. Later, // the volume Solid will be removed from this list. sim->internal_addCollidedSolid(solid0); sim->internal_addCollidedSolid(solid1); } } }
bool XmlODESettings::GetSettings(ODESimulator& sim) { string globals="globals"; TiXmlElement* c = e->FirstChildElement(globals); if(c) { printf("Parsing globals...\n"); //parse globals: gravity, etc Vector3 gravity; if(c->QueryValueAttribute("gravity",&gravity)==TIXML_SUCCESS) sim.SetGravity(gravity); double worldERP,worldCFM; if(c->QueryValueAttribute("ERP",&worldERP)==TIXML_SUCCESS) sim.SetERP(worldERP); if(c->QueryValueAttribute("CFM",&worldCFM)==TIXML_SUCCESS) sim.SetCFM(worldCFM); int maxContacts; if(c->QueryValueAttribute("maxContacts",&maxContacts)==TIXML_SUCCESS) sim.GetSettings().maxContacts = maxContacts; int boundaryLayer,rigidObjectCollisions,robotSelfCollisions,robotRobotCollisions; if(c->QueryValueAttribute("boundaryLayer",&boundaryLayer)==TIXML_SUCCESS) { printf("XML simulator: warning, boundary layer settings don't have an effect\n"); sim.GetSettings().boundaryLayerCollisions = boundaryLayer; } if(c->QueryValueAttribute("rigidObjectCollisions",&rigidObjectCollisions)==TIXML_SUCCESS) sim.GetSettings().rigidObjectCollisions = rigidObjectCollisions; if(c->QueryValueAttribute("robotSelfCollisions",&robotSelfCollisions)==TIXML_SUCCESS) sim.GetSettings().robotSelfCollisions = robotSelfCollisions; if(c->QueryValueAttribute("robotRobotCollisions",&robotRobotCollisions)==TIXML_SUCCESS) sim.GetSettings().robotRobotCollisions = robotRobotCollisions; } else c=e->FirstChildElement(); while(c!=NULL) { const char* name=c->Value(); printf("Parsing element %s\n",name); if(0 == strcmp(name,"terrain")) { int index; if(c->QueryValueAttribute("index",&index)==TIXML_SUCCESS) { Assert(index < (int)sim.numEnvs()); TiXmlElement* eg=c->FirstChildElement("geometry"); if(eg) { XmlODEGeometry g(eg); if(!g.Get(*sim.envGeom(index))) { fprintf(stderr,"Error reading terrain geometry from XML\n"); return false; } } } else { fprintf(stderr,"Error reading terrain index from XML file\n"); return false; } } else if(0 == strcmp(name,"object")) { int index; if(c->QueryValueAttribute("index",&index)==TIXML_SUCCESS) { Assert(index < (int)sim.numObjects()); TiXmlElement* eg=c->FirstChildElement("geometry"); if(eg) { XmlODEGeometry g(eg); if(!g.Get(*sim.object(index)->triMesh())) { fprintf(stderr,"Error reading object geometry from XML\n"); return false; } } TiXmlElement *ev=c->FirstChildElement("velocity"); if(ev) { Vector3 v,w; if(ev->QueryValueAttribute("linear",&v) != TIXML_SUCCESS) v.setZero(); if(ev->QueryValueAttribute("angular",&w) != TIXML_SUCCESS) w.setZero(); cout<<"Setting velocity "<<w<<", "<<v<<endl; sim.object(index)->SetVelocity(w,v); } } else { fprintf(stderr,"Error reading object index from XML file\n"); return false; } } else if(0 == strcmp(name,"robot")) { int index,bodyIndex=-1; if(c->QueryValueAttribute("index",&index)==TIXML_SUCCESS) { Assert(index < (int)sim.numRobots()); if(c->QueryValueAttribute("body",&bodyIndex)==TIXML_SUCCESS) { } else bodyIndex=-1; ODERobot* robot=sim.robot(index); TiXmlElement* eg=c->FirstChildElement("geometry"); if(eg) { XmlODEGeometry g(eg); if(bodyIndex < 0) { for(size_t i=0;i<robot->robot.links.size();i++) { if(robot->triMesh(i)) g.Get(*robot->triMesh(i)); } } else { Assert(bodyIndex < (int)robot->robot.links.size()); if(robot->triMesh(bodyIndex)) g.Get(*robot->triMesh(bodyIndex)); } } } else { fprintf(stderr,"Error reading robot index from XML file\n"); return false; } } c=c->NextSiblingElement(); } return true; }