int
PQP_Collide(PQP_CollideResult *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,
int flag)
{
double t1 = 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;
// clear the stats
res->num_bv_tests = 0;
res->num_tri_tests = 0;
// don't release the memory, but reset the num_pairs counter
res->num_pairs = 0;
// Okay, compute what transform [R,T] that takes us from cs1 to cs2.
// [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);
// 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 & OBB_TYPE
MxVpV(Ttemp,res->R,o2->child(0)->To,res->T);
VmV(Ttemp,Ttemp,o1->child(0)->To);
#else
MxVpV(Ttemp,res->R,o2->child(0)->Tr,res->T);
VmV(Ttemp,Ttemp,o1->child(0)->Tr);
#endif
MTxV(T,o1->child(0)->R,Ttemp);
// now start with both top level BVs
CollideRecurse(res,R,T,o1,0,o2,0,flag);
double t2 = GetTime();
res->query_time_secs = t2 - t1;
return PQP_OK;
}
inline
void
compute_moment(moment &M, PQP_REAL p[3], PQP_REAL q[3], PQP_REAL r[3])
{
PQP_REAL u[3], v[3], w[3];
// compute the area of the triangle
VmV(u, q, p);
VmV(v, r, p);
VcrossV(w, u, v);
M.A = 0.5 * Vlength(w);
if (M.A == 0.0)
{
// This triangle has zero area. The second order components
// would be eliminated with the usual formula, so, for the
// sake of robustness we use an alternative form. These are the
// centroid and second-order components of the triangle's vertices.
// centroid
M.m[0] = (p[0] + q[0] + r[0]) /3;
M.m[1] = (p[1] + q[1] + r[1]) /3;
M.m[2] = (p[2] + q[2] + r[2]) /3;
// second-order components
M.s[0][0] = (p[0]*p[0] + q[0]*q[0] + r[0]*r[0]);
M.s[0][1] = (p[0]*p[1] + q[0]*q[1] + r[0]*r[1]);
M.s[0][2] = (p[0]*p[2] + q[0]*q[2] + r[0]*r[2]);
M.s[1][1] = (p[1]*p[1] + q[1]*q[1] + r[1]*r[1]);
M.s[1][2] = (p[1]*p[2] + q[1]*q[2] + r[1]*r[2]);
M.s[2][2] = (p[2]*p[2] + q[2]*q[2] + r[2]*r[2]);
M.s[2][1] = M.s[1][2];
M.s[1][0] = M.s[0][1];
M.s[2][0] = M.s[0][2];
return;
}
// get the centroid
M.m[0] = (p[0] + q[0] + r[0])/3;
M.m[1] = (p[1] + q[1] + r[1])/3;
M.m[2] = (p[2] + q[2] + r[2])/3;
// get the second order components -- note the weighting by the area
M.s[0][0] = M.A*(9*M.m[0]*M.m[0]+p[0]*p[0]+q[0]*q[0]+r[0]*r[0])/12;
M.s[0][1] = M.A*(9*M.m[0]*M.m[1]+p[0]*p[1]+q[0]*q[1]+r[0]*r[1])/12;
M.s[1][1] = M.A*(9*M.m[1]*M.m[1]+p[1]*p[1]+q[1]*q[1]+r[1]*r[1])/12;
M.s[0][2] = M.A*(9*M.m[0]*M.m[2]+p[0]*p[2]+q[0]*q[2]+r[0]*r[2])/12;
M.s[1][2] = M.A*(9*M.m[1]*M.m[2]+p[1]*p[2]+q[1]*q[2]+r[1]*r[2])/12;
M.s[2][2] = M.A*(9*M.m[2]*M.m[2]+p[2]*p[2]+q[2]*q[2]+r[2]*r[2])/12;
M.s[2][1] = M.s[1][2];
M.s[1][0] = M.s[0][1];
M.s[2][0] = M.s[0][2];
}
void
make_parent_relative(PQP_Model *m, int bn,
const PQP_REAL parentR[3][3]
#if PQP_BV_TYPE & RSS_TYPE
,const PQP_REAL parentTr[3]
#endif
#if PQP_BV_TYPE & OBB_TYPE
,const PQP_REAL parentTo[3]
#endif
)
{
PQP_REAL Rpc[3][3], Tpc[3];
if (!m->child(bn)->Leaf())
{
// make children parent-relative
make_parent_relative(m,m->child(bn)->first_child,
m->child(bn)->R
#if PQP_BV_TYPE & RSS_TYPE
,m->child(bn)->Tr
#endif
#if PQP_BV_TYPE & OBB_TYPE
,m->child(bn)->To
#endif
);
make_parent_relative(m,m->child(bn)->first_child+1,
m->child(bn)->R
#if PQP_BV_TYPE & RSS_TYPE
,m->child(bn)->Tr
#endif
#if PQP_BV_TYPE & OBB_TYPE
,m->child(bn)->To
#endif
);
}
// make self parent relative
MTxM(Rpc,parentR,m->child(bn)->R);
McM(m->child(bn)->R,Rpc);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Tpc,m->child(bn)->Tr,parentTr);
MTxV(m->child(bn)->Tr,parentR,Tpc);
#endif
#if PQP_BV_TYPE & OBB_TYPE
VmV(Tpc,m->child(bn)->To,parentTo);
MTxV(m->child(bn)->To,parentR,Tpc);
#endif
}
Model::Model(char *tris_file)
{
FILE *fp = fopen(tris_file,"r");
if (fp == NULL)
{
fprintf(stderr,"Model Constructor: Couldn't open %s\n",tris_file);
exit(-1);
}
fscanf(fp,"%d",&ntris);
tri = new ModelTri[ntris];
int i;
for (i = 0; i < ntris; i++)
{
// read the tri verts
fscanf(fp,"%lf %lf %lf %lf %lf %lf %lf %lf %lf",
&tri[i].p0[0], &tri[i].p0[1], &tri[i].p0[2],
&tri[i].p1[0], &tri[i].p1[1], &tri[i].p1[2],
&tri[i].p2[0], &tri[i].p2[1], &tri[i].p2[2]);
// set the normal
double a[3],b[3];
VmV(a,tri[i].p1,tri[i].p0);
VmV(b,tri[i].p2,tri[i].p0);
VcrossV(tri[i].n,a,b);
Vnormalize(tri[i].n);
}
fclose(fp);
// generate display list
display_list = glGenLists(1);
glNewList(display_list,GL_COMPILE);
glBegin(GL_TRIANGLES);
for (i = 0; i < ntris; i++)
{
glNormal3dv(tri[i].n);
glVertex3dv(tri[i].p0);
glVertex3dv(tri[i].p1);
glVertex3dv(tri[i].p2);
}
glEnd();
glEndList();
}
//sets all the members of Tri3B given the values of the vertices
void Tri3B::Set( const PQP_REAL *pt1, const PQP_REAL *pt2,
const PQP_REAL *pt3, int iden )
{
id = iden;
//vertices of triangle in model's coord. frame
p1[0] = pt1[0];
p1[1] = pt1[1];
p1[2] = pt1[2];
p2[0] = pt2[0];
p2[1] = pt2[1];
p2[2] = pt2[2];
p3[0] = pt3[0];
p3[1] = pt3[1];
p3[2] = pt3[2];
//normalized vectors along side of triangle
VmV( s12, p2, p1 );
Vnormalize( s12 );
VmV( s23, p3, p2 );
Vnormalize( s23 );
VmV( s31, p1, p3 );
Vnormalize( s31 );
//normal vector to plane of triangle
VcrossV( n, s31, s12 );
Vnormalize( n );
//normal vectors to each side in the plane of the triangle
//from cross product of side-vector and normal
VcrossV( ns12, s12, n );
Vnormalize( ns12 );
VcrossV( ns23, s23, n );
Vnormalize( ns23 );
VcrossV( ns31, s31, n );
Vnormalize( ns31 );
}
REAL computeDistance( const REAL * bv1_m_center, const REAL * bv2_m_center, const REAL bv1_m_rad, const REAL bv2_m_rad, REAL * rot, const REAL * trans)
{
#pragma HLS INLINE recursive
REAL T[3];
REAL Ttemp[3];
VmV(Ttemp,trans,bv1_m_center);
MxVpV(T, rot, bv2_m_center, Ttemp);
return 1.0;
return Vlength(T) - (bv1_m_rad + bv2_m_rad);
}
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
CollideRecurse(PQP_CollideResult *res,
PQP_REAL R[3][3], PQP_REAL T[3], // b2 relative to b1
PQP_Model *o1, int b1,
PQP_Model *o2, int b2, int flag)
{
// first thing, see if we're overlapping
res->num_bv_tests++;
if (!BV_Overlap(R, T, o1->child(b1), o2->child(b2))) return;
// if we are, see if we test triangles next
int l1 = o1->child(b1)->Leaf();
int l2 = o2->child(b2)->Leaf();
if (l1 && l2)
{
res->num_tri_tests++;
#if 1
// transform the points in b2 into space of b1, then compare
Tri *t1 = &o1->tris[-o1->child(b1)->first_child - 1];
Tri *t2 = &o2->tris[-o2->child(b2)->first_child - 1];
PQP_REAL q1[3], q2[3], q3[3];
PQP_REAL *p1 = t1->p1;
PQP_REAL *p2 = t1->p2;
PQP_REAL *p3 = t1->p3;
MxVpV(q1, res->R, t2->p1, res->T);
MxVpV(q2, res->R, t2->p2, res->T);
MxVpV(q3, res->R, t2->p3, res->T);
if (TriContact(p1, p2, p3, q1, q2, q3))
{
// add this to result
res->Add(t1->id, t2->id);
}
#else
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];
if (TriDistance(res->R,res->T,t1,t2,p,q) == 0.0)
{
// add this to result
res->Add(t1->id, t2->id);
}
#endif
return;
}
// we dont, so decide whose children to visit next
PQP_REAL sz1 = o1->child(b1)->GetSize();
PQP_REAL sz2 = o2->child(b2)->GetSize();
PQP_REAL Rc[3][3],Tc[3],Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
int c1 = o1->child(b1)->first_child;
int c2 = c1 + 1;
MTxM(Rc,o1->child(c1)->R,R);
#if PQP_BV_TYPE & OBB_TYPE
VmV(Ttemp,T,o1->child(c1)->To);
#else
VmV(Ttemp,T,o1->child(c1)->Tr);
#endif
MTxV(Tc,o1->child(c1)->R,Ttemp);
CollideRecurse(res,Rc,Tc,o1,c1,o2,b2,flag);
if ((flag == PQP_FIRST_CONTACT) && (res->num_pairs > 0)) return;
MTxM(Rc,o1->child(c2)->R,R);
#if PQP_BV_TYPE & OBB_TYPE
VmV(Ttemp,T,o1->child(c2)->To);
#else
VmV(Ttemp,T,o1->child(c2)->Tr);
#endif
MTxV(Tc,o1->child(c2)->R,Ttemp);
CollideRecurse(res,Rc,Tc,o1,c2,o2,b2,flag);
}
else
{
int c1 = o2->child(b2)->first_child;
int c2 = c1 + 1;
MxM(Rc,R,o2->child(c1)->R);
#if PQP_BV_TYPE & OBB_TYPE
MxVpV(Tc,R,o2->child(c1)->To,T);
#else
MxVpV(Tc,R,o2->child(c1)->Tr,T);
#endif
CollideRecurse(res,Rc,Tc,o1,b1,o2,c1,flag);
if ((flag == PQP_FIRST_CONTACT) && (res->num_pairs > 0)) return;
MxM(Rc,R,o2->child(c2)->R);
#if PQP_BV_TYPE & OBB_TYPE
MxVpV(Tc,R,o2->child(c2)->To,T);
#else
MxVpV(Tc,R,o2->child(c2)->Tr,T);
#endif
CollideRecurse(res,Rc,Tc,o1,b1,o2,c2,flag);
}
}
int
PQP_Tolerance(PQP_ToleranceResult *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 tolerance,
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;
// Compute the 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)]
MTxM(res->R,R1,R2);
PQP_REAL Ttemp[3];
VmV(Ttemp, T2, T1);
MTxV(res->T, R1, Ttemp);
// set tolerance, used to prune the search
if (tolerance < 0.0) tolerance = 0.0;
res->tolerance = tolerance;
// clear the stats
res->num_bv_tests = 0;
res->num_tri_tests = 0;
// initially assume not closer than tolerance
res->closer_than_tolerance = 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);
// find a distance lower bound for trivial reject
PQP_REAL d = BV_Distance(R, T, o1->child(0), o2->child(0));
if (d <= res->tolerance)
{
// more work needed - choose routine according to queue size
if (qsize <= 2)
{
ToleranceRecurse(res, R, T, o1, 0, o2, 0);
}
else
{
res->qsize = qsize;
ToleranceQueueRecurse(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;
}
// 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 computeDistances(int size1, int size2, int CLeafType, int pNodeType, REAL * CLeaf_next_m_positions, REAL * CLeaf_m_translate, REAL * CLeaf_m_rotate, REAL * pNode_next_m_positions, REAL * pNode_m_translate, REAL * pNode_m_rotate, REAL * pNode_m_positions, REAL * CLeaf_m_positions, REAL * CLeaf_m_distances, REAL * rot, const REAL * trans){
#pragma HLS INLINE recursive
REAL cen[3], dist[3], vec1[3], vec2[3];
// If this is a PRO backbone, we need to add the Cd atom because
// it is part of a group with the Ca atom for Electrostatic purposes
if (CLeafType == BBP)
{
//assert(getNext());
//assert(getNext()->getType() == PRO);
MxVpV(vec1, CLeaf_m_rotate, CLeaf_next_m_positions + 6, CLeaf_m_translate); // 6 = i = 2
}
// If this is a PRO backbone, we need to add the Cd atom because
// it is part of a group with the Ca atom for Electrostatic purposes
if (pNodeType == BBP)
{
//assert(pLeaf->getNext());
//assert(pLeaf->getNext()->getType() == PRO);
REAL temp[3];
MxVpV(temp, pNode_m_rotate, pNode_next_m_positions + 6,
pNode_m_translate);
MxVpV(vec2, rot,temp, trans);
}
// Compute te distances between all pairs of atoms.
for(int j = 0; j < size2; j++)
{
//REAL *pNode_m_positions_p[3];
//for(int i = 0; i < 3; i++) pNode_m_positions_p[i] = TODO CHECK OTHER DIMENSION....
//MxVpV(cen, rot, pNode_m_positions[j], trans);
MxVpV(cen, rot, pNode_m_positions+ j * ROW, trans);
for (int i = 0; i < size1; i++)
{
VmV(dist, cen, CLeaf_m_positions + i*ROW);
CLeaf_m_distances[i * MAX_ROTAMER_SIZE + j] = Vlength2(dist);
// Add distances to the Cd of the second node (if type is BBP)
if (pNodeType == BBP)
{
VmV(dist, vec2, CLeaf_m_positions + i * ROW);
CLeaf_m_distances[i* MAX_ROTAMER_SIZE + size2] = Vlength2(dist);
}
}
// Add distances to the Cd of the first node (if type is BBP)
if (CLeafType == BBP)
{
VmV(dist, cen, vec1);
CLeaf_m_distances[size1*MAX_ROTAMER_SIZE + j] = Vlength2(dist);
}
}
// Add distance between the Cd of the first and second nodes (both BBPs)
if (CLeafType == BBP && pNodeType == BBP)
{
VmV(dist, vec2, vec1);
CLeaf_m_distances[size1*MAX_ROTAMER_SIZE + size2] = Vlength2(dist);
}
return;
}
int
box::split_recurse(int *t)
{
// For a single triangle, orientation is easily determined.
// The major axis is parallel to the longest edge.
// The minor axis is normal to the triangle.
// The in-between axis is determine by these two.
// this->pR, this->d, and this->pT are set herein.
P = N = 0;
tri *ptr = RAPID_tri + t[0];
// Find the major axis: parallel to the longest edge.
double u12[3], u23[3], u31[3];
// First compute the squared-lengths of each edge
VmV(u12, ptr->p1, ptr->p2);
double d12 = VdotV(u12,u12);
VmV(u23, ptr->p2, ptr->p3);
double d23 = VdotV(u23,u23);
VmV(u31, ptr->p3, ptr->p1);
double d31 = VdotV(u31,u31);
// Find the edge of longest squared-length, normalize it to
// unit length, and put result into a0.
double a0[3];
double l;
if (d12 > d23)
{
if (d12 > d31)
{
l = 1.0 / sqrt(d12);
a0[0] = u12[0] * l;
a0[1] = u12[1] * l;
a0[2] = u12[2] * l;
}
else
{
l = 1.0 / sqrt(d31);
a0[0] = u31[0] * l;
a0[1] = u31[1] * l;
a0[2] = u31[2] * l;
}
}
else
{
if (d23 > d31)
{
l = 1.0 / sqrt(d23);
a0[0] = u23[0] * l;
a0[1] = u23[1] * l;
a0[2] = u23[2] * l;
}
else
{
l = 1.0 / sqrt(d31);
a0[0] = u31[0] * l;
a0[1] = u31[1] * l;
a0[2] = u31[2] * l;
}
}
// Now compute unit normal to triangle, and put into a2.
double a2[3];
VcrossV(a2, u12, u23);
l = 1.0 / Vlength(a2); a2[0] *= l; a2[1] *= l; a2[2] *= l;
// a1 is a2 cross a0.
double a1[3];
VcrossV(a1, a2, a0);
// Now make the columns of this->pR the vectors a0, a1, and a2.
pR[0][0] = a0[0]; pR[0][1] = a1[0]; pR[0][2] = a2[0];
pR[1][0] = a0[1]; pR[1][1] = a1[1]; pR[1][2] = a2[1];
pR[2][0] = a0[2]; pR[2][1] = a1[2]; pR[2][2] = a2[2];
// Now compute the maximum and minimum extents of each vertex
// along each of the box axes. From this we will compute the
// box center and box dimensions.
double minval[3], maxval[3];
double c[3];
MTxV(c, pR, ptr->p1);
minval[0] = maxval[0] = c[0];
minval[1] = maxval[1] = c[1];
minval[2] = maxval[2] = c[2];
MTxV(c, pR, ptr->p2);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
MTxV(c, pR, ptr->p3);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
// With the max and min data, determine the center point and dimensions
// of the box
c[0] = (minval[0] + maxval[0])*0.5;
c[1] = (minval[1] + maxval[1])*0.5;
c[2] = (minval[2] + maxval[2])*0.5;
pT[0] = c[0] * pR[0][0] + c[1] * pR[0][1] + c[2] * pR[0][2];
pT[1] = c[0] * pR[1][0] + c[1] * pR[1][1] + c[2] * pR[1][2];
pT[2] = c[0] * pR[2][0] + c[1] * pR[2][1] + c[2] * pR[2][2];
d[0] = (maxval[0] - minval[0])*0.5;
d[1] = (maxval[1] - minval[1])*0.5;
d[2] = (maxval[2] - minval[2])*0.5;
// Assign the one triangle to this box
trp = ptr;
return RAPID_OK;
}
int
box::split_recurse(int *t, int n)
{
// The orientation for the parent box is already assigned to this->pR.
// The axis along which to split will be column 0 of this->pR.
// The mean point is passed in on this->pT.
// When this routine completes, the position and orientation in model
// space will be established, as well as its dimensions. Child boxes
// will be constructed and placed in the parent's CS.
if (n == 1)
{
return split_recurse(t);
}
// walk along the tris for the box, and do the following:
// 1. collect the max and min of the vertices along the axes of <or>.
// 2. decide which group the triangle goes in, performing appropriate swap.
// 3. accumulate the mean point and covariance data for that triangle.
accum M1, M2;
double C[3][3];
double c[3];
double minval[3], maxval[3];
int rc; // for return code on procedure calls.
int in;
tri *ptr;
int i;
double axdmp;
int n1 = 0; // The number of tris in group 1.
// Group 2 will have n - n1 tris.
// project approximate mean point onto splitting axis, and get coord.
axdmp = (pR[0][0] * pT[0] + pR[1][0] * pT[1] + pR[2][0] * pT[2]);
clear_accum(M1);
clear_accum(M2);
MTxV(c, pR, RAPID_tri[t[0]].p1);
minval[0] = maxval[0] = c[0];
minval[1] = maxval[1] = c[1];
minval[2] = maxval[2] = c[2];
for(i=0; i<n; i++)
{
in = t[i];
ptr = RAPID_tri + in;
MTxV(c, pR, ptr->p1);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
MTxV(c, pR, ptr->p2);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
MTxV(c, pR, ptr->p3);
minmax(minval[0], maxval[0], c[0]);
minmax(minval[1], maxval[1], c[1]);
minmax(minval[2], maxval[2], c[2]);
// grab the mean point of the in'th triangle, project
// it onto the splitting axis (1st column of pR) and
// see where it lies with respect to axdmp.
mean_from_moment(c, RAPID_moment[in]);
if (((pR[0][0]*c[0] + pR[1][0]*c[1] + pR[2][0]*c[2]) < axdmp)
&& ((n!=2)) || ((n==2) && (i==0)))
{
// accumulate first and second order moments for group 1
accum_moment(M1, RAPID_moment[in]);
// put it in group 1 by swapping t[i] with t[n1]
int temp = t[i];
t[i] = t[n1];
t[n1] = temp;
n1++;
}
else
{
// accumulate first and second order moments for group 2
accum_moment(M2, RAPID_moment[in]);
// leave it in group 2
// do nothing...it happens by default
}
}
// done using this->pT as a mean point.
// error check!
if ((n1 == 0) || (n1 == n))
{
// our partitioning has failed: all the triangles fell into just
// one of the groups. So, we arbitrarily partition them into
// equal parts, and proceed.
n1 = n/2;
// now recompute accumulated stuff
reaccum_moments(M1, t, n1);
reaccum_moments(M2, t + n1, n - n1);
}
// With the max and min data, determine the center point and dimensions
// of the parent box.
c[0] = (minval[0] + maxval[0])*0.5;
c[1] = (minval[1] + maxval[1])*0.5;
c[2] = (minval[2] + maxval[2])*0.5;
pT[0] = c[0] * pR[0][0] + c[1] * pR[0][1] + c[2] * pR[0][2];
pT[1] = c[0] * pR[1][0] + c[1] * pR[1][1] + c[2] * pR[1][2];
pT[2] = c[0] * pR[2][0] + c[1] * pR[2][1] + c[2] * pR[2][2];
d[0] = (maxval[0] - minval[0])*0.5;
d[1] = (maxval[1] - minval[1])*0.5;
d[2] = (maxval[2] - minval[2])*0.5;
// allocate new boxes
P = RAPID_boxes + RAPID_boxes_inited++;
N = RAPID_boxes + RAPID_boxes_inited++;
// Compute the orienations for the child boxes (eigenvectors of
// covariance matrix). Select the direction of maximum spread to be
// the split axis for each child.
double tR[3][3];
if (n1 > 1)
{
mean_from_accum(P->pT, M1);
covariance_from_accum(C, M1);
if (eigen_and_sort1(tR, C) > 30)
{
// unable to find an orientation. We'll just pick identity.
Midentity(tR);
}
McM(P->pR, tR);
if ((rc = P->split_recurse(t, n1)) != RAPID_OK) return rc;
}
else
{
if ((rc = P->split_recurse(t)) != RAPID_OK) return rc;
}
McM(C, P->pR); MTxM(P->pR, pR, C); // and F1
VmV(c, P->pT, pT); MTxV(P->pT, pR, c);
if ((n-n1) > 1)
{
mean_from_accum(N->pT, M2);
covariance_from_accum (C, M2);
if (eigen_and_sort1(tR, C) > 30)
{
// unable to find an orientation. We'll just pick identity.
Midentity(tR);
}
McM(N->pR, tR);
if ((rc = N->split_recurse(t + n1, n - n1)) != RAPID_OK) return rc;
}
else
{
if ((rc = N->split_recurse(t+n1)) != RAPID_OK) return rc;
}
McM(C, N->pR); MTxM(N->pR, pR, C);
VmV(c, N->pT, pT); MTxV(N->pT, pR, c);
return RAPID_OK;
}
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;
}
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);
}
}
}
}
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();
}
}
}
