/*! Adds a virtual contact on the body \a body1, which is assumed to
be the \a l-th link on the \a f-th finger of a robot. Works by
first creating a traditional contact, then converting it to a
virtual contact.
The whole VirtualContact scheme is in bad need of an overhaul.
*/
void
addVirtualContacts(Body *body1, int f, int l, Body *body2, ContactReport &contactSet,
bool softContactsOn )
{
if ( softContactsOn && body1->isElastic() ) {
findSoftNeighborhoods( body1, body2, contactSet );
mergeSoftNeighborhoods(body1, body2, contactSet);
}
ContactReport::iterator cp;
for (cp=contactSet.begin(); cp!=contactSet.end(); cp++) {
Contact *c1;
if ( softContactsOn && body1->isElastic() ) {
c1 = new SoftContact( body1, body2, cp->b1_pos, cp->b1_normal, &(cp->nghbd1) );
c1->setMate(NULL);
((SoftContact *)c1)->setUpFrictionEdges();
} else {
c1 = new PointContact(body1, body2, cp->b1_pos, cp->b1_normal);
c1->setMate(NULL);
}
VirtualContact *vc = new VirtualContact(f, l, c1);
vc->setBody(body1);
body1->addVirtualContact(vc);
delete c1;
}
}
/*! For elastic bodies, calculates the contact neighborhoods and puts
them in the contact set. Not done for rigid contacts.
*/
void findSoftNeighborhoods( Body *body1, Body *body2, ContactReport &contactSet )
{
ContactReport::iterator itr;
for( itr = contactSet.begin(); itr != contactSet.end(); itr++ ) {
DBGP("Contact finding regions:");
//right now, findregion assumes point is in body frame
//the units for the threshold and the radius should be in mm, NOT cm
//The input radius is proportional to the fourth root of the youngs mod/depth of mattress
//(units for Youngs Mod and mattress depth are in Pa and meters)
//This gives a radius around 6 mm, for rubber with youngs = 1.5E6 and h = 3E-3which is reasonable
double rad = pow( 1/(MAX( body1->getYoungs(), body2->getYoungs() )), 0.333 ) * 1000.0 * 0.4;
//the 0.4 is a fudge factor for the time being
//hack to ensure that the fit is at least resonable
if( rad <= 3.0 && rad >= 10.0 ) rad = 5.0;
body1->getWorld()->FindRegion( body1, itr->b1_pos, itr->b1_normal, rad, &(itr->nghbd1) );
DBGP("Neighborhood on body1 has " << itr->nghbd1.size() << " points");
body2->getWorld()->FindRegion( body2, itr->b2_pos, itr->b2_normal, rad, &(itr->nghbd2) );
DBGP("Neighborhood on body2 has " << itr->nghbd2.size() << " points");
}
}
/*! For elastic objects, if we have two contacts close to each other
we assume they are actually part of the same contact. This function
checks for overlapping neighborhoods and merges them.
*/
void
mergeSoftNeighborhoods(Body *body1, Body *body2, ContactReport &contactSet)
{
bool mergePerformed = true;
ContactReport::iterator refContact, otherContact;
while (mergePerformed) {
mergePerformed = false;
//check each contact agains each other contact
for (refContact=contactSet.begin(); refContact!=contactSet.end(); refContact++) {
for (otherContact = contactSet.begin(); otherContact != contactSet.end(); otherContact++) {
if (otherContact == refContact) continue;
//this arbitrarily checks only the neighborhoods on body1
if ( neighborhoodsOverlap(refContact->nghbd1, otherContact->nghbd1) ) {
DBGP("Overlap found");
// this should be improved; it keeps the closest contact but merges neighborhoods
// ideally it should keep the closest contact and RECALCULATE the neighborhoods
if ( contactDistance(body1, body2, *refContact) < contactDistance(body1, body2, *otherContact) ) {
mergeNeighborhoods( refContact->nghbd1, otherContact->nghbd1 );
mergeNeighborhoods( refContact->nghbd2, otherContact->nghbd2 );
contactSet.erase( otherContact );
} else {
mergeNeighborhoods( otherContact->nghbd1, refContact->nghbd1 );
mergeNeighborhoods( otherContact->nghbd2, refContact->nghbd2 );
contactSet.erase( refContact );
}
mergePerformed = true;
break;
} else {
DBGP("Overlap not found");
}
}
//if we have a merge, restart operation
if (mergePerformed)
break;
}
}
}
void
CollisionInterface::replaceContactSetWithPerimeter(ContactReport &contactSet)
{
if (contactSet.size() < 2) return;
double my_resabs = 1.0e-1;
// first check for a colinear point set
vec3 currLine, testLine;
ContactReport::iterator endPt1 = contactSet.begin();
ContactReport::iterator endPt2 = ++contactSet.begin();
ContactReport::iterator cp;
for (cp=contactSet.begin();cp!=contactSet.end();cp++) {
currLine = endPt2->b1_pos - endPt1->b1_pos;
testLine = cp->b1_pos - endPt1->b1_pos;
vec3 crossProd = testLine * currLine;
if ( crossProd.len() > my_resabs) break; // not colinear
double dot = testLine % currLine;
if (dot < 0) endPt1 = cp;
if (dot > currLine % currLine) endPt2 = cp;
}
if (cp==contactSet.end()) {
// colinear points
ContactReport tmpSet;
tmpSet.push_back(*endPt1);
tmpSet.push_back(*endPt2);
contactSet.clear();
contactSet = tmpSet;
return;
}
// compute the origin of the projection frame
vec3 contactNormal = contactSet.begin()->b1_normal;
vec3 normal = normalise(testLine * currLine);
double Soffset = contactSet.begin()->b1_pos % normal;
vec3 origin_pr = Soffset * normal;
// compute 2 other axes along the plane of S
vec3 axis1 = normalise(testLine);
vec3 axis2 = normal * axis1;
coordT *array = new coordT[contactSet.size()*2];
coordT *ptr = &array[0];
volatile int ptCount = 0;
for (cp=contactSet.begin(); cp!=contactSet.end(); cp++) {
*ptr++ = (cp->b1_pos - position::ORIGIN) % axis1;
*ptr++ = (cp->b1_pos - position::ORIGIN) % axis2;
ptCount++;
}
ContactReport tmpSet = contactSet;
contactSet.clear();
//qhull paramerers
int exitcode,curlong,totlong;
char options[200];
//serialize access to qhull which is not thread-safe
qhull_mutex.lock();
bool ismalloc = False; // True if qh_freeqhull should 'free(array)'
FILE *qhfp = fopen("logfile","w");
if (!qhfp) {
fprintf(stderr,"Could not open qhull logfile!\n");
qh_init_A(NULL, stdout, stderr, 0, NULL);
} else {
qh_init_A(NULL, qhfp, qhfp, 0, NULL);
}
if((exitcode = setjmp(qh errexit))) {
delete [] array;
if (qhfp) fclose(qhfp);
qhull_mutex.unlock();
return;
}
sprintf(options, "qhull n Pp");
try {
qh_initflags(options);
qh_init_B(&array[0],ptCount, 2, ismalloc);
qh_qhull();
qh_check_output();
} catch(...) {
//qhull has failed
DBGA("QHull CompactSet failed!!!");
//reinsert all original contacts
contactSet.insert(contactSet.begin(), tmpSet.begin(), tmpSet.begin());
delete [] array;
fclose(qhfp);
qhull_mutex.unlock();
return;
}
fclose(qhfp);
vertexT *vertex;
double x,y;
ContactData tmpContact;
// keep only those vertices in the set that match the convex hull vertices
FORALLvertices {
x = vertex->point[0];
y = vertex->point[1];
tmpContact.b1_pos[0] = x * axis1[0] + y * axis2[0] + origin_pr[0];
tmpContact.b1_pos[1] = x * axis1[1] + y * axis2[1] + origin_pr[1];
tmpContact.b1_pos[2] = x * axis1[2] + y * axis2[2] + origin_pr[2];
tmpContact.b1_normal = contactNormal;
for (cp=tmpSet.begin(); cp!=tmpSet.end(); cp++) {
if (fabs(cp->b1_pos[0] - tmpContact.b1_pos[0]) < my_resabs &&
fabs(cp->b1_pos[1] - tmpContact.b1_pos[1]) < my_resabs &&
fabs(cp->b1_pos[2] - tmpContact.b1_pos[2]) < my_resabs &&
fabs(cp->b1_normal[0] - tmpContact.b1_normal[0]) < my_resabs &&
fabs(cp->b1_normal[1] - tmpContact.b1_normal[1]) < my_resabs &&
fabs(cp->b1_normal[2] - tmpContact.b1_normal[2]) < my_resabs)
break;
}
if (cp==tmpSet.end()) {
} else {
contactSet.push_back(*cp);
}
}
qh NOerrexit= True;
qh_freeqhull(!qh_ALL);
qh_memfreeshort (&curlong, &totlong);
qhull_mutex.unlock();
delete [] array;
}
/*!
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() );
}
}
}
}
