ODEObjectID WorldSimulation::WorldToODEID(int id) const
{
int i=world->IsRigidObject(id);
if(i>=0) return ODEObjectID(2,i);
i=world->IsTerrain(id);
if(i>=0) return ODEObjectID(0,i);
i=world->IsRobot(id);
if(i>=0) return ODEObjectID(1,i);
pair<int,int> res=world->IsRobotLink(id);
if(res.first>=0) return ODEObjectID(1,res.first,res.second);
FatalError("World ID %d not valid\n",id);
return ODEObjectID();
}
void ODESimulator::DetectCollisions()
{
dJointGroupEmpty(contactGroupID);
//clear feedback structure
for(map<pair<ODEObjectID,ODEObjectID>,ODEContactList>::iterator i=contactList.begin();i!=contactList.end();i++) {
i->second.points.clear();
i->second.forces.clear();
i->second.feedbackIndices.clear();
}
gContacts.clear();
pair<ODEObjectID,ODEObjectID> cindex;
int jcount=0;
if(settings.rigidObjectCollisions) {
//call the collision routine between objects and the world
dSpaceCollide(envSpaceID,(void*)this,collisionCallback);
for(list<ODEContactResult>::iterator j=gContacts.begin();j!=gContacts.end();j++,jcount++) {
//// int is not necessarily big enough, use intptr_t
//int o1 = (int)dGeomGetData(j->o1);
//int o2 = (int)dGeomGetData(j->o2);
intptr_t o1 = (intptr_t)dGeomGetData(j->o1);
intptr_t o2 = (intptr_t)dGeomGetData(j->o2);
if(o1 < 0) { //it's an environment
cindex.first = ODEObjectID(0,(-o1-1));
}
else {
cindex.first = ODEObjectID(2,o1);
}
if(o2 < 0) { //it's an environment
cindex.second = ODEObjectID(0,(-o2-1));
}
else {
cindex.second = ODEObjectID(2,o2);
}
if(o1 < 0 && o2 < 0) {
fprintf(stderr,"Warning, detecting terrain-terrain collisions?\n");
}
else {
j->feedback.resize(j->contacts.size());
SetupContactResponse(cindex.first,cindex.second,jcount,*j);
}
}
}
//do robot-environment collisions
for(size_t i=0;i<robots.size();i++) {
cindex.first = ODEObjectID(1,i);
//call the collision routine between the robot and the world
list<ODEContactResult>::iterator gContactStart = --gContacts.end();
dSpaceCollide2((dxGeom *)robots[i]->space(),(dxGeom *)envSpaceID,(void*)this,collisionCallback);
++gContactStart;
ProcessContacts(gContactStart,gContacts.end(),settings);
//setup the contact "joints" and contactLists
for(list<ODEContactResult>::iterator j=gContactStart;j!=gContacts.end();j++,jcount++) {
bool isBodyo1 = false, isBodyo2 = false;
for(size_t k=0;k<robots[i]->robot.links.size();k++) {
if(robots[i]->triMesh(k) && j->o1 == robots[i]->geom(k)) isBodyo1=true;
if(robots[i]->triMesh(k) && j->o2 == robots[i]->geom(k)) isBodyo2=true;
}
intptr_t body,obj;
if(isBodyo2) {
printf("Warning, ODE collision result lists bodies in reverse order\n");
Assert(!isBodyo1);
body = (intptr_t)dGeomGetData(j->o2);
obj = (intptr_t)dGeomGetData(j->o1);
}
else {
Assert(!isBodyo2);
Assert(isBodyo1);
body = (intptr_t)dGeomGetData(j->o1);
obj = (intptr_t)dGeomGetData(j->o2);
}
//printf("Collision between body %d and obj %d\n",body,obj);
Assert(body >= 0 && body < (int)robots[i]->robot.links.size());
Assert(obj >= -(int)envs.size() && obj < (int)objects.size());
if(robots[i]->robot.parents[body] == -1 && obj < 0) { //fixed links
fprintf(stderr,"Warning, colliding a fixed link and the terrain\n");
continue;
}
cindex.first.bodyIndex = body;
if(obj < 0) { //it's an environment
cindex.second = ODEObjectID(0,(-obj-1));
}
else {
cindex.second = ODEObjectID(2,obj);
}
SetupContactResponse(cindex.first,cindex.second,jcount,*j);
}
if(settings.robotSelfCollisions) {
robots[i]->EnableSelfCollisions(true);
cindex.second = ODEObjectID(1,i);
list<ODEContactResult>::iterator gContactStart = --gContacts.end();
//call the self collision routine for the robot
dSpaceCollide(robots[i]->space(),(void*)robots[i],selfCollisionCallback);
++gContactStart;
ProcessContacts(gContactStart,gContacts.end(),settings);
//setup the contact "joints" and contactLists
for(list<ODEContactResult>::iterator j=gContactStart;j!=gContacts.end();j++,jcount++) {
intptr_t body1 = (intptr_t)dGeomGetData(j->o1);
intptr_t body2 = (intptr_t)dGeomGetData(j->o2);
//printf("Collision between body %d and body %d\n",body1,body2);
Assert(body1 >= 0 && body1 < (int)robots[i]->robot.links.size());
Assert(body2 >= 0 && body2 < (int)robots[i]->robot.links.size());
cindex.first.bodyIndex = body1;
cindex.second.bodyIndex = body2;
SetupContactResponse(cindex.first,cindex.second,jcount,*j);
}
}
if(settings.robotRobotCollisions) {
for(size_t k=i+1;k<robots.size();k++) {
cindex.second = ODEObjectID(1,k);
list<ODEContactResult>::iterator gContactStart = --gContacts.end();
dSpaceCollide2((dxGeom *)robots[i]->space(),(dxGeom *)robots[k]->space(),(void*)this,collisionCallback);
++gContactStart;
ProcessContacts(gContactStart,gContacts.end(),settings);
//setup the contact "joints" and contactLists
for(list<ODEContactResult>::iterator j=gContactStart;j!=gContacts.end();j++,jcount++) {
intptr_t body1 = (intptr_t)dGeomGetData(j->o1);
intptr_t body2 = (intptr_t)dGeomGetData(j->o2);
//printf("Collision between robot %d and robot %d\n",i,k);
Assert(body1 >= 0 && body1 < (int)robots[i]->robot.links.size());
Assert(body2 >= 0 && body2 < (int)robots[k]->robot.links.size());
cindex.first.bodyIndex = body1;
cindex.second.bodyIndex = body2;
SetupContactResponse(cindex.first,cindex.second,jcount,*j);
}
}
}
}
}