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() );
}
}
}
}
