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());
}
inline
PQP_REAL
TriDistance(PQP_REAL R[3][3], PQP_REAL T[3], Tri *t1, Tri *t2,
PQP_REAL p[3], PQP_REAL q[3])
{
// transform tri 2 into same space as tri 1
PQP_REAL tri1[3][3], tri2[3][3];
VcV(tri1[0], t1->p1);
VcV(tri1[1], t1->p2);
VcV(tri1[2], t1->p3);
MxVpV(tri2[0], R, t2->p1, T);
MxVpV(tri2[1], R, t2->p2, T);
MxVpV(tri2[2], R, t2->p3, T);
return TriDist(p,q,tri1,tri2);
}
int
split_tris(Tri *tris, int num_tris, PQP_REAL a[3], PQP_REAL c)
{
int i;
int c1 = 0;
PQP_REAL p[3];
PQP_REAL x;
Tri temp;
for(i = 0; i < num_tris; i++)
{
// loop invariant: up to (but not including) index c1 in group 1,
// then up to (but not including) index i in group 2
//
// [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x]
// c1 i
//
VcV(p, tris[i].p1);
VpV(p, p, tris[i].p2);
VpV(p, p, tris[i].p3);
x = VdotV(p, a);
x /= 3.0;
if (x <= c)
{
// group 1
temp = tris[i];
tris[i] = tris[c1];
tris[c1] = temp;
c1++;
}
else
{
// group 2 -- do nothing
}
}
// split arbitrarily if one group empty
if ((c1 == 0) || (c1 == num_tris)) c1 = num_tris/2;
return c1;
}
void
SegPoints(PQP_REAL VEC[3],
PQP_REAL X[3], PQP_REAL Y[3], // closest points
const PQP_REAL P[3], const PQP_REAL A[3], // seg 1 origin, vector
const PQP_REAL Q[3], const PQP_REAL B[3]) // seg 2 origin, vector
{
PQP_REAL T[3], A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T;
PQP_REAL TMP[3];
VmV(T,Q,P);
A_dot_A = VdotV(A,A);
B_dot_B = VdotV(B,B);
A_dot_B = VdotV(A,B);
A_dot_T = VdotV(A,T);
B_dot_T = VdotV(B,T);
// t parameterizes ray P,A
// u parameterizes ray Q,B
PQP_REAL t,u;
// compute t for the closest point on ray P,A to
// ray Q,B
PQP_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B;
t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom;
// clamp result so t is on the segment P,A
if ((t < 0) || isnan(t)) t = 0; else if (t > 1) t = 1;
// find u for point on ray Q,B closest to point at t
u = (t*A_dot_B - B_dot_T) / B_dot_B;
// if u is on segment Q,B, t and u correspond to
// closest points, otherwise, clamp u, recompute and
// clamp t
if ((u <= 0) || isnan(u)) {
VcV(Y, Q);
t = A_dot_T / A_dot_A;
if ((t <= 0) || isnan(t)) {
VcV(X, P);
VmV(VEC, Q, P);
}
else if (t >= 1) {
VpV(X, P, A);
VmV(VEC, Q, X);
}
else {
VpVxS(X, P, A, t);
VcrossV(TMP, T, A);
VcrossV(VEC, A, TMP);
}
}
else if (u >= 1) {
VpV(Y, Q, B);
t = (A_dot_B + A_dot_T) / A_dot_A;
if ((t <= 0) || isnan(t)) {
VcV(X, P);
VmV(VEC, Y, P);
}
else if (t >= 1) {
VpV(X, P, A);
VmV(VEC, Y, X);
}
else {
VpVxS(X, P, A, t);
VmV(T, Y, P);
VcrossV(TMP, T, A);
VcrossV(VEC, A, TMP);
}
}
else {
VpVxS(Y, Q, B, u);
if ((t <= 0) || isnan(t)) {
VcV(X, P);
VcrossV(TMP, T, B);
VcrossV(VEC, B, TMP);
}
else if (t >= 1) {
VpV(X, P, A);
VmV(T, Q, X);
VcrossV(TMP, T, B);
VcrossV(VEC, B, TMP);
}
else {
VpVxS(X, P, A, t);
VcrossV(VEC, A, B);
if (VdotV(VEC, T) < 0) {
VxS(VEC, VEC, -1);
}
}
}
}
PQP_REAL
TriDist(PQP_REAL P[3], PQP_REAL Q[3],
const PQP_REAL S[3][3], const PQP_REAL T[3][3])
{
// Compute vectors along the 6 sides
PQP_REAL Sv[3][3], Tv[3][3];
PQP_REAL VEC[3];
VmV(Sv[0],S[1],S[0]);
VmV(Sv[1],S[2],S[1]);
VmV(Sv[2],S[0],S[2]);
VmV(Tv[0],T[1],T[0]);
VmV(Tv[1],T[2],T[1]);
VmV(Tv[2],T[0],T[2]);
// For each edge pair, the vector connecting the closest points
// of the edges defines a slab (parallel planes at head and tail
// enclose the slab). If we can show that the off-edge vertex of
// each triangle is outside of the slab, then the closest points
// of the edges are the closest points for the triangles.
// Even if these tests fail, it may be helpful to know the closest
// points found, and whether the triangles were shown disjoint
PQP_REAL V[3];
PQP_REAL Z[3];
PQP_REAL minP[3], minQ[3], mindd;
int shown_disjoint = 0;
mindd = VdistV2(S[0],T[0]) + 1; // Set first minimum safely high
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
// Find closest points on edges i & j, plus the
// vector (and distance squared) between these points
SegPoints(VEC,P,Q,S[i],Sv[i],T[j],Tv[j]);
VmV(V,Q,P);
PQP_REAL dd = VdotV(V,V);
// Verify this closest point pair only if the distance
// squared is less than the minimum found thus far.
if (dd <= mindd)
{
VcV(minP,P);
VcV(minQ,Q);
mindd = dd;
VmV(Z,S[(i+2)%3],P);
PQP_REAL a = VdotV(Z,VEC);
VmV(Z,T[(j+2)%3],Q);
PQP_REAL b = VdotV(Z,VEC);
if ((a <= 0) && (b >= 0)) return sqrt(dd);
PQP_REAL p = VdotV(V, VEC);
if (a < 0) a = 0;
if (b > 0) b = 0;
if ((p - a + b) > 0) shown_disjoint = 1;
}
}
}
// No edge pairs contained the closest points.
// either:
// 1. one of the closest points is a vertex, and the
// other point is interior to a face.
// 2. the triangles are overlapping.
// 3. an edge of one triangle is parallel to the other's face. If
// cases 1 and 2 are not true, then the closest points from the 9
// edge pairs checks above can be taken as closest points for the
// triangles.
// 4. possibly, the triangles were degenerate. When the
// triangle points are nearly colinear or coincident, one
// of above tests might fail even though the edges tested
// contain the closest points.
// First check for case 1
PQP_REAL Sn[3], Snl;
VcrossV(Sn,Sv[0],Sv[1]); // Compute normal to S triangle
Snl = VdotV(Sn,Sn); // Compute square of length of normal
// If cross product is long enough,
if (Snl > 1e-15)
{
// Get projection lengths of T points
PQP_REAL Tp[3];
VmV(V,S[0],T[0]);
Tp[0] = VdotV(V,Sn);
VmV(V,S[0],T[1]);
Tp[1] = VdotV(V,Sn);
VmV(V,S[0],T[2]);
Tp[2] = VdotV(V,Sn);
// If Sn is a separating direction,
// find point with smallest projection
int point = -1;
if ((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0))
{
if (Tp[0] < Tp[1]) point = 0; else point = 1;
if (Tp[2] < Tp[point]) point = 2;
}
else if ((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0))
{
if (Tp[0] > Tp[1]) point = 0; else point = 1;
if (Tp[2] > Tp[point]) point = 2;
}
// If Sn is a separating direction,
if (point >= 0)
{
shown_disjoint = 1;
// Test whether the point found, when projected onto the
// other triangle, lies within the face.
VmV(V,T[point],S[0]);
VcrossV(Z,Sn,Sv[0]);
if (VdotV(V,Z) > 0)
{
VmV(V,T[point],S[1]);
VcrossV(Z,Sn,Sv[1]);
if (VdotV(V,Z) > 0)
{
VmV(V,T[point],S[2]);
VcrossV(Z,Sn,Sv[2]);
if (VdotV(V,Z) > 0)
{
// T[point] passed the test - it's a closest point for
// the T triangle; the other point is on the face of S
VpVxS(P,T[point],Sn,Tp[point]/Snl);
VcV(Q,T[point]);
return sqrt(VdistV2(P,Q));
}
}
}
}
}
PQP_REAL Tn[3], Tnl;
VcrossV(Tn,Tv[0],Tv[1]);
Tnl = VdotV(Tn,Tn);
if (Tnl > 1e-15)
{
PQP_REAL Sp[3];
VmV(V,T[0],S[0]);
Sp[0] = VdotV(V,Tn);
VmV(V,T[0],S[1]);
Sp[1] = VdotV(V,Tn);
VmV(V,T[0],S[2]);
Sp[2] = VdotV(V,Tn);
int point = -1;
if ((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0))
{
if (Sp[0] < Sp[1]) point = 0; else point = 1;
if (Sp[2] < Sp[point]) point = 2;
}
else if ((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0))
{
if (Sp[0] > Sp[1]) point = 0; else point = 1;
if (Sp[2] > Sp[point]) point = 2;
}
if (point >= 0)
{
shown_disjoint = 1;
VmV(V,S[point],T[0]);
VcrossV(Z,Tn,Tv[0]);
if (VdotV(V,Z) > 0)
{
VmV(V,S[point],T[1]);
VcrossV(Z,Tn,Tv[1]);
if (VdotV(V,Z) > 0)
{
VmV(V,S[point],T[2]);
VcrossV(Z,Tn,Tv[2]);
if (VdotV(V,Z) > 0)
{
VcV(P,S[point]);
VpVxS(Q,S[point],Tn,Sp[point]/Tnl);
return sqrt(VdistV2(P,Q));
}
}
}
}
}
// Case 1 can't be shown.
// If one of these tests showed the triangles disjoint,
// we assume case 3 or 4, otherwise we conclude case 2,
// that the triangles overlap.
if (shown_disjoint)
{
VcV(P,minP);
VcV(Q,minQ);
return sqrt(mindd);
}
else return 0;
}
int
PQP_Distance(PQP_DistanceResult *res,
PQP_REAL R1[3][3], PQP_REAL T1[3], PQP_Model *o1,
PQP_REAL R2[3][3], PQP_REAL T2[3], PQP_Model *o2,
PQP_REAL rel_err, PQP_REAL abs_err,
int qsize)
{
double time1 = GetTime();
// make sure that the models are built
if (o1->build_state != PQP_BUILD_STATE_PROCESSED)
return PQP_ERR_UNPROCESSED_MODEL;
if (o2->build_state != PQP_BUILD_STATE_PROCESSED)
return PQP_ERR_UNPROCESSED_MODEL;
// Okay, compute what transform [R,T] that takes us from cs2 to cs1.
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
// First compute the rotation part, then translation part
MTxM(res->R,R1,R2);
PQP_REAL Ttemp[3];
VmV(Ttemp, T2, T1);
MTxV(res->T, R1, Ttemp);
// establish initial upper bound using last triangles which
// provided the minimum distance
PQP_REAL p[3],q[3];
res->distance = TriDistance(res->R,res->T,o1->last_tri,o2->last_tri,p,q);
VcV(res->p1,p);
VcV(res->p2,q);
// initialize error bounds
res->abs_err = abs_err;
res->rel_err = rel_err;
// clear the stats
res->num_bv_tests = 0;
res->num_tri_tests = 0;
// compute the transform from o1->child(0) to o2->child(0)
PQP_REAL Rtemp[3][3], R[3][3], T[3];
MxM(Rtemp,res->R,o2->child(0)->R);
MTxM(R,o1->child(0)->R,Rtemp);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(Ttemp,res->R,o2->child(0)->Tr,res->T);
VmV(Ttemp,Ttemp,o1->child(0)->Tr);
#else
MxVpV(Ttemp,res->R,o2->child(0)->To,res->T);
VmV(Ttemp,Ttemp,o1->child(0)->To);
#endif
MTxV(T,o1->child(0)->R,Ttemp);
// choose routine according to queue size
if (qsize <= 2)
{
DistanceRecurse(res,R,T,o1,0,o2,0);
}
else
{
res->qsize = qsize;
DistanceQueueRecurse(res,R,T,o1,0,o2,0);
}
// res->p2 is in cs 1 ; transform it to cs 2
PQP_REAL u[3];
VmV(u, res->p2, res->T);
MTxV(res->p2, res->R, u);
double time2 = GetTime();
res->query_time_secs = time2 - time1;
return PQP_OK;
}
void
DistanceQueueRecurse(PQP_DistanceResult *res,
PQP_REAL R[3][3], PQP_REAL T[3],
PQP_Model *o1, int b1,
PQP_Model *o2, int b2)
{
BVTQ bvtq(res->qsize);
BVT min_test;
min_test.b1 = b1;
min_test.b2 = b2;
McM(min_test.R,R);
VcV(min_test.T,T);
while(1)
{
int l1 = o1->child(min_test.b1)->Leaf();
int l2 = o2->child(min_test.b2)->Leaf();
if (l1 && l2)
{
// both leaves. Test the triangles beneath them.
res->num_tri_tests++;
PQP_REAL p[3], q[3];
Tri *t1 = &o1->tris[-o1->child(min_test.b1)->first_child - 1];
Tri *t2 = &o2->tris[-o2->child(min_test.b2)->first_child - 1];
PQP_REAL d = TriDistance(res->R,res->T,t1,t2,p,q);
if (d < res->distance)
{
res->distance = d;
VcV(res->p1, p); // p already in c.s. 1
VcV(res->p2, q); // q must be transformed
// into c.s. 2 later
o1->last_tri = t1;
o2->last_tri = t2;
}
}
else if (bvtq.GetNumTests() == bvtq.GetSize() - 1)
{
// queue can't get two more tests, recur
DistanceQueueRecurse(res,min_test.R,min_test.T,
o1,min_test.b1,o2,min_test.b2);
}
else
{
// decide how to descend to children
PQP_REAL sz1 = o1->child(min_test.b1)->GetSize();
PQP_REAL sz2 = o2->child(min_test.b2)->GetSize();
res->num_bv_tests += 2;
BVT bvt1,bvt2;
PQP_REAL Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
// put new tests on queue consisting of min_test.b2
// with children of min_test.b1
int c1 = o1->child(min_test.b1)->first_child;
int c2 = c1 + 1;
// init bv test 1
bvt1.b1 = c1;
bvt1.b2 = min_test.b2;
MTxM(bvt1.R,o1->child(c1)->R,min_test.R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,min_test.T,o1->child(c1)->Tr);
#else
VmV(Ttemp,min_test.T,o1->child(c1)->To);
#endif
MTxV(bvt1.T,o1->child(c1)->R,Ttemp);
bvt1.d = BV_Distance(bvt1.R,bvt1.T,
o1->child(bvt1.b1),o2->child(bvt1.b2));
// init bv test 2
bvt2.b1 = c2;
bvt2.b2 = min_test.b2;
MTxM(bvt2.R,o1->child(c2)->R,min_test.R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,min_test.T,o1->child(c2)->Tr);
#else
VmV(Ttemp,min_test.T,o1->child(c2)->To);
#endif
MTxV(bvt2.T,o1->child(c2)->R,Ttemp);
bvt2.d = BV_Distance(bvt2.R,bvt2.T,
o1->child(bvt2.b1),o2->child(bvt2.b2));
}
else
{
// put new tests on queue consisting of min_test.b1
// with children of min_test.b2
int c1 = o2->child(min_test.b2)->first_child;
int c2 = c1 + 1;
// init bv test 1
bvt1.b1 = min_test.b1;
bvt1.b2 = c1;
MxM(bvt1.R,min_test.R,o2->child(c1)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(bvt1.T,min_test.R,o2->child(c1)->Tr,min_test.T);
#else
MxVpV(bvt1.T,min_test.R,o2->child(c1)->To,min_test.T);
#endif
bvt1.d = BV_Distance(bvt1.R,bvt1.T,
o1->child(bvt1.b1),o2->child(bvt1.b2));
// init bv test 2
bvt2.b1 = min_test.b1;
bvt2.b2 = c2;
MxM(bvt2.R,min_test.R,o2->child(c2)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(bvt2.T,min_test.R,o2->child(c2)->Tr,min_test.T);
#else
MxVpV(bvt2.T,min_test.R,o2->child(c2)->To,min_test.T);
#endif
bvt2.d = BV_Distance(bvt2.R,bvt2.T,
o1->child(bvt2.b1),o2->child(bvt2.b2));
}
bvtq.AddTest(bvt1);
bvtq.AddTest(bvt2);
}
if (bvtq.Empty())
{
break;
}
else
{
min_test = bvtq.ExtractMinTest();
if ((min_test.d + res->abs_err >= res->distance) &&
((min_test.d * (1 + res->rel_err)) >= res->distance))
{
break;
}
}
}
}
void
DistanceRecurse(PQP_DistanceResult *res,
PQP_REAL R[3][3], PQP_REAL T[3], // b2 relative to b1
PQP_Model *o1, int b1,
PQP_Model *o2, int b2)
{
PQP_REAL sz1 = o1->child(b1)->GetSize();
PQP_REAL sz2 = o2->child(b2)->GetSize();
int l1 = o1->child(b1)->Leaf();
int l2 = o2->child(b2)->Leaf();
if (l1 && l2)
{
// both leaves. Test the triangles beneath them.
res->num_tri_tests++;
PQP_REAL p[3], q[3];
Tri *t1 = &o1->tris[-o1->child(b1)->first_child - 1];
Tri *t2 = &o2->tris[-o2->child(b2)->first_child - 1];
PQP_REAL d = TriDistance(res->R,res->T,t1,t2,p,q);
if (d < res->distance)
{
res->distance = d;
VcV(res->p1, p); // p already in c.s. 1
VcV(res->p2, q); // q must be transformed
// into c.s. 2 later
o1->last_tri = t1;
o2->last_tri = t2;
}
return;
}
// First, perform distance tests on the children. Then traverse
// them recursively, but test the closer pair first, the further
// pair second.
int a1,a2,c1,c2; // new bv tests 'a' and 'c'
PQP_REAL R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
// visit the children of b1
a1 = o1->child(b1)->first_child;
a2 = b2;
c1 = o1->child(b1)->first_child+1;
c2 = b2;
MTxM(R1,o1->child(a1)->R,R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,T,o1->child(a1)->Tr);
#else
VmV(Ttemp,T,o1->child(a1)->To);
#endif
MTxV(T1,o1->child(a1)->R,Ttemp);
MTxM(R2,o1->child(c1)->R,R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,T,o1->child(c1)->Tr);
#else
VmV(Ttemp,T,o1->child(c1)->To);
#endif
MTxV(T2,o1->child(c1)->R,Ttemp);
}
else
{
// visit the children of b2
a1 = b1;
a2 = o2->child(b2)->first_child;
c1 = b1;
c2 = o2->child(b2)->first_child+1;
MxM(R1,R,o2->child(a2)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(T1,R,o2->child(a2)->Tr,T);
#else
MxVpV(T1,R,o2->child(a2)->To,T);
#endif
MxM(R2,R,o2->child(c2)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(T2,R,o2->child(c2)->Tr,T);
#else
MxVpV(T2,R,o2->child(c2)->To,T);
#endif
}
res->num_bv_tests += 2;
PQP_REAL d1 = BV_Distance(R1, T1, o1->child(a1), o2->child(a2));
PQP_REAL d2 = BV_Distance(R2, T2, o1->child(c1), o2->child(c2));
if (d2 < d1)
{
if ((d2 < (res->distance - res->abs_err)) ||
(d2*(1 + res->rel_err) < res->distance))
{
DistanceRecurse(res, R2, T2, o1, c1, o2, c2);
}
if ((d1 < (res->distance - res->abs_err)) ||
(d1*(1 + res->rel_err) < res->distance))
{
DistanceRecurse(res, R1, T1, o1, a1, o2, a2);
}
}
else
{
if ((d1 < (res->distance - res->abs_err)) ||
(d1*(1 + res->rel_err) < res->distance))
{
DistanceRecurse(res, R1, T1, o1, a1, o2, a2);
}
if ((d2 < (res->distance - res->abs_err)) ||
(d2*(1 + res->rel_err) < res->distance))
{
DistanceRecurse(res, R2, T2, o1, c1, o2, c2);
}
}
}
void
ToleranceQueueRecurse(PQP_ToleranceResult *res,
PQP_REAL R[3][3], PQP_REAL T[3],
PQP_Model *o1, int b1,
PQP_Model *o2, int b2)
{
BVTQ bvtq(res->qsize);
BVT min_test;
min_test.b1 = b1;
min_test.b2 = b2;
McM(min_test.R,R);
VcV(min_test.T,T);
while(1)
{
int l1 = o1->child(min_test.b1)->Leaf();
int l2 = o2->child(min_test.b2)->Leaf();
if (l1 && l2)
{
// both leaves - find if tri pair within tolerance
res->num_tri_tests++;
PQP_REAL p[3], q[3];
Tri *t1 = &o1->tris[-o1->child(min_test.b1)->first_child - 1];
Tri *t2 = &o2->tris[-o2->child(min_test.b2)->first_child - 1];
PQP_REAL d = TriDistance(res->R,res->T,t1,t2,p,q);
if (d <= res->tolerance)
{
// triangle pair distance less than tolerance
res->closer_than_tolerance = 1;
res->distance = d;
VcV(res->p1, p); // p already in c.s. 1
VcV(res->p2, q); // q must be transformed
// into c.s. 2 later
return;
}
}
else if (bvtq.GetNumTests() == bvtq.GetSize() - 1)
{
// queue can't get two more tests, recur
ToleranceQueueRecurse(res,min_test.R,min_test.T,
o1,min_test.b1,o2,min_test.b2);
if (res->closer_than_tolerance == 1) return;
}
else
{
// decide how to descend to children
PQP_REAL sz1 = o1->child(min_test.b1)->GetSize();
PQP_REAL sz2 = o2->child(min_test.b2)->GetSize();
res->num_bv_tests += 2;
BVT bvt1,bvt2;
PQP_REAL Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
// add two new tests to queue, consisting of min_test.b2
// with the children of min_test.b1
int c1 = o1->child(min_test.b1)->first_child;
int c2 = c1 + 1;
// init bv test 1
bvt1.b1 = c1;
bvt1.b2 = min_test.b2;
MTxM(bvt1.R,o1->child(c1)->R,min_test.R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,min_test.T,o1->child(c1)->Tr);
#else
VmV(Ttemp,min_test.T,o1->child(c1)->To);
#endif
MTxV(bvt1.T,o1->child(c1)->R,Ttemp);
bvt1.d = BV_Distance(bvt1.R,bvt1.T,
o1->child(bvt1.b1),o2->child(bvt1.b2));
// init bv test 2
bvt2.b1 = c2;
bvt2.b2 = min_test.b2;
MTxM(bvt2.R,o1->child(c2)->R,min_test.R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,min_test.T,o1->child(c2)->Tr);
#else
VmV(Ttemp,min_test.T,o1->child(c2)->To);
#endif
MTxV(bvt2.T,o1->child(c2)->R,Ttemp);
bvt2.d = BV_Distance(bvt2.R,bvt2.T,
o1->child(bvt2.b1),o2->child(bvt2.b2));
}
else
{
// add two new tests to queue, consisting of min_test.b1
// with the children of min_test.b2
int c1 = o2->child(min_test.b2)->first_child;
int c2 = c1 + 1;
// init bv test 1
bvt1.b1 = min_test.b1;
bvt1.b2 = c1;
MxM(bvt1.R,min_test.R,o2->child(c1)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(bvt1.T,min_test.R,o2->child(c1)->Tr,min_test.T);
#else
MxVpV(bvt1.T,min_test.R,o2->child(c1)->To,min_test.T);
#endif
bvt1.d = BV_Distance(bvt1.R,bvt1.T,
o1->child(bvt1.b1),o2->child(bvt1.b2));
// init bv test 2
bvt2.b1 = min_test.b1;
bvt2.b2 = c2;
MxM(bvt2.R,min_test.R,o2->child(c2)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(bvt2.T,min_test.R,o2->child(c2)->Tr,min_test.T);
#else
MxVpV(bvt2.T,min_test.R,o2->child(c2)->To,min_test.T);
#endif
bvt2.d = BV_Distance(bvt2.R,bvt2.T,
o1->child(bvt2.b1),o2->child(bvt2.b2));
}
// put children tests in queue
if (bvt1.d <= res->tolerance) bvtq.AddTest(bvt1);
if (bvt2.d <= res->tolerance) bvtq.AddTest(bvt2);
}
if (bvtq.Empty() || (bvtq.MinTest() > res->tolerance))
{
res->closer_than_tolerance = 0;
return;
}
else
{
min_test = bvtq.ExtractMinTest();
}
}
}
// Tolerance Stuff
//
//---------------------------------------------------------------------------
void
ToleranceRecurse(PQP_ToleranceResult *res,
PQP_REAL R[3][3], PQP_REAL T[3],
PQP_Model *o1, int b1, PQP_Model *o2, int b2)
{
PQP_REAL sz1 = o1->child(b1)->GetSize();
PQP_REAL sz2 = o2->child(b2)->GetSize();
int l1 = o1->child(b1)->Leaf();
int l2 = o2->child(b2)->Leaf();
if (l1 && l2)
{
// both leaves - find if tri pair within tolerance
res->num_tri_tests++;
PQP_REAL p[3], q[3];
Tri *t1 = &o1->tris[-o1->child(b1)->first_child - 1];
Tri *t2 = &o2->tris[-o2->child(b2)->first_child - 1];
PQP_REAL d = TriDistance(res->R,res->T,t1,t2,p,q);
if (d <= res->tolerance)
{
// triangle pair distance less than tolerance
res->closer_than_tolerance = 1;
res->distance = d;
VcV(res->p1, p); // p already in c.s. 1
VcV(res->p2, q); // q must be transformed
// into c.s. 2 later
}
return;
}
int a1,a2,c1,c2; // new bv tests 'a' and 'c'
PQP_REAL R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
// visit the children of b1
a1 = o1->child(b1)->first_child;
a2 = b2;
c1 = o1->child(b1)->first_child+1;
c2 = b2;
MTxM(R1,o1->child(a1)->R,R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,T,o1->child(a1)->Tr);
#else
VmV(Ttemp,T,o1->child(a1)->To);
#endif
MTxV(T1,o1->child(a1)->R,Ttemp);
MTxM(R2,o1->child(c1)->R,R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,T,o1->child(c1)->Tr);
#else
VmV(Ttemp,T,o1->child(c1)->To);
#endif
MTxV(T2,o1->child(c1)->R,Ttemp);
}
else
{
// visit the children of b2
a1 = b1;
a2 = o2->child(b2)->first_child;
c1 = b1;
c2 = o2->child(b2)->first_child+1;
MxM(R1,R,o2->child(a2)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(T1,R,o2->child(a2)->Tr,T);
#else
MxVpV(T1,R,o2->child(a2)->To,T);
#endif
MxM(R2,R,o2->child(c2)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(T2,R,o2->child(c2)->Tr,T);
#else
MxVpV(T2,R,o2->child(c2)->To,T);
#endif
}
res->num_bv_tests += 2;
PQP_REAL d1 = BV_Distance(R1, T1, o1->child(a1), o2->child(a2));
PQP_REAL d2 = BV_Distance(R2, T2, o1->child(c1), o2->child(c2));
if (d2 < d1)
{
if (d2 <= res->tolerance) ToleranceRecurse(res, R2, T2, o1, c1, o2, c2);
if (res->closer_than_tolerance) return;
if (d1 <= res->tolerance) ToleranceRecurse(res, R1, T1, o1, a1, o2, a2);
}
else
{
if (d1 <= res->tolerance) ToleranceRecurse(res, R1, T1, o1, a1, o2, a2);
if (res->closer_than_tolerance) return;
if (d2 <= res->tolerance) ToleranceRecurse(res, R2, T2, o1, c1, o2, c2);
}
}
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();
}
void
DisplayCB()
{
BeginDraw();
// set up model transformations
if (animate)
{
rot1 += .1;
rot2 += .2;
rot3 += .3;
}
PQP_REAL R1[3][3],R2[3][3],T1[3],T2[3];
PQP_REAL M1[3][3],M2[3][3],M3[3][3];
T1[0] = -1;
T1[1] = 0.0;
T1[2] = 0.0;
T2[0] = 1;
T2[1] = 0.0;
T2[2] = 0.0;
MRotX(M1,rot1);
MRotY(M2,rot2);
MxM(M3,M1,M2);
MRotZ(M1,rot3);
MxM(R1,M3,M1);
MRotX(M1,rot3);
MRotY(M2,rot1);
MxM(M3,M1,M2);
MRotZ(M1,rot2);
MxM(R2,M3,M1);
// perform distance query
PQP_REAL rel_err = 0.0;
PQP_REAL abs_err = 0.0;
PQP_DistanceResult res;
PQP_Distance(&res,R1,T1,&bunny,R2,T2,&torus,rel_err,abs_err);
// draw the models
glColor3d(0.0,0.0,1.0);
double oglm[16];
MVtoOGL(oglm,R1,T1);
glPushMatrix();
glMultMatrixd(oglm);
bunny_to_draw->Draw();
glPopMatrix();
glColor3d(0.0,1.0,0.0);
MVtoOGL(oglm,R2,T2);
glPushMatrix();
glMultMatrixd(oglm);
torus_to_draw->Draw();
glPopMatrix();
// draw the closest points as small spheres
glColor3d(1.0,0.0,0.0);
PQP_REAL P1[3],P2[3],V1[3],V2[3];
VcV(P1,res.P1());
VcV(P2,res.P2());
// each point is in the space of its model;
// transform to world space
MxVpV(V1,R1,P1,T1);
/*
glPushMatrix();
glTranslated(V1[0],V1[1],V1[2]);
glutSolidSphere(.05,15,15);
glPopMatrix();
*/
MxVpV(V2,R2,P2,T2);
/*
glPushMatrix();
glTranslated(V2[0],V2[1],V2[2]);
glutSolidSphere(.05,15,15);
glPopMatrix();
*/
// draw the line between the closest points
float v1p[3], v2p[3];
for (int i = 0; i < 3; ++i) {
v1p[i] = V1[i];
v2p[i] = V2[i];
}
//glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3v(v1p);
glVertex3v(v2p);
glEnd();
//glEnable(GL_LIGHTING);
EndDraw();
}
