int ColPair::Tolerance(PQP_ToleranceResult& tres,
PQP_REAL tolerance)
{
static PQP_REAL R1[3][3];
static PQP_REAL T1[3];
static PQP_REAL R2[3][3];
static PQP_REAL T2[3];
fVec3 p1(models[0]->joint->abs_pos);
fMat33 r1(models[0]->joint->abs_att);
fVec3 p2(models[1]->joint->abs_pos);
fMat33 r2(models[1]->joint->abs_att);
// memo: definition of R1 and R2 is row major in PQP
R1[0][0] = r1(0,0);
R1[0][1] = r1(0,1);
R1[0][2] = r1(0,2);
R1[1][0] = r1(1,0);
R1[1][1] = r1(1,1);
R1[1][2] = r1(1,2);
R1[2][0] = r1(2,0);
R1[2][1] = r1(2,1);
R1[2][2] = r1(2,2);
T1[0] = p1(0);
T1[1] = p1(1);
T1[2] = p1(2);
R2[0][0] = r2(0,0);
R2[0][1] = r2(0,1);
R2[0][2] = r2(0,2);
R2[1][0] = r2(1,0);
R2[1][1] = r2(1,1);
R2[1][2] = r2(1,2);
R2[2][0] = r2(2,0);
R2[2][1] = r2(2,1);
R2[2][2] = r2(2,2);
T2[0] = p2(0);
T2[1] = p2(1);
T2[2] = p2(2);
return PQP_Tolerance(&tres,
R1, T1, models[0]->model,
R2, T2, models[1]->model,
tolerance);
}
void
cb_display()
{
BeginDraw();
int i;
PQP_CollideResult cres;
PQP_DistanceResult dres;
PQP_ToleranceResult tres;
double oglm[16];
switch(query_type)
{
case 0:
// draw model 1
glColor3f(1,1,1); // setup color and transform
MVtoOGL(oglm,R1[step],T1[step]);
glPushMatrix();
glMultMatrixd(oglm);
torus1_drawn->Draw(); // do gl rendering
glPopMatrix(); // restore transform
// draw model 2
MVtoOGL(oglm,R2[step],T2[step]);
glPushMatrix();
glMultMatrixd(oglm);
torus2_drawn->Draw();
glPopMatrix();
break;
case 1:
// perform collision query
PQP_Collide(&cres,R1[step],T1[step],torus1_tested,
R2[step],T2[step],torus2_tested,
PQP_ALL_CONTACTS);
// draw model 1 and its overlapping tris
MVtoOGL(oglm,R1[step],T1[step]);
glPushMatrix();
glMultMatrixd(oglm);
glColor3f(1,1,1);
torus1_drawn->Draw();
glColor3f(1,0,0);
for(i = 0; i < cres.NumPairs(); i++)
{
torus1_drawn->DrawTri(cres.Id1(i));
}
glPopMatrix();
// draw model 2 and its overlapping tris
MVtoOGL(oglm,R2[step],T2[step]);
glPushMatrix();
glMultMatrixd(oglm);
glColor3f(1,1,1);
torus2_drawn->Draw();
glColor3f(1,0,0);
for(i = 0; i < cres.NumPairs(); i++)
{
torus2_drawn->DrawTri(cres.Id2(i));
}
glPopMatrix();
break;
case 2:
// perform distance query
PQP_Distance(&dres,R1[step],T1[step],torus1_tested,
R2[step],T2[step],torus2_tested,
0.0,0.0);
// draw models
glColor3f(1,1,1);
MVtoOGL(oglm,R1[step],T1[step]);
glPushMatrix();
glMultMatrixd(oglm);
torus1_drawn->Draw();
glPopMatrix();
MVtoOGL(oglm,R2[step],T2[step]);
glPushMatrix();
glMultMatrixd(oglm);
torus2_drawn->Draw();
glPopMatrix();
// draw the closest points as small spheres
glColor3f(0,1,0);
PQP_REAL P1[3],P2[3],V1[3],V2[3];
VcV(P1,dres.P1());
VcV(P2,dres.P2());
// each point is in the space of its model;
// transform to world space
MxVpV(V1,R1[step],P1,T1[step]);
glPushMatrix();
glTranslated(V1[0],V1[1],V1[2]);
glutSolidSphere(.01,15,15);
glPopMatrix();
MxVpV(V2,R2[step],P2,T2[step]);
glPushMatrix();
glTranslated(V2[0],V2[1],V2[2]);
glutSolidSphere(.01,15,15);
glPopMatrix();
// draw the line between the closest points
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3v(V1);
glVertex3v(V2);
glEnd();
glEnable(GL_LIGHTING);
break;
case 3:
// perform tolerance query
PQP_Tolerance(&tres,R1[step],T1[step],torus1_tested,
R2[step],T2[step],torus2_tested,
tolerance);
if (tres.CloserThanTolerance())
glColor3f(0,0,1);
else
glColor3f(1,1,1);
// draw models
MVtoOGL(oglm,R1[step],T1[step]);
glPushMatrix();
glMultMatrixd(oglm);
torus1_drawn->Draw();
glPopMatrix();
MVtoOGL(oglm,R2[step],T2[step]);
glPushMatrix();
glMultMatrixd(oglm);
torus2_drawn->Draw();
glPopMatrix();
break;
}
EndDraw();
}
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;
}