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);
}
}
Vector3 WorldSimulation::ContactForce(int aid,int bid)
{
ODEObjectID a=WorldToODEID(aid);
if(bid < 0) {
Vector3 sum(Zero);
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
ODEContactList* c=NULL;
if(i->first.first == a)
c=odesim.GetContactFeedback(a,i->first.second);
else if(i->first.second == a)
c=odesim.GetContactFeedback(i->first.first,a);
if(c) {
Vector3 isum(Zero);
for(size_t j=0;j<c->forces.size();j++)
isum += c->forces[j];
//add to the accumulator
if(a == i->first.first) sum+=isum;
else sum-=isum;
}
}
return sum;
}
else {
ODEObjectID b=WorldToODEID(bid);
ODEContactList* c=odesim.GetContactFeedback(a,b);
Vector3 sum(Zero);
if(!c) return sum;
for(size_t i=0;i<c->forces.size();i++)
sum += c->forces[i];
if(a<b) return sum;
else return -sum;
}
}
ContactFeedbackInfo* WorldSimulation::GetContactFeedback(int aid,int bid)
{
pair<ODEObjectID,ODEObjectID> index(WorldToODEID(aid),WorldToODEID(bid));
if(index.second < index.first)
swap(index.second,index.first);
if(contactFeedback.count(index)==0) return NULL;
return &contactFeedback[index];
}
bool WorldSimulation::InContact(int aid,int bid)
{
ODEObjectID a=WorldToODEID(aid);
if(bid < 0) return odesim.InContact(a);
else {
ODEObjectID b=WorldToODEID(bid);
return odesim.InContact(a,b);
}
}
void WorldSimulation::EnableContactFeedback(int aid,int bid,bool accum,bool accumFull)
{
ContactFeedbackInfo f;
f.accum = accum;
f.accumFull = accumFull;
Reset(f);
pair<ODEObjectID,ODEObjectID> index(WorldToODEID(aid),WorldToODEID(bid));
if(index.second < index.first)
swap(index.second,index.first);
contactFeedback[index] = f;
odesim.EnableContactFeedback(index.first,index.second);
}
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;
}
}
ODEContactList* WorldSimulation::GetContactList(int aid,int bid)
{
ODEObjectID a=WorldToODEID(aid);
ODEObjectID b=WorldToODEID(bid);
return odesim.GetContactFeedback(a,b);
}
