double PQP_Distance(double R1[3][3], double T1[3], PQP_Model *PQP_Model1, double R2[3][3], double T2[3], PQP_Model *PQP_Model2, PQP_REAL *P1, PQP_REAL *P2, int qsize) { PQP_REAL V1[3], V2[3]; PQP_DistanceResult dres; PQP_Distance(&dres, R1, T1, PQP_Model1, R2, T2, PQP_Model2, 0.0, 0.0, qsize); VcV(V1, dres.P1()); VcV(V2, dres.P2()); MxVpV(P1, R1, V1, T1); MxVpV(P2, R2, V2, T2); return (double)(dres.Distance()); }
int main() { // initialize PQP model pointers PQP_Model *b1 = new PQP_Model; PQP_Model *b2 = new PQP_Model; // Add trianges to form tori fprintf(stderr, "loading tris into PQP_Model objects..."); fflush(stderr); PQP_REAL a = (PQP_REAL)1.0; // major radius of the tori PQP_REAL b = (PQP_REAL)0.2; // minor radius of the tori int n1 = 50; // tori will have n1*n2*2 triangles each int n2 = 50; int uc, vc; int count = 0; b1->BeginModel(); b2->BeginModel(); for(uc=0; uc<n1; uc++) { for(vc=0; vc<n2; vc++) { PQP_REAL u1 = (PQP_REAL)(2.0*PI*uc) / n1; PQP_REAL u2 = (PQP_REAL)(2.0*PI*(uc+1)) / n1; PQP_REAL v1 = (PQP_REAL)(2.0*PI*vc) / n2; PQP_REAL v2 = (PQP_REAL)(2.0*PI*(vc+1)) / n2; PQP_REAL p1[3], p2[3], p3[3], p4[3]; p1[0] = (a - b * cos(v1)) * cos(u1); p2[0] = (a - b * cos(v1)) * cos(u2); p3[0] = (a - b * cos(v2)) * cos(u1); p4[0] = (a - b * cos(v2)) * cos(u2); p1[1] = (a - b * cos(v1)) * sin(u1); p2[1] = (a - b * cos(v1)) * sin(u2); p3[1] = (a - b * cos(v2)) * sin(u1); p4[1] = (a - b * cos(v2)) * sin(u2); p1[2] = b * sin(v1); p2[2] = b * sin(v1); p3[2] = b * sin(v2); p4[2] = b * sin(v2); b1->AddTri(p1, p2, p3, count); b1->AddTri(p4, p2, p3, count+1); b2->AddTri(p1, p2, p3, count); b2->AddTri(p4, p2, p3, count+1); count += 2; } } fprintf(stderr, "done\n"); fflush(stderr); fprintf(stderr, "Tori have %d triangles each.\n", count); fprintf(stderr, "building hierarchies..."); fflush(stderr); b1->EndModel(); b2->EndModel(); fprintf(stderr, "done.\n"); b1->MemUsage(1); b2->MemUsage(1); fflush(stderr); // now we are free to call the proximity routines. // but first, construct the transformations that define the placement // of our two hierarchies in world space: // this placement causes them to overlap a large amount. PQP_REAL R1[3][3], R2[3][3], T1[3], T2[3]; R1[0][0] = R1[1][1] = R1[2][2] = 1.0; R1[0][1] = R1[1][0] = R1[2][0] = 0.0; R1[0][2] = R1[1][2] = R1[2][1] = 0.0; R2[0][0] = R2[1][1] = R2[2][2] = 1.0; R2[0][1] = R2[1][0] = R2[2][0] = 0.0; R2[0][2] = R2[1][2] = R2[2][1] = 0.0; T1[0] = 1.0; T1[1] = 0.0; T1[2] = 0.0; T2[0] = 0.0; T2[1] = 0.0; T2[2] = 0.0; // perform a collision query PQP_CollideResult cres; PQP_Collide(&cres, R1, T1, b1, R2, T2, b2, PQP_ALL_CONTACTS); // looking at the report, we can see where all the contacts were, and // also how many tests were necessary: printf("\nAll contact collision query between overlapping tori:\n"); printf("Num BV tests: %d\n", cres.NumBVTests()); printf("Num Tri tests: %d\n", cres.NumTriTests()); printf("Num contact pairs: %d\n", cres.NumPairs()); #if LISTS int i; for(i=0; i<cres.NumPairs(); i++) { printf("\t contact %4d: tri %4d and tri %4d\n", i, cres.Id1(i), cres.Id2(i)); } #endif // Notice the PQP_ALL_CONTACTS flag we used in the call to PQP_Collide. // The alternative is to use the PQP_FIRST_CONTACT flag, instead. // The result is that the collide routine searches for any contact, // but not all of them. It can take many many fewer tests to locate a single // contact. PQP_Collide(&cres, R1, T1, b1, R2, T2, b2, PQP_FIRST_CONTACT); printf("\nFirst contact collision query between overlapping tori:\n"); printf("Num BV tests: %d\n", cres.NumBVTests()); printf("Num Tri tests: %d\n", cres.NumTriTests()); printf("Num contact pairs: %d\n", cres.NumPairs()); #if LISTS for(i=0; i<cres.NumPairs(); i++) { printf("\t contact %4d: tri %4d and tri %4d\n", i, cres.Id1(i), cres.Id2(i)); } #endif // Perform a distance query, which should return a distance of 0.0 PQP_DistanceResult dres; PQP_Distance(&dres, R1, T1, b1, R2, T2, b2, 0.0, 0.0); printf("\nDistance query between overlapping tori\n"); printf("Num BV tests: %d\n", dres.NumBVTests()); printf("Num Tri tests: %d\n", dres.NumTriTests()); printf("Distance: %lf\n", dres.Distance()); // by rotating one of them around the x-axis 90 degrees, they // are now interlocked, but not quite touching. R1[0][0] = 1.0; R1[0][1] = 0.0; R1[0][2] = 0.0; R1[1][0] = 0.0; R1[1][1] = 0.0; R1[1][2] =-1.0; R1[2][0] = 0.0; R1[2][1] = 1.0; R1[2][2] = 0.0; PQP_Collide(&cres, R1, T1, b1, R2, T2, b2, PQP_FIRST_CONTACT); printf("\nCollision query between interlocked but nontouching tori:\n"); printf("Num BV tests: %d\n", cres.NumBVTests()); printf("Num Tri tests: %d\n", cres.NumTriTests()); printf("Num contact pairs: %d\n", cres.NumPairs()); #if LISTS for(i=0; i<cres.NumPairs(); i++) { printf("\t contact %4d: tri %4d and tri %4d\n", i, cres.Id1(i), cres.Id2(i)); } #endif // Perform a distance query - the distance found should be greater than zero PQP_Distance(&dres, R1, T1, b1, R2, T2, b2, 0.0, 0.0); printf("\nDistance query between interlocked but nontouching tori\n"); printf("Num BV tests: %d\n", dres.NumBVTests()); printf("Num Tri tests: %d\n", dres.NumTriTests()); printf("Distance: %lf\n", dres.Distance()); // Perform two tolerance queries. One tolerance setting is greater than the // distance between the models, and one tolerance is less than the distance. PQP_ToleranceResult tres; PQP_REAL tolerance = (PQP_REAL).60; PQP_Tolerance(&tres, R1, T1, b1, R2, T2, b2, tolerance); printf("\nTolerance query between interlocked but nontouching tori\n" "with tolerance %lf\n", tolerance); printf("Num BV tests: %d\n", tres.NumBVTests()); printf("Num Tri tests: %d\n", tres.NumTriTests()); printf("Closer than tolerance? ",tolerance); if (tres.CloserThanTolerance()) printf("yes.\n"); else printf("no.\n"); tolerance = (PQP_REAL).40; PQP_Tolerance(&tres, R1, T1, b1, R2, T2, b2, tolerance); printf("\nTolerance query between interlocked but nontouching tori\n" "with tolerance %lf\n", tolerance); printf("Num BV tests: %d\n", tres.NumBVTests()); printf("Num Tri tests: %d\n", tres.NumTriTests()); printf("Closer than tolerance? ",tolerance); if (tres.CloserThanTolerance()) printf("yes.\n"); else printf("no.\n"); // by moving one of the tori closer to the other, they // almost touch. This is the case that requires a lot // of work wiht methods which use bounding boxes of limited // aspect ratio. Oriented bounding boxes are more efficient // at determining noncontact than spheres, octree, or axis-aligned // bounding boxes for scenarios like this. In this case, the interlocked // tori are separated by 0.0001 at their closest point. T1[0] = (PQP_REAL)1.5999; PQP_Collide(&cres, R1, T1, b1, R2, T2, b2, PQP_FIRST_CONTACT); printf("\nCollision query on interlocked and almost touching tori:\n"); printf("Num BV tests: %d\n", cres.NumBVTests()); printf("Num Tri tests: %d\n", cres.NumTriTests()); printf("Num contact pairs: %d\n", cres.NumPairs()); #if LISTS for(i=0; i<cres.NumPairs(); i++) { printf("\t contact %4d: tri %4d and tri %4d\n", i, cres.Id1(i), cres.Id2(i)); } #endif PQP_Distance(&dres, R1, T1, b1, R2, T2, b2, 0.0, 0.0); printf("\nDistance query between interlocked and almost touching tori\n"); printf("Num BV tests: %d\n", dres.NumBVTests()); printf("Num Tri tests: %d\n", dres.NumTriTests()); printf("Distance: %lf\n", dres.Distance()); tolerance = (PQP_REAL)0.00015; PQP_Tolerance(&tres, R1, T1, b1, R2, T2, b2, tolerance); printf("\nTolerance query between interlocked and almost touching tori\n" "with tolerance %lf\n", tolerance); printf("Num BV tests: %d\n", tres.NumBVTests()); printf("Num Tri tests: %d\n", tres.NumTriTests()); printf("Closer than tolerance? ",tolerance); if (tres.CloserThanTolerance()) printf("yes.\n"); else printf("no.\n"); tolerance = (PQP_REAL)0.00005; PQP_Tolerance(&tres, R1, T1, b1, R2, T2, b2, tolerance); printf("\nTolerance query between interlocked and almost touching tori\n" "with tolerance %lf\n", tolerance); printf("Num BV tests: %d\n", tres.NumBVTests()); printf("Num Tri tests: %d\n", tres.NumTriTests()); printf("Closer than tolerance? ",tolerance); if (tres.CloserThanTolerance()) printf("yes.\n"); else printf("no.\n"); delete b1; delete b2; return 0; }