PQP.cpp
Go to the documentation of this file.
00001 /*************************************************************************\
00002 
00003   Copyright 1999 The University of North Carolina at Chapel Hill.
00004   All Rights Reserved.
00005 
00006   Permission to use, copy, modify and distribute this software and its
00007   documentation for educational, research and non-profit purposes, without
00008   fee, and without a written agreement is hereby granted, provided that the
00009   above copyright notice and the following three paragraphs appear in all
00010   copies.
00011 
00012   IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
00013   LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
00014   CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
00015   USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
00016   OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
00017   DAMAGES.
00018 
00019   THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
00020   WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
00021   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE
00022   PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
00023   NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
00024   UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
00025 
00026   The authors may be contacted via:
00027 
00028   US Mail:             S. Gottschalk, E. Larsen
00029                        Department of Computer Science
00030                        Sitterson Hall, CB #3175
00031                        University of N. Carolina
00032                        Chapel Hill, NC 27599-3175
00033 
00034   Phone:               (919)962-1749
00035 
00036   EMail:               geom@cs.unc.edu
00037 
00038 
00039 \**************************************************************************/
00040 
00041 #include <stdio.h>
00042 #include <string.h>
00043 #include "PQP.h"
00044 #include "BVTQ.h"
00045 #include "Build.h"
00046 #include "MatVec.h"
00047 #include "GetTime.h"
00048 #include "TriDist.h"
00049 
00050 enum BUILD_STATE
00051 { 
00052   PQP_BUILD_STATE_EMPTY,     // empty state, immediately after constructor
00053   PQP_BUILD_STATE_BEGUN,     // after BeginModel(), state for adding triangles
00054   PQP_BUILD_STATE_PROCESSED  // after tree has been built, ready to use
00055 };
00056 
00057 PQP_Model::PQP_Model()
00058 {
00059   // no bounding volume tree yet
00060 
00061   b = 0;  
00062   num_bvs_alloced = 0;
00063   num_bvs = 0;
00064 
00065   // no tri list yet
00066 
00067   tris = 0;
00068   num_tris = 0;
00069   num_tris_alloced = 0;
00070 
00071   last_tri = 0;
00072 
00073   build_state = PQP_BUILD_STATE_EMPTY;
00074 }
00075 
00076 PQP_Model::~PQP_Model()
00077 {
00078   if (b != NULL)
00079     delete [] b;
00080   if (tris != NULL)
00081     delete [] tris;
00082 }
00083 
00084 int
00085 PQP_Model::BeginModel(int n)
00086 {
00087   // reset to initial state if necessary
00088 
00089   if (build_state != PQP_BUILD_STATE_EMPTY) 
00090   {
00091     delete [] b;
00092     delete [] tris;
00093   
00094     num_tris = num_bvs = num_tris_alloced = num_bvs_alloced = 0;
00095   }
00096 
00097   // prepare model for addition of triangles
00098 
00099   if (n <= 0) n = 8;
00100   num_tris_alloced = n;
00101   tris = new Tri[n];
00102   if (!tris) 
00103   {
00104     fprintf(stderr, "PQP Error!  Out of memory for tri array on "
00105                     "BeginModel() call!\n");
00106     return PQP_ERR_MODEL_OUT_OF_MEMORY;  
00107   }
00108 
00109   // give a warning if called out of sequence
00110 
00111   if (build_state != PQP_BUILD_STATE_EMPTY)
00112   {
00113     fprintf(stderr,
00114             "PQP Warning! Called BeginModel() on a PQP_Model that \n"
00115             "was not empty. This model was cleared and previous\n"
00116             "triangle additions were lost.\n");
00117     build_state = PQP_BUILD_STATE_BEGUN;
00118     return PQP_ERR_BUILD_OUT_OF_SEQUENCE;
00119   }
00120 
00121   build_state = PQP_BUILD_STATE_BEGUN;
00122   return PQP_OK;
00123 }
00124 
00125 int
00126 PQP_Model::AddTri(const PQP_REAL *p1, 
00127                   const PQP_REAL *p2, 
00128                   const PQP_REAL *p3, 
00129                   int id)
00130 {
00131   if (build_state == PQP_BUILD_STATE_EMPTY)
00132   {
00133     BeginModel();
00134   }
00135   else if (build_state == PQP_BUILD_STATE_PROCESSED)
00136   {
00137     fprintf(stderr,"PQP Warning! Called AddTri() on PQP_Model \n"
00138                    "object that was already ended. AddTri() was\n"
00139                    "ignored.  Must do a BeginModel() to clear the\n"
00140                    "model for addition of new triangles\n");
00141     return PQP_ERR_BUILD_OUT_OF_SEQUENCE;
00142   }
00143         
00144   // allocate for new triangles
00145 
00146   if (num_tris >= num_tris_alloced)
00147   {
00148     Tri *temp;
00149     temp = new Tri[num_tris_alloced*2];
00150     if (!temp)
00151     {
00152       fprintf(stderr, "PQP Error!  Out of memory for tri array on"
00153                       " AddTri() call!\n");
00154       return PQP_ERR_MODEL_OUT_OF_MEMORY;  
00155     }
00156     memcpy(temp, tris, sizeof(Tri)*num_tris);
00157     delete [] tris;
00158     tris = temp;
00159     num_tris_alloced = num_tris_alloced*2;
00160   }
00161   
00162   // initialize the new triangle
00163 
00164   tris[num_tris].p1[0] = p1[0];
00165   tris[num_tris].p1[1] = p1[1];
00166   tris[num_tris].p1[2] = p1[2];
00167 
00168   tris[num_tris].p2[0] = p2[0];
00169   tris[num_tris].p2[1] = p2[1];
00170   tris[num_tris].p2[2] = p2[2];
00171 
00172   tris[num_tris].p3[0] = p3[0];
00173   tris[num_tris].p3[1] = p3[1];
00174   tris[num_tris].p3[2] = p3[2];
00175 
00176   tris[num_tris].id = id;
00177 
00178   num_tris += 1;
00179 
00180   return PQP_OK;
00181 }
00182 
00183 int
00184 PQP_Model::EndModel()
00185 {
00186   if (build_state == PQP_BUILD_STATE_PROCESSED)
00187   {
00188     fprintf(stderr,"PQP Warning! Called EndModel() on PQP_Model \n"
00189                    "object that was already ended. EndModel() was\n"
00190                    "ignored.  Must do a BeginModel() to clear the\n"
00191                    "model for addition of new triangles\n");
00192     return PQP_ERR_BUILD_OUT_OF_SEQUENCE;
00193   }
00194 
00195   // report error is no tris
00196 
00197   if (num_tris == 0)
00198   {
00199     fprintf(stderr,"PQP Error! EndModel() called on model with"
00200                    " no triangles\n");
00201     return PQP_ERR_BUILD_EMPTY_MODEL;
00202   }
00203 
00204   // shrink fit tris array 
00205 
00206   if (num_tris_alloced > num_tris)
00207   {
00208     Tri *new_tris = new Tri[num_tris];
00209     if (!new_tris) 
00210     {
00211       fprintf(stderr, "PQP Error!  Out of memory for tri array "
00212                       "in EndModel() call!\n");
00213       return PQP_ERR_MODEL_OUT_OF_MEMORY;  
00214     }
00215     memcpy(new_tris, tris, sizeof(Tri)*num_tris);
00216     delete [] tris;
00217     tris = new_tris;
00218     num_tris_alloced = num_tris;
00219   }
00220 
00221   // create an array of BVs for the model
00222 
00223   b = new BV[2*num_tris - 1];
00224   if (!b)
00225   {
00226     fprintf(stderr,"PQP Error! out of memory for BV array "
00227                    "in EndModel()\n");
00228     return PQP_ERR_MODEL_OUT_OF_MEMORY;
00229   }
00230   num_bvs_alloced = 2*num_tris - 1;
00231   num_bvs = 0;
00232 
00233   // we should build the model now.
00234 
00235   build_model(this);
00236   build_state = PQP_BUILD_STATE_PROCESSED;
00237 
00238   last_tri = tris;
00239 
00240   return PQP_OK;
00241 }
00242 
00243 int
00244 PQP_Model::MemUsage(int msg)
00245 {
00246   int mem_bv_list = sizeof(BV)*num_bvs;
00247   int mem_tri_list = sizeof(Tri)*num_tris;
00248 
00249   int total_mem = mem_bv_list + mem_tri_list + sizeof(PQP_Model);
00250 
00251   if (msg) 
00252   {
00253     fprintf(stderr,"Total for model %p: %d bytes\n", this, total_mem);
00254     fprintf(stderr,"BVs: %d alloced, take %ld bytes each\n",
00255             num_bvs, sizeof(BV));
00256     fprintf(stderr,"Tris: %d alloced, take %ld bytes each\n",
00257             num_tris, sizeof(Tri));
00258   }
00259   
00260   return total_mem;
00261 }
00262 
00263 //  COLLIDE STUFF
00264 //
00265 //--------------------------------------------------------------------------
00266 
00267 PQP_CollideResult::PQP_CollideResult()
00268 {
00269   pairs = 0;
00270   num_pairs = num_pairs_alloced = 0;
00271   num_bv_tests = 0;
00272   num_tri_tests = 0;
00273 }
00274 
00275 PQP_CollideResult::~PQP_CollideResult()
00276 {
00277   delete [] pairs;
00278 }
00279 
00280 void
00281 PQP_CollideResult::FreePairsList()
00282 {
00283   num_pairs = num_pairs_alloced = 0;
00284   delete [] pairs;
00285   pairs = 0;
00286 }
00287 
00288 // may increase OR reduce mem usage
00289 void
00290 PQP_CollideResult::SizeTo(int n)
00291 {
00292   CollisionPair *temp;
00293 
00294   if (n < num_pairs) 
00295   {
00296     fprintf(stderr, "PQP Error: Internal error in "
00297                     "'PQP_CollideResult::SizeTo(int n)'\n");
00298     fprintf(stderr, "       n = %d, but num_pairs = %d\n", n, num_pairs);
00299     return;
00300   }
00301   
00302   temp = new CollisionPair[n];
00303   memcpy(temp, pairs, num_pairs*sizeof(CollisionPair));
00304   delete [] pairs;
00305   pairs = temp;
00306   num_pairs_alloced = n;
00307   return;
00308 }
00309 
00310 void
00311 PQP_CollideResult::Add(int a, int b)
00312 {
00313   if (num_pairs >= num_pairs_alloced) 
00314   {
00315     // allocate more
00316 
00317     SizeTo(num_pairs_alloced*2+8);
00318   }
00319 
00320   // now proceed as usual
00321 
00322   pairs[num_pairs].id1 = a;
00323   pairs[num_pairs].id2 = b;
00324   num_pairs++;
00325 }
00326 
00327 // TRIANGLE OVERLAP TEST
00328        
00329 inline
00330 PQP_REAL
00331 max(PQP_REAL a, PQP_REAL b, PQP_REAL c)
00332 {
00333   PQP_REAL t = a;
00334   if (b > t) t = b;
00335   if (c > t) t = c;
00336   return t;
00337 }
00338 
00339 inline
00340 PQP_REAL
00341 min(PQP_REAL a, PQP_REAL b, PQP_REAL c)
00342 {
00343   PQP_REAL t = a;
00344   if (b < t) t = b;
00345   if (c < t) t = c;
00346   return t;
00347 }
00348 
00349 int
00350 project6(PQP_REAL *ax, 
00351          PQP_REAL *p1, PQP_REAL *p2, PQP_REAL *p3, 
00352          PQP_REAL *q1, PQP_REAL *q2, PQP_REAL *q3)
00353 {
00354   PQP_REAL P1 = VdotV(ax, p1);
00355   PQP_REAL P2 = VdotV(ax, p2);
00356   PQP_REAL P3 = VdotV(ax, p3);
00357   PQP_REAL Q1 = VdotV(ax, q1);
00358   PQP_REAL Q2 = VdotV(ax, q2);
00359   PQP_REAL Q3 = VdotV(ax, q3);
00360   
00361   PQP_REAL mx1 = max(P1, P2, P3);
00362   PQP_REAL mn1 = min(P1, P2, P3);
00363   PQP_REAL mx2 = max(Q1, Q2, Q3);
00364   PQP_REAL mn2 = min(Q1, Q2, Q3);
00365 
00366   if (mn1 > mx2) return 0;
00367   if (mn2 > mx1) return 0;
00368   return 1;
00369 }
00370 
00371 // very robust triangle intersection test
00372 // uses no divisions
00373 // works on coplanar triangles
00374 int 
00375 TriContact(PQP_REAL *P1, PQP_REAL *P2, PQP_REAL *P3,
00376            PQP_REAL *Q1, PQP_REAL *Q2, PQP_REAL *Q3) 
00377 {
00378 
00379   // One triangle is (p1,p2,p3).  Other is (q1,q2,q3).
00380   // Edges are (e1,e2,e3) and (f1,f2,f3).
00381   // Normals are n1 and m1
00382   // Outwards are (g1,g2,g3) and (h1,h2,h3).
00383   //  
00384   // We assume that the triangle vertices are in the same coordinate system.
00385   //
00386   // First thing we do is establish a new c.s. so that p1 is at (0,0,0).
00387 
00388   PQP_REAL p1[3], p2[3], p3[3];
00389   PQP_REAL q1[3], q2[3], q3[3];
00390   PQP_REAL e1[3], e2[3], e3[3];
00391   PQP_REAL f1[3], f2[3], f3[3];
00392   PQP_REAL g1[3], g2[3], g3[3];
00393   PQP_REAL h1[3], h2[3], h3[3];
00394   PQP_REAL n1[3], m1[3];
00395 
00396   PQP_REAL ef11[3], ef12[3], ef13[3];
00397   PQP_REAL ef21[3], ef22[3], ef23[3];
00398   PQP_REAL ef31[3], ef32[3], ef33[3];
00399   
00400   p1[0] = P1[0] - P1[0];  p1[1] = P1[1] - P1[1];  p1[2] = P1[2] - P1[2];
00401   p2[0] = P2[0] - P1[0];  p2[1] = P2[1] - P1[1];  p2[2] = P2[2] - P1[2];
00402   p3[0] = P3[0] - P1[0];  p3[1] = P3[1] - P1[1];  p3[2] = P3[2] - P1[2];
00403   
00404   q1[0] = Q1[0] - P1[0];  q1[1] = Q1[1] - P1[1];  q1[2] = Q1[2] - P1[2];
00405   q2[0] = Q2[0] - P1[0];  q2[1] = Q2[1] - P1[1];  q2[2] = Q2[2] - P1[2];
00406   q3[0] = Q3[0] - P1[0];  q3[1] = Q3[1] - P1[1];  q3[2] = Q3[2] - P1[2];
00407   
00408   e1[0] = p2[0] - p1[0];  e1[1] = p2[1] - p1[1];  e1[2] = p2[2] - p1[2];
00409   e2[0] = p3[0] - p2[0];  e2[1] = p3[1] - p2[1];  e2[2] = p3[2] - p2[2];
00410   e3[0] = p1[0] - p3[0];  e3[1] = p1[1] - p3[1];  e3[2] = p1[2] - p3[2];
00411 
00412   f1[0] = q2[0] - q1[0];  f1[1] = q2[1] - q1[1];  f1[2] = q2[2] - q1[2];
00413   f2[0] = q3[0] - q2[0];  f2[1] = q3[1] - q2[1];  f2[2] = q3[2] - q2[2];
00414   f3[0] = q1[0] - q3[0];  f3[1] = q1[1] - q3[1];  f3[2] = q1[2] - q3[2];
00415   
00416   VcrossV(n1, e1, e2);
00417   VcrossV(m1, f1, f2);
00418 
00419   VcrossV(g1, e1, n1);
00420   VcrossV(g2, e2, n1);
00421   VcrossV(g3, e3, n1);
00422   VcrossV(h1, f1, m1);
00423   VcrossV(h2, f2, m1);
00424   VcrossV(h3, f3, m1);
00425 
00426   VcrossV(ef11, e1, f1);
00427   VcrossV(ef12, e1, f2);
00428   VcrossV(ef13, e1, f3);
00429   VcrossV(ef21, e2, f1);
00430   VcrossV(ef22, e2, f2);
00431   VcrossV(ef23, e2, f3);
00432   VcrossV(ef31, e3, f1);
00433   VcrossV(ef32, e3, f2);
00434   VcrossV(ef33, e3, f3);
00435   
00436   // now begin the series of tests
00437 
00438   if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0;
00439   if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0;
00440   
00441   if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0;
00442   if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0;
00443   if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0;
00444   if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0;
00445   if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0;
00446   if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0;
00447   if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0;
00448   if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0;
00449   if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0;
00450 
00451   if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0;
00452   if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0;
00453   if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0;
00454   if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0;
00455   if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0;
00456   if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0;
00457 
00458   return 1;
00459 }
00460 
00461 inline
00462 PQP_REAL
00463 TriDistance(PQP_REAL R[3][3], PQP_REAL T[3], Tri *t1, Tri *t2,
00464             PQP_REAL p[3], PQP_REAL q[3])
00465 {
00466   // transform tri 2 into same space as tri 1
00467 
00468   PQP_REAL tri1[3][3], tri2[3][3];
00469 
00470   VcV(tri1[0], t1->p1);
00471   VcV(tri1[1], t1->p2);
00472   VcV(tri1[2], t1->p3);
00473   MxVpV(tri2[0], R, t2->p1, T);
00474   MxVpV(tri2[1], R, t2->p2, T);
00475   MxVpV(tri2[2], R, t2->p3, T);
00476                                 
00477   return TriDist(p,q,tri1,tri2);
00478 }
00479 
00480 
00481 void
00482 CollideRecurse(PQP_CollideResult *res,
00483                PQP_REAL R[3][3], PQP_REAL T[3], // b2 relative to b1
00484                PQP_Model *o1, int b1, 
00485                PQP_Model *o2, int b2, int flag)
00486 {
00487   // first thing, see if we're overlapping
00488 
00489   res->num_bv_tests++;
00490 
00491   if (!BV_Overlap(R, T, o1->child(b1), o2->child(b2))) return;
00492 
00493   // if we are, see if we test triangles next
00494 
00495   int l1 = o1->child(b1)->Leaf();
00496   int l2 = o2->child(b2)->Leaf();
00497 
00498   if (l1 && l2) 
00499   {
00500     res->num_tri_tests++;
00501 
00502 #if 1
00503     // transform the points in b2 into space of b1, then compare
00504 
00505     Tri *t1 = &o1->tris[-o1->child(b1)->first_child - 1];
00506     Tri *t2 = &o2->tris[-o2->child(b2)->first_child - 1];
00507     PQP_REAL q1[3], q2[3], q3[3];
00508     PQP_REAL *p1 = t1->p1;
00509     PQP_REAL *p2 = t1->p2;
00510     PQP_REAL *p3 = t1->p3;    
00511     MxVpV(q1, res->R, t2->p1, res->T);
00512     MxVpV(q2, res->R, t2->p2, res->T);
00513     MxVpV(q3, res->R, t2->p3, res->T);
00514     if (TriContact(p1, p2, p3, q1, q2, q3)) 
00515     {
00516       // add this to result
00517 
00518       res->Add(t1->id, t2->id);
00519     }
00520 #else
00521     PQP_REAL p[3], q[3];
00522 
00523     Tri *t1 = &o1->tris[-o1->child(b1)->first_child - 1];
00524     Tri *t2 = &o2->tris[-o2->child(b2)->first_child - 1];
00525 
00526     if (TriDistance(res->R,res->T,t1,t2,p,q) == 0.0)
00527     {
00528       // add this to result
00529 
00530       res->Add(t1->id, t2->id);
00531     }
00532 #endif
00533 
00534     return;
00535   }
00536 
00537   // we dont, so decide whose children to visit next
00538 
00539   PQP_REAL sz1 = o1->child(b1)->GetSize();
00540   PQP_REAL sz2 = o2->child(b2)->GetSize();
00541 
00542   PQP_REAL Rc[3][3],Tc[3],Ttemp[3];
00543     
00544   if (l2 || (!l1 && (sz1 > sz2)))
00545   {
00546     int c1 = o1->child(b1)->first_child;
00547     int c2 = c1 + 1;
00548 
00549     MTxM(Rc,o1->child(c1)->R,R);
00550 #if PQP_BV_TYPE & OBB_TYPE
00551     VmV(Ttemp,T,o1->child(c1)->To);
00552 #else
00553     VmV(Ttemp,T,o1->child(c1)->Tr);
00554 #endif
00555     MTxV(Tc,o1->child(c1)->R,Ttemp);
00556     CollideRecurse(res,Rc,Tc,o1,c1,o2,b2,flag);
00557 
00558     if ((flag == PQP_FIRST_CONTACT) && (res->num_pairs > 0)) return;
00559 
00560     MTxM(Rc,o1->child(c2)->R,R);
00561 #if PQP_BV_TYPE & OBB_TYPE
00562     VmV(Ttemp,T,o1->child(c2)->To);
00563 #else
00564     VmV(Ttemp,T,o1->child(c2)->Tr);
00565 #endif
00566     MTxV(Tc,o1->child(c2)->R,Ttemp);
00567     CollideRecurse(res,Rc,Tc,o1,c2,o2,b2,flag);
00568   }
00569   else 
00570   {
00571     int c1 = o2->child(b2)->first_child;
00572     int c2 = c1 + 1;
00573 
00574     MxM(Rc,R,o2->child(c1)->R);
00575 #if PQP_BV_TYPE & OBB_TYPE
00576     MxVpV(Tc,R,o2->child(c1)->To,T);
00577 #else
00578     MxVpV(Tc,R,o2->child(c1)->Tr,T);
00579 #endif
00580     CollideRecurse(res,Rc,Tc,o1,b1,o2,c1,flag);
00581 
00582     if ((flag == PQP_FIRST_CONTACT) && (res->num_pairs > 0)) return;
00583 
00584     MxM(Rc,R,o2->child(c2)->R);
00585 #if PQP_BV_TYPE & OBB_TYPE
00586     MxVpV(Tc,R,o2->child(c2)->To,T);
00587 #else
00588     MxVpV(Tc,R,o2->child(c2)->Tr,T);
00589 #endif
00590     CollideRecurse(res,Rc,Tc,o1,b1,o2,c2,flag);
00591   }
00592 }
00593 
00594 int 
00595 PQP_Collide(PQP_CollideResult *res,
00596             PQP_REAL R1[3][3], PQP_REAL T1[3], PQP_Model *o1,
00597             PQP_REAL R2[3][3], PQP_REAL T2[3], PQP_Model *o2,
00598             int flag)
00599 {
00600   double t1 = GetTime();
00601 
00602   // make sure that the models are built
00603 
00604   if (o1->build_state != PQP_BUILD_STATE_PROCESSED) 
00605     return PQP_ERR_UNPROCESSED_MODEL;
00606   if (o2->build_state != PQP_BUILD_STATE_PROCESSED) 
00607     return PQP_ERR_UNPROCESSED_MODEL;
00608 
00609   // clear the stats
00610 
00611   res->num_bv_tests = 0;
00612   res->num_tri_tests = 0;
00613   
00614   // don't release the memory, but reset the num_pairs counter
00615 
00616   res->num_pairs = 0;
00617   
00618   // Okay, compute what transform [R,T] that takes us from cs1 to cs2.
00619   // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
00620   // First compute the rotation part, then translation part
00621 
00622   MTxM(res->R,R1,R2);
00623   PQP_REAL Ttemp[3];
00624   VmV(Ttemp, T2, T1);  
00625   MTxV(res->T, R1, Ttemp);
00626   
00627   // compute the transform from o1->child(0) to o2->child(0)
00628 
00629   PQP_REAL Rtemp[3][3], R[3][3], T[3];
00630 
00631   MxM(Rtemp,res->R,o2->child(0)->R);
00632   MTxM(R,o1->child(0)->R,Rtemp);
00633 
00634 #if PQP_BV_TYPE & OBB_TYPE
00635   MxVpV(Ttemp,res->R,o2->child(0)->To,res->T);
00636   VmV(Ttemp,Ttemp,o1->child(0)->To);
00637 #else
00638   MxVpV(Ttemp,res->R,o2->child(0)->Tr,res->T);
00639   VmV(Ttemp,Ttemp,o1->child(0)->Tr);
00640 #endif
00641 
00642   MTxV(T,o1->child(0)->R,Ttemp);
00643 
00644   // now start with both top level BVs  
00645 
00646   CollideRecurse(res,R,T,o1,0,o2,0,flag);
00647   
00648   double t2 = GetTime();
00649   res->query_time_secs = t2 - t1;
00650   
00651   return PQP_OK; 
00652 }
00653 
00654 #if PQP_BV_TYPE & RSS_TYPE // distance/tolerance only available with RSS
00655                            // unless an OBB distance test is supplied in 
00656                            // BV.cpp
00657 
00658 // DISTANCE STUFF
00659 //
00660 //--------------------------------------------------------------------------
00661 
00662 void
00663 DistanceRecurse(PQP_DistanceResult *res,
00664                 PQP_REAL R[3][3], PQP_REAL T[3], // b2 relative to b1
00665                 PQP_Model *o1, int b1,
00666                 PQP_Model *o2, int b2)
00667 {
00668   PQP_REAL sz1 = o1->child(b1)->GetSize();
00669   PQP_REAL sz2 = o2->child(b2)->GetSize();
00670   int l1 = o1->child(b1)->Leaf();
00671   int l2 = o2->child(b2)->Leaf();
00672 
00673   if (l1 && l2)
00674   {
00675     // both leaves.  Test the triangles beneath them.
00676 
00677     res->num_tri_tests++;
00678 
00679     PQP_REAL p[3], q[3];
00680 
00681     Tri *t1 = &o1->tris[-o1->child(b1)->first_child - 1];
00682     Tri *t2 = &o2->tris[-o2->child(b2)->first_child - 1];
00683 
00684     PQP_REAL d = TriDistance(res->R,res->T,t1,t2,p,q);
00685   
00686     if (d < res->distance) 
00687     {
00688       res->distance = d;
00689 
00690       VcV(res->p1, p);         // p already in c.s. 1
00691       VcV(res->p2, q);         // q must be transformed 
00692                                // into c.s. 2 later
00693       o1->last_tri = t1;
00694       o2->last_tri = t2;
00695     }
00696 
00697     return;
00698   }
00699 
00700   // First, perform distance tests on the children. Then traverse 
00701   // them recursively, but test the closer pair first, the further 
00702   // pair second.
00703 
00704   int a1,a2,c1,c2;  // new bv tests 'a' and 'c'
00705   PQP_REAL R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3];
00706 
00707   if (l2 || (!l1 && (sz1 > sz2)))
00708   {
00709     // visit the children of b1
00710 
00711     a1 = o1->child(b1)->first_child;
00712     a2 = b2;
00713     c1 = o1->child(b1)->first_child+1;
00714     c2 = b2;
00715     
00716     MTxM(R1,o1->child(a1)->R,R);
00717 #if PQP_BV_TYPE & RSS_TYPE
00718     VmV(Ttemp,T,o1->child(a1)->Tr);
00719 #else
00720     VmV(Ttemp,T,o1->child(a1)->To);
00721 #endif
00722     MTxV(T1,o1->child(a1)->R,Ttemp);
00723 
00724     MTxM(R2,o1->child(c1)->R,R);
00725 #if PQP_BV_TYPE & RSS_TYPE
00726     VmV(Ttemp,T,o1->child(c1)->Tr);
00727 #else
00728     VmV(Ttemp,T,o1->child(c1)->To);
00729 #endif
00730     MTxV(T2,o1->child(c1)->R,Ttemp);
00731   }
00732   else 
00733   {
00734     // visit the children of b2
00735 
00736     a1 = b1;
00737     a2 = o2->child(b2)->first_child;
00738     c1 = b1;
00739     c2 = o2->child(b2)->first_child+1;
00740 
00741     MxM(R1,R,o2->child(a2)->R);
00742 #if PQP_BV_TYPE & RSS_TYPE
00743     MxVpV(T1,R,o2->child(a2)->Tr,T);
00744 #else
00745     MxVpV(T1,R,o2->child(a2)->To,T);
00746 #endif
00747 
00748     MxM(R2,R,o2->child(c2)->R);
00749 #if PQP_BV_TYPE & RSS_TYPE
00750     MxVpV(T2,R,o2->child(c2)->Tr,T);
00751 #else
00752     MxVpV(T2,R,o2->child(c2)->To,T);
00753 #endif
00754   }
00755 
00756   res->num_bv_tests += 2;
00757 
00758   PQP_REAL d1 = BV_Distance(R1, T1, o1->child(a1), o2->child(a2));
00759   PQP_REAL d2 = BV_Distance(R2, T2, o1->child(c1), o2->child(c2));
00760 
00761   if (d2 < d1)
00762   {
00763     if ((d2 < (res->distance - res->abs_err)) || 
00764         (d2*(1 + res->rel_err) < res->distance)) 
00765     {      
00766       DistanceRecurse(res, R2, T2, o1, c1, o2, c2);      
00767     }
00768 
00769     if ((d1 < (res->distance - res->abs_err)) || 
00770         (d1*(1 + res->rel_err) < res->distance)) 
00771     {      
00772       DistanceRecurse(res, R1, T1, o1, a1, o2, a2);
00773     }
00774   }
00775   else 
00776   {
00777     if ((d1 < (res->distance - res->abs_err)) || 
00778         (d1*(1 + res->rel_err) < res->distance)) 
00779     {      
00780       DistanceRecurse(res, R1, T1, o1, a1, o2, a2);
00781     }
00782 
00783     if ((d2 < (res->distance - res->abs_err)) || 
00784         (d2*(1 + res->rel_err) < res->distance)) 
00785     {      
00786       DistanceRecurse(res, R2, T2, o1, c1, o2, c2);      
00787     }
00788   }
00789 }
00790 
00791 void
00792 DistanceQueueRecurse(PQP_DistanceResult *res, 
00793                      PQP_REAL R[3][3], PQP_REAL T[3],
00794                      PQP_Model *o1, int b1,
00795                      PQP_Model *o2, int b2)
00796 {
00797   BVTQ bvtq(res->qsize);
00798 
00799   BVT min_test;
00800   min_test.b1 = b1;
00801   min_test.b2 = b2;
00802   McM(min_test.R,R);
00803   VcV(min_test.T,T);
00804 
00805   while(1) 
00806   {  
00807     int l1 = o1->child(min_test.b1)->Leaf();
00808     int l2 = o2->child(min_test.b2)->Leaf();
00809     
00810     if (l1 && l2) 
00811     {  
00812       // both leaves.  Test the triangles beneath them.
00813 
00814       res->num_tri_tests++;
00815 
00816       PQP_REAL p[3], q[3];
00817 
00818       Tri *t1 = &o1->tris[-o1->child(min_test.b1)->first_child - 1];
00819       Tri *t2 = &o2->tris[-o2->child(min_test.b2)->first_child - 1];
00820 
00821       PQP_REAL d = TriDistance(res->R,res->T,t1,t2,p,q);
00822   
00823       if (d < res->distance)
00824       {
00825         res->distance = d;
00826 
00827         VcV(res->p1, p);         // p already in c.s. 1
00828         VcV(res->p2, q);         // q must be transformed 
00829                                  // into c.s. 2 later
00830         o1->last_tri = t1;
00831         o2->last_tri = t2;
00832       }
00833     }            
00834     else if (bvtq.GetNumTests() == bvtq.GetSize() - 1) 
00835     {  
00836       // queue can't get two more tests, recur
00837       
00838       DistanceQueueRecurse(res,min_test.R,min_test.T,
00839                            o1,min_test.b1,o2,min_test.b2);
00840     }
00841     else 
00842     {  
00843       // decide how to descend to children
00844       
00845       PQP_REAL sz1 = o1->child(min_test.b1)->GetSize();
00846       PQP_REAL sz2 = o2->child(min_test.b2)->GetSize();
00847 
00848       res->num_bv_tests += 2;
00849  
00850       BVT bvt1,bvt2;
00851       PQP_REAL Ttemp[3];
00852 
00853       if (l2 || (!l1 && (sz1 > sz2)))   
00854       {  
00855         // put new tests on queue consisting of min_test.b2 
00856         // with children of min_test.b1 
00857       
00858         int c1 = o1->child(min_test.b1)->first_child;
00859         int c2 = c1 + 1;
00860 
00861         // init bv test 1
00862 
00863         bvt1.b1 = c1;
00864         bvt1.b2 = min_test.b2;
00865         MTxM(bvt1.R,o1->child(c1)->R,min_test.R);
00866 #if PQP_BV_TYPE & RSS_TYPE
00867         VmV(Ttemp,min_test.T,o1->child(c1)->Tr);
00868 #else
00869         VmV(Ttemp,min_test.T,o1->child(c1)->To);
00870 #endif
00871         MTxV(bvt1.T,o1->child(c1)->R,Ttemp);
00872         bvt1.d = BV_Distance(bvt1.R,bvt1.T,
00873                             o1->child(bvt1.b1),o2->child(bvt1.b2));
00874 
00875         // init bv test 2
00876 
00877         bvt2.b1 = c2;
00878         bvt2.b2 = min_test.b2;
00879         MTxM(bvt2.R,o1->child(c2)->R,min_test.R);
00880 #if PQP_BV_TYPE & RSS_TYPE
00881         VmV(Ttemp,min_test.T,o1->child(c2)->Tr);
00882 #else
00883         VmV(Ttemp,min_test.T,o1->child(c2)->To);
00884 #endif
00885         MTxV(bvt2.T,o1->child(c2)->R,Ttemp);
00886         bvt2.d = BV_Distance(bvt2.R,bvt2.T,
00887                             o1->child(bvt2.b1),o2->child(bvt2.b2));
00888       }
00889       else 
00890       {
00891         // put new tests on queue consisting of min_test.b1 
00892         // with children of min_test.b2
00893       
00894         int c1 = o2->child(min_test.b2)->first_child;
00895         int c2 = c1 + 1;
00896 
00897         // init bv test 1
00898 
00899         bvt1.b1 = min_test.b1;
00900         bvt1.b2 = c1;
00901         MxM(bvt1.R,min_test.R,o2->child(c1)->R);
00902 #if PQP_BV_TYPE & RSS_TYPE
00903         MxVpV(bvt1.T,min_test.R,o2->child(c1)->Tr,min_test.T);
00904 #else
00905         MxVpV(bvt1.T,min_test.R,o2->child(c1)->To,min_test.T);
00906 #endif
00907         bvt1.d = BV_Distance(bvt1.R,bvt1.T,
00908                             o1->child(bvt1.b1),o2->child(bvt1.b2));
00909 
00910         // init bv test 2
00911 
00912         bvt2.b1 = min_test.b1;
00913         bvt2.b2 = c2;
00914         MxM(bvt2.R,min_test.R,o2->child(c2)->R);
00915 #if PQP_BV_TYPE & RSS_TYPE
00916         MxVpV(bvt2.T,min_test.R,o2->child(c2)->Tr,min_test.T);
00917 #else
00918         MxVpV(bvt2.T,min_test.R,o2->child(c2)->To,min_test.T);
00919 #endif
00920         bvt2.d = BV_Distance(bvt2.R,bvt2.T,
00921                             o1->child(bvt2.b1),o2->child(bvt2.b2));
00922       }
00923 
00924       bvtq.AddTest(bvt1);       
00925       bvtq.AddTest(bvt2);
00926     }
00927 
00928     if (bvtq.Empty())
00929     {
00930       break;
00931     }
00932     else
00933     {
00934       min_test = bvtq.ExtractMinTest();
00935 
00936       if ((min_test.d + res->abs_err >= res->distance) && 
00937          ((min_test.d * (1 + res->rel_err)) >= res->distance)) 
00938       {
00939         break;
00940       }
00941     }
00942   }  
00943 }       
00944 
00945 int 
00946 PQP_Distance(PQP_DistanceResult *res,
00947              PQP_REAL R1[3][3], PQP_REAL T1[3], PQP_Model *o1,
00948              PQP_REAL R2[3][3], PQP_REAL T2[3], PQP_Model *o2,
00949              PQP_REAL rel_err, PQP_REAL abs_err,
00950              int qsize)
00951 {
00952   
00953   double time1 = GetTime();
00954   
00955   // make sure that the models are built
00956 
00957   if (o1->build_state != PQP_BUILD_STATE_PROCESSED) 
00958     return PQP_ERR_UNPROCESSED_MODEL;
00959   if (o2->build_state != PQP_BUILD_STATE_PROCESSED) 
00960     return PQP_ERR_UNPROCESSED_MODEL;
00961 
00962   // Okay, compute what transform [R,T] that takes us from cs2 to cs1.
00963   // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
00964   // First compute the rotation part, then translation part
00965 
00966   MTxM(res->R,R1,R2);
00967   PQP_REAL Ttemp[3];
00968   VmV(Ttemp, T2, T1);  
00969   MTxV(res->T, R1, Ttemp);
00970   
00971   // establish initial upper bound using last triangles which 
00972   // provided the minimum distance
00973 
00974   PQP_REAL p[3],q[3];
00975   res->distance = TriDistance(res->R,res->T,o1->last_tri,o2->last_tri,p,q);
00976   VcV(res->p1,p);
00977   VcV(res->p2,q);
00978 
00979   // initialize error bounds
00980 
00981   res->abs_err = abs_err;
00982   res->rel_err = rel_err;
00983   
00984   // clear the stats
00985 
00986   res->num_bv_tests = 0;
00987   res->num_tri_tests = 0;
00988   
00989   // compute the transform from o1->child(0) to o2->child(0)
00990 
00991   PQP_REAL Rtemp[3][3], R[3][3], T[3];
00992 
00993   MxM(Rtemp,res->R,o2->child(0)->R);
00994   MTxM(R,o1->child(0)->R,Rtemp);
00995   
00996 #if PQP_BV_TYPE & RSS_TYPE
00997   MxVpV(Ttemp,res->R,o2->child(0)->Tr,res->T);
00998   VmV(Ttemp,Ttemp,o1->child(0)->Tr);
00999 #else
01000   MxVpV(Ttemp,res->R,o2->child(0)->To,res->T);
01001   VmV(Ttemp,Ttemp,o1->child(0)->To);
01002 #endif
01003   MTxV(T,o1->child(0)->R,Ttemp);
01004 
01005   // choose routine according to queue size
01006   
01007   if (qsize <= 2)
01008   {
01009     DistanceRecurse(res,R,T,o1,0,o2,0);    
01010   }
01011   else 
01012   { 
01013     res->qsize = qsize;
01014 
01015     DistanceQueueRecurse(res,R,T,o1,0,o2,0);
01016   }
01017 
01018   // res->p2 is in cs 1 ; transform it to cs 2
01019 
01020   PQP_REAL u[3];
01021   VmV(u, res->p2, res->T);
01022   MTxV(res->p2, res->R, u);
01023 
01024   double time2 = GetTime();
01025   res->query_time_secs = time2 - time1;  
01026 
01027   return PQP_OK;
01028 }
01029 
01030 // Tolerance Stuff
01031 //
01032 //---------------------------------------------------------------------------
01033 void 
01034 ToleranceRecurse(PQP_ToleranceResult *res, 
01035                  PQP_REAL R[3][3], PQP_REAL T[3],
01036                  PQP_Model *o1, int b1, PQP_Model *o2, int b2)
01037 {
01038   PQP_REAL sz1 = o1->child(b1)->GetSize();
01039   PQP_REAL sz2 = o2->child(b2)->GetSize();
01040   int l1 = o1->child(b1)->Leaf();
01041   int l2 = o2->child(b2)->Leaf();
01042 
01043   if (l1 && l2) 
01044   {
01045     // both leaves - find if tri pair within tolerance
01046     
01047     res->num_tri_tests++;
01048 
01049     PQP_REAL p[3], q[3];
01050 
01051     Tri *t1 = &o1->tris[-o1->child(b1)->first_child - 1];
01052     Tri *t2 = &o2->tris[-o2->child(b2)->first_child - 1];
01053 
01054     PQP_REAL d = TriDistance(res->R,res->T,t1,t2,p,q);
01055     
01056     if (d <= res->tolerance)  
01057     {  
01058       // triangle pair distance less than tolerance
01059 
01060       res->closer_than_tolerance = 1;
01061       res->distance = d;
01062       VcV(res->p1, p);         // p already in c.s. 1
01063       VcV(res->p2, q);         // q must be transformed 
01064                                // into c.s. 2 later
01065     }
01066 
01067     return;
01068   }
01069 
01070   int a1,a2,c1,c2;  // new bv tests 'a' and 'c'
01071   PQP_REAL R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3];
01072 
01073   if (l2 || (!l1 && (sz1 > sz2)))
01074   {
01075     // visit the children of b1
01076 
01077     a1 = o1->child(b1)->first_child;
01078     a2 = b2;
01079     c1 = o1->child(b1)->first_child+1;
01080     c2 = b2;
01081     
01082     MTxM(R1,o1->child(a1)->R,R);
01083 #if PQP_BV_TYPE & RSS_TYPE
01084     VmV(Ttemp,T,o1->child(a1)->Tr);
01085 #else
01086     VmV(Ttemp,T,o1->child(a1)->To);
01087 #endif
01088     MTxV(T1,o1->child(a1)->R,Ttemp);
01089 
01090     MTxM(R2,o1->child(c1)->R,R);
01091 #if PQP_BV_TYPE & RSS_TYPE
01092     VmV(Ttemp,T,o1->child(c1)->Tr);
01093 #else
01094     VmV(Ttemp,T,o1->child(c1)->To);
01095 #endif
01096     MTxV(T2,o1->child(c1)->R,Ttemp);
01097   }
01098   else 
01099   {
01100     // visit the children of b2
01101 
01102     a1 = b1;
01103     a2 = o2->child(b2)->first_child;
01104     c1 = b1;
01105     c2 = o2->child(b2)->first_child+1;
01106 
01107     MxM(R1,R,o2->child(a2)->R);
01108 #if PQP_BV_TYPE & RSS_TYPE
01109     MxVpV(T1,R,o2->child(a2)->Tr,T);
01110 #else
01111     MxVpV(T1,R,o2->child(a2)->To,T);
01112 #endif
01113     MxM(R2,R,o2->child(c2)->R);
01114 #if PQP_BV_TYPE & RSS_TYPE
01115     MxVpV(T2,R,o2->child(c2)->Tr,T);
01116 #else
01117     MxVpV(T2,R,o2->child(c2)->To,T);
01118 #endif
01119   }
01120 
01121   res->num_bv_tests += 2;
01122 
01123   PQP_REAL d1 = BV_Distance(R1, T1, o1->child(a1), o2->child(a2));
01124   PQP_REAL d2 = BV_Distance(R2, T2, o1->child(c1), o2->child(c2));
01125 
01126   if (d2 < d1) 
01127   {
01128     if (d2 <= res->tolerance) ToleranceRecurse(res, R2, T2, o1, c1, o2, c2);
01129     if (res->closer_than_tolerance) return;
01130     if (d1 <= res->tolerance) ToleranceRecurse(res, R1, T1, o1, a1, o2, a2);
01131   }
01132   else 
01133   {
01134     if (d1 <= res->tolerance) ToleranceRecurse(res, R1, T1, o1, a1, o2, a2);
01135     if (res->closer_than_tolerance) return;
01136     if (d2 <= res->tolerance) ToleranceRecurse(res, R2, T2, o1, c1, o2, c2);
01137   }
01138 }
01139 
01140 void
01141 ToleranceQueueRecurse(PQP_ToleranceResult *res,
01142                       PQP_REAL R[3][3], PQP_REAL T[3],
01143                       PQP_Model *o1, int b1,
01144                       PQP_Model *o2, int b2)
01145 {
01146   BVTQ bvtq(res->qsize);
01147   BVT min_test;
01148   min_test.b1 = b1;
01149   min_test.b2 = b2;
01150   McM(min_test.R,R);
01151   VcV(min_test.T,T);
01152 
01153   while(1)
01154   {  
01155     int l1 = o1->child(min_test.b1)->Leaf();
01156     int l2 = o2->child(min_test.b2)->Leaf();
01157     
01158     if (l1 && l2) 
01159     {  
01160       // both leaves - find if tri pair within tolerance
01161     
01162       res->num_tri_tests++;
01163 
01164       PQP_REAL p[3], q[3];
01165 
01166       Tri *t1 = &o1->tris[-o1->child(min_test.b1)->first_child - 1];
01167       Tri *t2 = &o2->tris[-o2->child(min_test.b2)->first_child - 1];
01168 
01169       PQP_REAL d = TriDistance(res->R,res->T,t1,t2,p,q);
01170     
01171       if (d <= res->tolerance)  
01172       {  
01173         // triangle pair distance less than tolerance
01174 
01175         res->closer_than_tolerance = 1;
01176         res->distance = d;
01177         VcV(res->p1, p);         // p already in c.s. 1
01178         VcV(res->p2, q);         // q must be transformed 
01179                                  // into c.s. 2 later
01180         return;
01181       }
01182     }
01183     else if (bvtq.GetNumTests() == bvtq.GetSize() - 1)
01184     {  
01185       // queue can't get two more tests, recur
01186       
01187       ToleranceQueueRecurse(res,min_test.R,min_test.T,
01188                             o1,min_test.b1,o2,min_test.b2);
01189       if (res->closer_than_tolerance == 1) return;
01190     }
01191     else 
01192     {  
01193       // decide how to descend to children
01194       
01195       PQP_REAL sz1 = o1->child(min_test.b1)->GetSize();
01196       PQP_REAL sz2 = o2->child(min_test.b2)->GetSize();
01197 
01198       res->num_bv_tests += 2;
01199       
01200       BVT bvt1,bvt2;
01201       PQP_REAL Ttemp[3];
01202 
01203       if (l2 || (!l1 && (sz1 > sz2)))   
01204       {
01205               // add two new tests to queue, consisting of min_test.b2
01206         // with the children of min_test.b1
01207 
01208         int c1 = o1->child(min_test.b1)->first_child;
01209         int c2 = c1 + 1;
01210 
01211         // init bv test 1
01212 
01213         bvt1.b1 = c1;
01214         bvt1.b2 = min_test.b2;
01215         MTxM(bvt1.R,o1->child(c1)->R,min_test.R);
01216 #if PQP_BV_TYPE & RSS_TYPE
01217         VmV(Ttemp,min_test.T,o1->child(c1)->Tr);
01218 #else
01219         VmV(Ttemp,min_test.T,o1->child(c1)->To);
01220 #endif
01221         MTxV(bvt1.T,o1->child(c1)->R,Ttemp);
01222         bvt1.d = BV_Distance(bvt1.R,bvt1.T,
01223                             o1->child(bvt1.b1),o2->child(bvt1.b2));
01224 
01225               // init bv test 2
01226 
01227               bvt2.b1 = c2;
01228               bvt2.b2 = min_test.b2;
01229               MTxM(bvt2.R,o1->child(c2)->R,min_test.R);
01230 #if PQP_BV_TYPE & RSS_TYPE
01231               VmV(Ttemp,min_test.T,o1->child(c2)->Tr);
01232 #else
01233               VmV(Ttemp,min_test.T,o1->child(c2)->To);
01234 #endif
01235               MTxV(bvt2.T,o1->child(c2)->R,Ttemp);
01236         bvt2.d = BV_Distance(bvt2.R,bvt2.T,
01237                             o1->child(bvt2.b1),o2->child(bvt2.b2));
01238       }
01239       else 
01240       {
01241         // add two new tests to queue, consisting of min_test.b1
01242         // with the children of min_test.b2
01243 
01244         int c1 = o2->child(min_test.b2)->first_child;
01245         int c2 = c1 + 1;
01246 
01247         // init bv test 1
01248 
01249         bvt1.b1 = min_test.b1;
01250         bvt1.b2 = c1;
01251         MxM(bvt1.R,min_test.R,o2->child(c1)->R);
01252 #if PQP_BV_TYPE & RSS_TYPE
01253         MxVpV(bvt1.T,min_test.R,o2->child(c1)->Tr,min_test.T);
01254 #else
01255         MxVpV(bvt1.T,min_test.R,o2->child(c1)->To,min_test.T);
01256 #endif
01257         bvt1.d = BV_Distance(bvt1.R,bvt1.T,
01258                             o1->child(bvt1.b1),o2->child(bvt1.b2));
01259 
01260         // init bv test 2
01261 
01262         bvt2.b1 = min_test.b1;
01263         bvt2.b2 = c2;
01264         MxM(bvt2.R,min_test.R,o2->child(c2)->R);
01265 #if PQP_BV_TYPE & RSS_TYPE
01266         MxVpV(bvt2.T,min_test.R,o2->child(c2)->Tr,min_test.T);
01267 #else
01268         MxVpV(bvt2.T,min_test.R,o2->child(c2)->To,min_test.T);
01269 #endif
01270         bvt2.d = BV_Distance(bvt2.R,bvt2.T,
01271                             o1->child(bvt2.b1),o2->child(bvt2.b2));
01272       }
01273 
01274       // put children tests in queue
01275 
01276       if (bvt1.d <= res->tolerance) bvtq.AddTest(bvt1);
01277       if (bvt2.d <= res->tolerance) bvtq.AddTest(bvt2);
01278     }
01279 
01280     if (bvtq.Empty() || (bvtq.MinTest() > res->tolerance)) 
01281     {
01282       res->closer_than_tolerance = 0;
01283       return;
01284     }
01285     else 
01286     {
01287       min_test = bvtq.ExtractMinTest();
01288     }
01289   }  
01290 }       
01291 
01292 int
01293 PQP_Tolerance(PQP_ToleranceResult *res,
01294               PQP_REAL R1[3][3], PQP_REAL T1[3], PQP_Model *o1,
01295               PQP_REAL R2[3][3], PQP_REAL T2[3], PQP_Model *o2,
01296               PQP_REAL tolerance,
01297               int qsize)
01298 {
01299   double time1 = GetTime();
01300 
01301   // make sure that the models are built
01302 
01303   if (o1->build_state != PQP_BUILD_STATE_PROCESSED) 
01304     return PQP_ERR_UNPROCESSED_MODEL;
01305   if (o2->build_state != PQP_BUILD_STATE_PROCESSED) 
01306     return PQP_ERR_UNPROCESSED_MODEL;
01307   
01308   // Compute the transform [R,T] that takes us from cs2 to cs1.
01309   // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
01310 
01311   MTxM(res->R,R1,R2);
01312   PQP_REAL Ttemp[3];
01313   VmV(Ttemp, T2, T1);
01314   MTxV(res->T, R1, Ttemp);
01315 
01316   // set tolerance, used to prune the search
01317 
01318   if (tolerance < 0.0) tolerance = 0.0;
01319   res->tolerance = tolerance;
01320   
01321   // clear the stats
01322 
01323   res->num_bv_tests = 0;
01324   res->num_tri_tests = 0;
01325 
01326   // initially assume not closer than tolerance
01327 
01328   res->closer_than_tolerance = 0;
01329   
01330   // compute the transform from o1->child(0) to o2->child(0)
01331 
01332   PQP_REAL Rtemp[3][3], R[3][3], T[3];
01333 
01334   MxM(Rtemp,res->R,o2->child(0)->R);
01335   MTxM(R,o1->child(0)->R,Rtemp);
01336 #if PQP_BV_TYPE & RSS_TYPE
01337   MxVpV(Ttemp,res->R,o2->child(0)->Tr,res->T);
01338   VmV(Ttemp,Ttemp,o1->child(0)->Tr);
01339 #else
01340   MxVpV(Ttemp,res->R,o2->child(0)->To,res->T);
01341   VmV(Ttemp,Ttemp,o1->child(0)->To);
01342 #endif
01343   MTxV(T,o1->child(0)->R,Ttemp);
01344 
01345   // find a distance lower bound for trivial reject
01346 
01347   PQP_REAL d = BV_Distance(R, T, o1->child(0), o2->child(0));
01348   
01349   if (d <= res->tolerance)
01350   {
01351     // more work needed - choose routine according to queue size
01352 
01353     if (qsize <= 2) 
01354     {
01355       ToleranceRecurse(res, R, T, o1, 0, o2, 0);
01356     }
01357     else 
01358     {
01359       res->qsize = qsize;
01360       ToleranceQueueRecurse(res, R, T, o1, 0, o2, 0);
01361     }
01362   }
01363 
01364   // res->p2 is in cs 1 ; transform it to cs 2
01365 
01366   PQP_REAL u[3];
01367   VmV(u, res->p2, res->T);
01368   MTxV(res->p2, res->R, u);
01369 
01370   double time2 = GetTime();
01371   res->query_time_secs = time2 - time1;
01372 
01373   return PQP_OK;
01374 }
01375 
01376 #endif


jskeus
Author(s): JSK Alumnis
autogenerated on Tue Mar 7 2017 04:04:34