bool WorldSimulation::InContact(int aid,int bid)
{
//try finding this in the feedback map
if(bid < 0) {
ODEObjectID a=WorldToODEID(aid);
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
if(i->first.first == a || i->first.second == a) {
if(i->second.inContact) return true;
}
}
}
else {
ContactFeedbackInfo* info=GetContactFeedback(aid,bid);
if(info) {
return info->inContact;
}
}
ODEObjectID a=WorldToODEID(aid);
if(bid < 0) return odesim.InContact(a);
else {
ODEObjectID b=WorldToODEID(bid);
return odesim.InContact(a,b);
}
}
bool WorldSimulation::HadSeparation(int aid,int bid)
{
if(bid < 0) {
ODEObjectID a=WorldToODEID(aid);
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
if(i->first.first == a || i->first.second == a) {
if(i->second.hadSeparation) return true;
}
}
return false;
}
else {
ContactFeedbackInfo* info=GetContactFeedback(aid,bid);
if(!info) return false;
return info->hadSeparation;
}
}
Vector3 WorldSimulation::MeanContactForce(int aid,int bid)
{
ODEObjectID a=WorldToODEID(aid);
if(bid < 0) {
Vector3 sum(Zero);
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
if(a == i->first.first)
sum += i->second.meanForce;
else if(a == i->first.second)
sum -= i->second.meanForce;
}
return sum;
}
else {
ContactFeedbackInfo* info=GetContactFeedback(aid,bid);
if(aid<bid) return info->meanForce;
else return info->meanForce;
}
}
