void
RegionFilteredSensor::sensorModel(){
double forces[6] = {0,0,0,0,0,0};
std::list<Contact *>::const_iterator cp;
std::list<Contact *> cList = sbody->getContacts();
//Adding contacts
if(sbody->getWorld()->dynamicsAreOn()){
for(cp = cList.begin(); cp != cList.end(); cp++){
double * contactForce = (*cp)->getDynamicContactWrench();
//std::cout << "Contact pos:" << (*cp)->getPosition()[0] <<" " <<(*cp)->getPosition()[1] <<" "<< (*cp)->getPosition()[2] << std::endl;
if (filterContact(*cp))
addVector6(forces, contactForce);
}
//Adding Forces
double ts = getTimeStep();
if (ts > 0.0)
for(int ind = 0; ind < 6; ind++){
myOutput.sensorReading[ind] = forces[ind] * (retention_level) + myOutput.sensorReading[ind] * (1.0-retention_level);
//std::cout << myOutput.sensorReading[ind] << " ";
}
}
else{
myOutput.sensorReading[2] = 0;
for(cp = cList.begin(); cp != cList.end(); cp++){
if(sbody->getWorld()->softContactsAreOn() && ((*cp)->getBody1()->isElastic() || (*cp)->getBody2()->isElastic())){
std::vector<position> pVec;
std::vector<double> forceVec;
(*cp)->getStaticContactInfo(pVec, forceVec);
//transform the boundary positions OUT of body coordinates to contact coordinates
//this results in fewer expensive matrix operations
//fixme test that this is correct
transf contact = (*cp)->getContactFrame();//.inverse();
/*For the sake of argument, lets say we take the position of the contact on the body,
and look at how that works
*/
mat3 contactRot = (*cp)->getRot();
for (unsigned int pInd = 0; pInd < pVec.size(); pInd ++){
//it appears that the contact frame is a roto-inversion
if(filterContact(pos[0],pos[1], (contactRot*(1000*pVec[pInd]))*contact))
myOutput.sensorReading[2]+= forceVec[pInd]* 10000000;
}
}
else if (filterContact(*cp)){
myOutput.sensorReading[2] += 1;
}
}
}
IVMat->emissiveColor.setValue(1.0-myOutput.sensorReading[2]/3,1.0,1.0-myOutput.sensorReading[2]/3);
//std::cout<<std::endl;
}
TalkableFilter::FilterResult HideOfflineWithoutDescriptionTalkableFilter::filterBuddy(const Buddy &buddy)
{
if (!m_enabled)
return Undecided;
auto const &contact = m_buddyPreferredManager->preferredContact(buddy, false);
if (!contact)
return Rejected;
return filterContact(contact);
}
bool
RegionFilteredSensor::filterContact(Contact * cp){
position ps = cp->getPosition();
return filterContact(pos[0], pos[1], ps);
}
