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;
}
