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