/*! This is a helper function for grasp force optimization routines. Given a
set of contacts and a matrix of contact wrenches expressed in *local*
contact coordinates, it will set these wrenches in the dynamic wrench slot
of the contacts for rendering purposes.
*/
void
Grasp::displayContactWrenches(std::list<Contact*> *contacts,
const Matrix &contactWrenches)
{
int count = 0;
std::list<Contact*>::iterator it;
for (it=contacts->begin(); it!=contacts->end(); it++, count++) {
Contact *contact = *it;
if (contact->getBody2()->isDynamic()) {
//wrench is also scaled down for now for rendering and output purposes
//we also transform the wrench to the mate's coordinate system, which
//usually involves negating the x and z axes
double dynWrench[6];
for (int i=0; i<6; i++) {
dynWrench[i] = -1.0 * 1.0e-6 * contactWrenches.elem(6*count+i,0);
}
//the y axis is not negated
dynWrench[1] = -1.0 * dynWrench[1];
dynWrench[4] = -1.0 * dynWrench[4];
contact->getMate()->setDynamicContactWrench(dynWrench);
}
}
}
/*!
Takes pointers to the two bodies in contact, and the set of contacts returned
from the collision detection system, and adds a contact to each body for each
contact in the set.
*/
void
addContacts(Body *body1, Body *body2, ContactReport &contactSet, bool softContactsOn )
{
ContactReport::iterator cp;
Contact *c1,*c2;
int i;
if ( softContactsOn && ( body1->isElastic() || body2->isElastic() ) ) {
findSoftNeighborhoods( body1, body2, contactSet);
DBGP("Before merge: " << contactSet.size());
mergeSoftNeighborhoods( body1, body2, contactSet);
DBGP("After merge: " << contactSet.size());
}
for (i=0,cp=contactSet.begin();cp!=contactSet.end();cp++,i++) {
DBGP( body1->getName().latin1() << " - " << body2->getName().latin1() << " contact: " <<
cp->b1_pos << " " << cp->b1_normal );
//this is an attempt to check if the contact normals point in the right direction
//based on the distance between the bodies. It is meant to help with bad geometry with ill-defined
//normals. Can be removed completely - should never come up for good geometry
if (! checkContactNormals(body1, body2, &(*cp)) ) {
DBGP("Wrong normals detected!");
}
if ( softContactsOn && ( body1->isElastic() || body2->isElastic() ) ) {
c1 = new SoftContact( body1, body2, cp->b1_pos, cp->b1_normal, &(cp->nghbd1) );
c2 = new SoftContact( body2, body1, cp->b2_pos, cp->b2_normal, &(cp->nghbd2) );
c1->setMate(c2);
c2->setMate(c1);
((SoftContact *)c1)->setUpFrictionEdges();
((SoftContact *)c2)->setUpFrictionEdges();
} else {
c1 = new PointContact(body1,body2,cp->b1_pos,cp->b1_normal);
c2 = new PointContact(body2,body1,cp->b2_pos,cp->b2_normal);
c1->setMate(c2);
c2->setMate(c1);
}
body1->addContact(c1);
body2->addContact(c2);
//check if the new contacts inherit two contacts from previous time step
//if so, remove ancestors so nobody else inherits them
Contact *ancestor = body1->checkContactInheritance(c1);
if (ancestor) {
c1->inherit(ancestor);
if (!ancestor->getMate())
fprintf(stderr,"No mate for inherited contact!!\n");
else
c2->inherit(ancestor->getMate());
//careful: this also deletes the removed contact so remove the mate first
if (ancestor->getMate()) body2->removePrevContact( ancestor->getMate() );
body1->removePrevContact( ancestor );
} else {
ancestor = body2->checkContactInheritance(c2);
if (ancestor){
if (!ancestor->getMate())
fprintf(stderr,"No mate for inherited contact!!\n");
else
c1->inherit(ancestor->getMate());
c2->inherit(ancestor);
if (ancestor->getMate()) body1->removePrevContact( ancestor->getMate() );
body2->removePrevContact( ancestor );
} else {
// fprintf(stderr,"New contact between %s and %s\n",body1->getName().latin1(), body2->getName().latin1() );
}
}
}
}