BV.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:             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 <stdlib.h>
00042 #include <math.h>
00043 #include "BV.h"
00044 #include "MatVec.h"
00045 #include "RectDist.h"
00046 #include "OBB_Disjoint.h"
00047 
00048 BV::BV()
00049 {
00050   first_child = 0;
00051 }
00052 
00053 BV::~BV()
00054 {
00055 }
00056 
00057 static
00058 inline 
00059 PQP_REAL 
00060 MaxOfTwo(PQP_REAL a, PQP_REAL b) 
00061 {
00062   if (a > b) return a;
00063   return b;
00064 }
00065 
00066 void
00067 BV::FitToTris(PQP_REAL O[3][3], Tri *tris, int num_tris)
00068 {
00069   // store orientation
00070 
00071   McM(R,O);
00072 
00073   // project points of tris to R coordinates
00074 
00075   int num_points = 3*num_tris;
00076   PQP_REAL (*P)[3] = new PQP_REAL[num_points][3];
00077   int point = 0;
00078   int i;
00079   for (i = 0; i < num_tris; i++) 
00080   {
00081     MTxV(P[point],R,tris[i].p1);
00082     point++;
00083 
00084     MTxV(P[point],R,tris[i].p2);
00085     point++;
00086 
00087     MTxV(P[point],R,tris[i].p3);
00088     point++;
00089   }
00090 
00091   PQP_REAL minx, maxx, miny, maxy, minz, maxz, c[3];
00092 
00093 #if PQP_BV_TYPE & OBB_TYPE
00094   minx = maxx = P[0][0];
00095   miny = maxy = P[0][1];
00096   minz = maxz = P[0][2];
00097   for (i = 1; i < num_points; i++)
00098   {
00099     if (P[i][0] < minx) minx = P[i][0];
00100     else if (P[i][0] > maxx) maxx = P[i][0];
00101     if (P[i][1] < miny) miny = P[i][1];
00102     else if (P[i][1] > maxy) maxy = P[i][1];
00103     if (P[i][2] < minz) minz = P[i][2];
00104     else if (P[i][2] > maxz) maxz = P[i][2];
00105   }
00106   c[0] = (PQP_REAL)0.5*(maxx + minx);
00107   c[1] = (PQP_REAL)0.5*(maxy + miny);
00108   c[2] = (PQP_REAL)0.5*(maxz + minz);
00109   MxV(To,R,c);
00110 
00111   d[0] = (PQP_REAL)0.5*(maxx - minx);
00112   d[1] = (PQP_REAL)0.5*(maxy - miny);
00113   d[2] = (PQP_REAL)0.5*(maxz - minz);
00114 #endif
00115   
00116 #if PQP_BV_TYPE & RSS_TYPE
00117 
00118   // compute thickness, which determines radius, and z of rectangle corner
00119   
00120   PQP_REAL cz,radsqr;
00121   minz = maxz = P[0][2];
00122   for (i = 1; i < num_points; i++) 
00123   {
00124     if (P[i][2] < minz) minz = P[i][2];
00125     else if (P[i][2] > maxz) maxz = P[i][2];
00126   }
00127   r = (PQP_REAL)0.5*(maxz - minz);
00128   radsqr = r*r;
00129   cz = (PQP_REAL)0.5*(maxz + minz);
00130 
00131   // compute an initial length of rectangle along x direction
00132 
00133   // find minx and maxx as starting points
00134 
00135   int minindex, maxindex;
00136   minindex = maxindex = 0;
00137   for (i = 1; i < num_points; i++) 
00138   {
00139     if (P[i][0] < P[minindex][0]) minindex = i; 
00140     else if (P[i][0] > P[maxindex][0]) maxindex = i;
00141   }
00142   PQP_REAL x, dz;
00143   dz = P[minindex][2] - cz;
00144   minx = P[minindex][0] + sqrt(MaxOfTwo(radsqr - dz*dz,0));
00145   dz = P[maxindex][2] - cz;
00146   maxx = P[maxindex][0] - sqrt(MaxOfTwo(radsqr - dz*dz,0));
00147 
00148   // grow minx
00149 
00150   for (i = 0; i < num_points; i++) 
00151   {
00152     if (P[i][0] < minx) 
00153     {
00154       dz = P[i][2] - cz;
00155       x = P[i][0] + sqrt(MaxOfTwo(radsqr - dz*dz,0));
00156       if (x < minx) minx = x;
00157     }
00158   }
00159 
00160   // grow maxx
00161 
00162   for (i = 0; i < num_points; i++) 
00163   {
00164     if (P[i][0] > maxx) 
00165     {
00166       dz = P[i][2] - cz;
00167       x = P[i][0] - sqrt(MaxOfTwo(radsqr - dz*dz,0));
00168       if (x > maxx) maxx = x;
00169     }
00170   }
00171   
00172   // compute an initial length of rectangle along y direction
00173 
00174   // find miny and maxy as starting points
00175 
00176   minindex = maxindex = 0;
00177   for (i = 1; i < num_points; i++) 
00178   {
00179     if (P[i][1] < P[minindex][1]) minindex = i;
00180     else if (P[i][1] > P[maxindex][1]) maxindex = i;
00181   }
00182   PQP_REAL y;
00183   dz = P[minindex][2] - cz;
00184   miny = P[minindex][1] + sqrt(MaxOfTwo(radsqr - dz*dz,0));
00185   dz = P[maxindex][2] - cz;
00186   maxy = P[maxindex][1] - sqrt(MaxOfTwo(radsqr - dz*dz,0));
00187 
00188   // grow miny
00189 
00190   for (i = 0; i < num_points; i++) 
00191   {
00192     if (P[i][1] < miny) 
00193     {
00194       dz = P[i][2] - cz;
00195       y = P[i][1] + sqrt(MaxOfTwo(radsqr - dz*dz,0));
00196       if (y < miny) miny = y;
00197     }
00198   }
00199 
00200   // grow maxy
00201 
00202   for (i = 0; i < num_points; i++) 
00203   {
00204     if (P[i][1] > maxy) 
00205     {
00206       dz = P[i][2] - cz;
00207       y = P[i][1] - sqrt(MaxOfTwo(radsqr - dz*dz,0));
00208       if (y > maxy) maxy = y;
00209     }
00210   }
00211   
00212   // corners may have some points which are not covered - grow lengths if
00213   // necessary
00214   
00215   PQP_REAL dx, dy, u, t;
00216   PQP_REAL a = sqrt((PQP_REAL)0.5);
00217   for (i = 0; i < num_points; i++) 
00218   {
00219     if (P[i][0] > maxx) 
00220     {
00221       if (P[i][1] > maxy) 
00222       {
00223         dx = P[i][0] - maxx;
00224         dy = P[i][1] - maxy;
00225         u = dx*a + dy*a;
00226         t = (a*u - dx)*(a*u - dx) + 
00227             (a*u - dy)*(a*u - dy) +
00228             (cz - P[i][2])*(cz - P[i][2]);
00229         u = u - sqrt(MaxOfTwo(radsqr - t,0));
00230         if (u > 0) 
00231         {
00232           maxx += u*a;
00233           maxy += u*a;
00234         }
00235       }
00236       else if (P[i][1] < miny) 
00237       {
00238         dx = P[i][0] - maxx;
00239         dy = P[i][1] - miny;
00240         u = dx*a - dy*a;
00241         t = (a*u - dx)*(a*u - dx) + 
00242             (-a*u - dy)*(-a*u - dy) +
00243             (cz - P[i][2])*(cz - P[i][2]);
00244         u = u - sqrt(MaxOfTwo(radsqr - t,0));
00245         if (u > 0) 
00246         {
00247           maxx += u*a;
00248           miny -= u*a;
00249         }
00250       }
00251     }
00252     else if (P[i][0] < minx) 
00253     {
00254       if (P[i][1] > maxy) 
00255       {
00256         dx = P[i][0] - minx;
00257         dy = P[i][1] - maxy;
00258         u = dy*a - dx*a;
00259         t = (-a*u - dx)*(-a*u - dx) + 
00260             (a*u - dy)*(a*u - dy) +
00261             (cz - P[i][2])*(cz - P[i][2]);
00262         u = u - sqrt(MaxOfTwo(radsqr - t,0));
00263         if (u > 0) 
00264         {
00265           minx -= u*a;
00266           maxy += u*a;
00267         }     
00268       }
00269       else if (P[i][1] < miny) 
00270       {
00271         dx = P[i][0] - minx;
00272         dy = P[i][1] - miny;
00273         u = -dx*a - dy*a;
00274         t = (-a*u - dx)*(-a*u - dx) + 
00275             (-a*u - dy)*(-a*u - dy) +
00276             (cz - P[i][2])*(cz - P[i][2]);
00277         u = u - sqrt(MaxOfTwo(radsqr - t,0));
00278         if (u > 0) 
00279         {
00280           minx -= u*a; 
00281           miny -= u*a;
00282         }
00283       }
00284     }
00285   }
00286 
00287   c[0] = minx;
00288   c[1] = miny;
00289   c[2] = cz;
00290   MxV(Tr,R,c);
00291 
00292   l[0] = maxx - minx;  
00293   if (l[0] < 0) l[0] = 0;
00294   l[1] = maxy - miny;
00295   if (l[1] < 0) l[1] = 0;
00296 #endif
00297 
00298   delete [] P;
00299 }
00300 
00301 int 
00302 BV_Overlap(PQP_REAL R[3][3], PQP_REAL T[3], BV *b1, BV *b2)
00303 {
00304 #if PQP_BV_TYPE & OBB_TYPE
00305   return (obb_disjoint(R,T,b1->d,b2->d) == 0);
00306 #else
00307   PQP_REAL dist = RectDist(R,T,b1->l,b2->l);
00308   if (dist <= (b1->r + b2->r)) return 1;
00309   return 0;
00310 #endif
00311 }
00312 
00313 #if PQP_BV_TYPE & RSS_TYPE
00314 PQP_REAL
00315 BV_Distance(PQP_REAL R[3][3], PQP_REAL T[3], BV *b1, BV *b2)
00316 {
00317   PQP_REAL dist = RectDist(R,T,b1->l,b2->l);
00318   dist -= (b1->r + b2->r);
00319   return (dist < (PQP_REAL)0.0)? (PQP_REAL)0.0 : dist;
00320 }
00321 #endif
00322 
00323 


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