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