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