bool ODESimulator::InContact(const ODEObjectID& a) const
{
if(a.type == 0) { //environment
//must loop through all objects to see what is in contact with the
//environment
for(size_t i=0;i<objects.size();i++) {
if(HasContact(objects[i]->body(),0)) return true;
}
for(size_t i=0;i<robots.size();i++) {
for(size_t j=0;j<robots[i]->robot.links.size();j++) {
if(HasContact(robots[i]->body(j),0)) return true;
}
}
return false;
}
else if(a.type == 2) {//object
return HasContact(objects[a.index]->body());
}
else {
if(a.bodyIndex >= 0)
return HasContact(robots[a.index]->body(a.bodyIndex));
else { //any robot link
for(size_t i=0;i<robots[a.index]->robot.links.size();i++)
if(HasContact(robots[a.index]->body(i))) return true;
return false;
}
}
return false;
}
Vec3 GetContact() const
{
assert(HasContact());
Vec3 mid = pt[0];
for(int i = 1; i < count; ++i)
mid += pt[i];
mid /= float(count);
return mid;
}
bool ODESimulator::InContact(const ODEObjectID& a,const ODEObjectID& b) const
{
vector<dBodyID> bodya, bodyb;
if(a.type == 0) {
bodya.push_back(0);
}
else if(a.type == 2) {
bodya.push_back(objects[a.index]->body());
}
else {
if(a.bodyIndex >= 0)
bodya.push_back(robots[a.index]->body(a.bodyIndex));
else {
for(size_t i=0;i<robots[a.index]->robot.links.size();i++)
if(robots[a.index]->body(i))
bodya.push_back(robots[a.index]->body(i));
}
}
if(b.type == 0) {
bodyb.push_back(0);
}
else if(b.type == 2) {
bodyb.push_back(objects[b.index]->body());
}
else {
if(b.bodyIndex >= 0)
bodyb.push_back(robots[b.index]->body(b.bodyIndex));
else {
for(size_t i=0;i<robots[b.index]->robot.links.size();i++)
if(robots[b.index]->body(i))
bodyb.push_back(robots[b.index]->body(i));
}
}
for(size_t i=0;i<bodya.size();i++)
for(size_t j=0;j<bodyb.size();j++)
if(HasContact(bodya[i],bodyb[j])) return true;
return false;
}