RectDist.h
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 #ifndef PQP_RECTDIST_H
00042 #define PQP_RECTDIST_H
00043 
00044 #include <math.h>
00045 #include "MatVec.h" 
00046 #include "PQP_Compile.h"
00047   
00048 // ClipToRange
00049 //
00050 // clips val between a and b
00051 
00052 inline 
00053 void
00054 ClipToRange(PQP_REAL &val, const PQP_REAL &a, const PQP_REAL &b)
00055 {
00056   if (val < a) val = a;
00057   else if (val > b) val = b;
00058 }
00059 
00060 // SegCoords
00061 //
00062 // finds the parameters t & u corresponding to the two closest points 
00063 // on a pair of line segments 
00064 //
00065 // The first segment is defined as 
00066 //
00067 // Pa + A*t, 0 <= t <= a, 
00068 // 
00069 // where "Pa" is one endpoint of the segment, "A" is a unit vector 
00070 // pointing to the other endpoint, and t is a scalar that produces
00071 // all the points between the two endpoints. Since "A" is a unit
00072 // vector, "a" is the segment's length.
00073 //
00074 // The second segment is 
00075 //
00076 // Pb + B*u, 0 <= u <= b
00077 //
00078 // In my application, many of the terms needed by the algorithm
00079 // are already computed for other purposes, so I pass these terms to 
00080 // the function instead of complete specifications of each segment. 
00081 // "T" in the dot products is the vector between Pa and Pb.
00082 //
00083 // The algorithm is from
00084 //
00085 // Vladimir J. Lumelsky,
00086 // On fast computation of distance between line segments.
00087 // In Information Processing Letters, no. 21, pages 55-61, 1985.   
00088 
00089 inline
00090 void 
00091 SegCoords(PQP_REAL& t, PQP_REAL& u, 
00092           const PQP_REAL& a, const PQP_REAL& b, 
00093           const PQP_REAL& A_dot_B, 
00094           const PQP_REAL& A_dot_T, 
00095           const PQP_REAL& B_dot_T)
00096 {  
00097   PQP_REAL denom = 1 - (A_dot_B)*(A_dot_B);
00098 
00099   if (denom == 0) t = 0;
00100   else
00101   {
00102     t = (A_dot_T - B_dot_T*A_dot_B)/denom;
00103     ClipToRange(t,0,a);
00104   }
00105   
00106   u = t*A_dot_B - B_dot_T;
00107   if (u < 0) 
00108   {
00109     u = 0;
00110     t = A_dot_T;
00111     ClipToRange(t,0,a);
00112   }
00113   else if (u > b) 
00114   {
00115     u = b;
00116     t = u*A_dot_B + A_dot_T;
00117     ClipToRange(t,0,a);
00118   }
00119 }
00120 
00121 // InVoronoi
00122 //
00123 // returns whether the nearest point on rectangle edge 
00124 // Pb + B*u, 0 <= u <= b, to the rectangle edge,
00125 // Pa + A*t, 0 <= t <= a, is within the half space 
00126 // determined by the point Pa and the direction Anorm.
00127 //
00128 // A,B, and Anorm are unit vectors.
00129 // T is the vector between Pa and Pb.
00130 
00131 inline
00132 int 
00133 InVoronoi(const PQP_REAL &a, 
00134           const PQP_REAL &b,  
00135           const PQP_REAL &Anorm_dot_B, 
00136           const PQP_REAL &Anorm_dot_T,  
00137           const PQP_REAL &A_dot_B,
00138           const PQP_REAL &A_dot_T,
00139           const PQP_REAL &B_dot_T)
00140 { 
00141   if (myfabs(Anorm_dot_B) < 1e-7) return 0;
00142 
00143   PQP_REAL t, u, v;
00144  
00145   u = -Anorm_dot_T / Anorm_dot_B; 
00146   ClipToRange(u,0,b); 
00147   
00148   t = u*A_dot_B + A_dot_T; 
00149   ClipToRange(t,0,a); 
00150   
00151   v = t*A_dot_B - B_dot_T; 
00152   
00153   if (Anorm_dot_B > 0) 
00154   {
00155     if (v > (u + 1e-7)) return 1;
00156   }
00157   else 
00158   {
00159     if (v < (u - 1e-7)) return 1;
00160   }
00161   return 0; 
00162 } 
00163 
00164 
00165 // RectDist
00166 //
00167 // Finds the distance between two rectangles A and B.  A is assumed
00168 // to have its corner on the origin, one side aligned with
00169 // x, the other side aligned with y, and its normal aligned with z.
00170 // 
00171 // [Rab,Tab] gives the orientation and corner position of rectangle B
00172 // 
00173 // a[2] are the side lengths of A, b[2] are the side lengths of B
00174 
00175 inline
00176 PQP_REAL
00177 RectDist(PQP_REAL Rab[3][3], PQP_REAL Tab[3], 
00178           PQP_REAL a[2], PQP_REAL b[2])
00179 {
00180   PQP_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
00181 
00182   A0_dot_B0 = Rab[0][0];
00183   A0_dot_B1 = Rab[0][1];
00184   A1_dot_B0 = Rab[1][0];
00185   A1_dot_B1 = Rab[1][1];
00186 
00187   PQP_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
00188   PQP_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1; 
00189  
00190   aA0_dot_B0 = a[0]*A0_dot_B0;
00191   aA0_dot_B1 = a[0]*A0_dot_B1;
00192   aA1_dot_B0 = a[1]*A1_dot_B0;
00193   aA1_dot_B1 = a[1]*A1_dot_B1;
00194   bA0_dot_B0 = b[0]*A0_dot_B0;
00195   bA1_dot_B0 = b[0]*A1_dot_B0;
00196   bA0_dot_B1 = b[1]*A0_dot_B1;
00197   bA1_dot_B1 = b[1]*A1_dot_B1;
00198 
00199   PQP_REAL Tba[3];
00200   MTxV(Tba,Rab,Tab);
00201 
00202   PQP_REAL S[3], t, u;
00203 
00204   // determine if any edge pair contains the closest points
00205 
00206   PQP_REAL ALL_x, ALU_x, AUL_x, AUU_x;
00207   PQP_REAL BLL_x, BLU_x, BUL_x, BUU_x;
00208   PQP_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
00209 
00210   ALL_x = -Tba[0];
00211   ALU_x = ALL_x + aA1_dot_B0;
00212   AUL_x = ALL_x + aA0_dot_B0;
00213   AUU_x = ALU_x + aA0_dot_B0;
00214 
00215   if (ALL_x < ALU_x)
00216   { 
00217     LA1_lx = ALL_x;
00218     LA1_ux = ALU_x;
00219     UA1_lx = AUL_x;    
00220     UA1_ux = AUU_x;
00221   }
00222   else
00223   { 
00224     LA1_lx = ALU_x;
00225     LA1_ux = ALL_x;
00226     UA1_lx = AUU_x;    
00227     UA1_ux = AUL_x;
00228   }
00229 
00230   BLL_x = Tab[0];
00231   BLU_x = BLL_x + bA0_dot_B1;
00232   BUL_x = BLL_x + bA0_dot_B0;
00233   BUU_x = BLU_x + bA0_dot_B0;
00234   
00235   if (BLL_x < BLU_x)
00236   { 
00237     LB1_lx = BLL_x;
00238     LB1_ux = BLU_x;
00239     UB1_lx = BUL_x;    
00240     UB1_ux = BUU_x;
00241   }
00242   else
00243   { 
00244     LB1_lx = BLU_x;
00245     LB1_ux = BLL_x;
00246     UB1_lx = BUU_x;    
00247     UB1_ux = BUL_x;
00248   }
00249 
00250   // UA1, UB1
00251   
00252   if ((UA1_ux > b[0]) && (UB1_ux > a[0]))
00253   {
00254     if (((UA1_lx > b[0]) || 
00255           InVoronoi(b[1],a[1],A1_dot_B0,aA0_dot_B0 - b[0] - Tba[0],
00256                                 A1_dot_B1, aA0_dot_B1 - Tba[1], 
00257                     -Tab[1] - bA1_dot_B0))
00258         &&
00259         
00260         ((UB1_lx > a[0]) || 
00261           InVoronoi(a[1],b[1],A0_dot_B1,Tab[0] + bA0_dot_B0 - a[0],
00262                     A1_dot_B1,Tab[1] + bA1_dot_B0,Tba[1] - aA0_dot_B1)))
00263     {            
00264       SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1] + bA1_dot_B0,
00265                 Tba[1] - aA0_dot_B1);
00266       
00267       S[0] = Tab[0] + Rab[0][0]*b[0] + Rab[0][1]*u - a[0] ;
00268       S[1] = Tab[1] + Rab[1][0]*b[0] + Rab[1][1]*u - t;
00269       S[2] = Tab[2] + Rab[2][0]*b[0] + Rab[2][1]*u;
00270       return sqrt(VdotV(S,S));
00271     }    
00272   }
00273 
00274 
00275   // UA1, LB1
00276 
00277   if ((UA1_lx < 0) && (LB1_ux > a[0]))
00278   {
00279     if (((UA1_ux < 0) ||
00280           InVoronoi(b[1],a[1],-A1_dot_B0,Tba[0] - aA0_dot_B0,
00281                     A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1]))
00282         &&
00283 
00284         ((LB1_lx > a[0]) ||
00285           InVoronoi(a[1],b[1],A0_dot_B1,Tab[0] - a[0],
00286                     A1_dot_B1,Tab[1],Tba[1] - aA0_dot_B1)))
00287     {
00288       SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1],Tba[1] - aA0_dot_B1);
00289 
00290       S[0] = Tab[0] + Rab[0][1]*u - a[0];
00291       S[1] = Tab[1] + Rab[1][1]*u - t;
00292       S[2] = Tab[2] + Rab[2][1]*u;
00293       return sqrt(VdotV(S,S));
00294     }
00295   }
00296 
00297   // LA1, UB1
00298 
00299   if ((LA1_ux > b[0]) && (UB1_lx < 0))
00300   {
00301     if (((LA1_lx > b[0]) || 
00302           InVoronoi(b[1],a[1],A1_dot_B0,-Tba[0] - b[0],
00303                     A1_dot_B1,-Tba[1], -Tab[1] - bA1_dot_B0))
00304           &&
00305         
00306         ((UB1_ux < 0) || 
00307           InVoronoi(a[1],b[1],-A0_dot_B1, -Tab[0] - bA0_dot_B0,
00308                     A1_dot_B1, Tab[1] + bA1_dot_B0,Tba[1])))
00309     {
00310 
00311       SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1] + bA1_dot_B0,Tba[1]);
00312 
00313       S[0] = Tab[0] + Rab[0][0]*b[0] + Rab[0][1]*u;
00314       S[1] = Tab[1] + Rab[1][0]*b[0] + Rab[1][1]*u - t;
00315       S[2] = Tab[2] + Rab[2][0]*b[0] + Rab[2][1]*u;
00316       return sqrt(VdotV(S,S));
00317     }
00318   }
00319 
00320   // LA1, LB1
00321 
00322   if ((LA1_lx < 0) && (LB1_lx < 0))
00323   {   
00324     if (((LA1_ux < 0) || 
00325           InVoronoi(b[1],a[1],-A1_dot_B0,Tba[0],A1_dot_B1,
00326                     -Tba[1],-Tab[1]))
00327           &&
00328 
00329         ((LB1_ux < 0) || 
00330           InVoronoi(a[1],b[1],-A0_dot_B1,-Tab[0],A1_dot_B1,
00331                     Tab[1], Tba[1])))
00332     {
00333       SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1],Tba[1]);    
00334 
00335       S[0] = Tab[0] + Rab[0][1]*u;
00336       S[1] = Tab[1] + Rab[1][1]*u - t;
00337       S[2] = Tab[2] + Rab[2][1]*u;
00338       return sqrt(VdotV(S,S));
00339     }
00340   }
00341 
00342   PQP_REAL ALL_y, ALU_y, AUL_y, AUU_y;
00343 
00344   ALL_y = -Tba[1];
00345   ALU_y = ALL_y + aA1_dot_B1;
00346   AUL_y = ALL_y + aA0_dot_B1;
00347   AUU_y = ALU_y + aA0_dot_B1;
00348   
00349   PQP_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
00350 
00351   if (ALL_y < ALU_y)
00352   { 
00353     LA1_ly = ALL_y;
00354     LA1_uy = ALU_y;
00355     UA1_ly = AUL_y;    
00356     UA1_uy = AUU_y;
00357   }
00358   else
00359   { 
00360     LA1_ly = ALU_y;
00361     LA1_uy = ALL_y;
00362     UA1_ly = AUU_y;    
00363     UA1_uy = AUL_y;
00364   }
00365 
00366   if (BLL_x < BUL_x)
00367   {
00368     LB0_lx = BLL_x;
00369     LB0_ux = BUL_x;
00370     UB0_lx = BLU_x;
00371     UB0_ux = BUU_x;
00372   }
00373   else
00374   {
00375     LB0_lx = BUL_x;
00376     LB0_ux = BLL_x;
00377     UB0_lx = BUU_x;
00378     UB0_ux = BLU_x;
00379   }
00380 
00381   // UA1, UB0
00382 
00383   if ((UA1_uy > b[1]) && (UB0_ux > a[0]))
00384   {   
00385     if (((UA1_ly > b[1]) || 
00386           InVoronoi(b[0],a[1],A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1],
00387                     A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1))
00388           &&
00389         
00390         ((UB0_lx > a[0]) || 
00391           InVoronoi(a[1],b[0],A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1,
00392                     A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0)))
00393     {
00394       SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1] + bA1_dot_B1,
00395                 Tba[0] - aA0_dot_B0);
00396 
00397       S[0] = Tab[0] + Rab[0][1]*b[1] + Rab[0][0]*u - a[0] ;
00398       S[1] = Tab[1] + Rab[1][1]*b[1] + Rab[1][0]*u - t;
00399       S[2] = Tab[2] + Rab[2][1]*b[1] + Rab[2][0]*u;
00400       return sqrt(VdotV(S,S));
00401     }
00402   }
00403 
00404   // UA1, LB0
00405 
00406   if ((UA1_ly < 0) && (LB0_ux > a[0]))
00407   {
00408     if (((UA1_uy < 0) || 
00409           InVoronoi(b[0],a[1],-A1_dot_B1, Tba[1] - aA0_dot_B1,A1_dot_B0,
00410                     aA0_dot_B0 - Tba[0], -Tab[1]))
00411           &&
00412 
00413         ((LB0_lx > a[0]) || 
00414           InVoronoi(a[1],b[0],A0_dot_B0,Tab[0] - a[0],
00415                     A1_dot_B0,Tab[1],Tba[0] - aA0_dot_B0)))
00416     {
00417       SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1],Tba[0] - aA0_dot_B0);
00418 
00419       S[0] = Tab[0] + Rab[0][0]*u - a[0];
00420       S[1] = Tab[1] + Rab[1][0]*u - t;
00421       S[2] = Tab[2] + Rab[2][0]*u;
00422       return sqrt(VdotV(S,S)); 
00423     }
00424   }
00425 
00426   // LA1, UB0
00427 
00428   if ((LA1_uy > b[1]) && (UB0_lx < 0))
00429   {
00430     if (((LA1_ly > b[1]) || 
00431         InVoronoi(b[0],a[1],A1_dot_B1,-Tba[1] - b[1],
00432                   A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1))     
00433         &&
00434 
00435         ((UB0_ux < 0) ||             
00436           InVoronoi(a[1],b[0],-A0_dot_B0, -Tab[0] - bA0_dot_B1,A1_dot_B0,
00437                     Tab[1] + bA1_dot_B1,Tba[0])))
00438     {
00439       SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1] + bA1_dot_B1,Tba[0]);
00440 
00441       S[0] = Tab[0] + Rab[0][1]*b[1] + Rab[0][0]*u;
00442       S[1] = Tab[1] + Rab[1][1]*b[1] + Rab[1][0]*u - t;
00443       S[2] = Tab[2] + Rab[2][1]*b[1] + Rab[2][0]*u;
00444       return sqrt(VdotV(S,S));
00445     }
00446   }
00447 
00448   // LA1, LB0
00449 
00450   if ((LA1_ly < 0) && (LB0_lx < 0))
00451   {
00452     if (((LA1_uy < 0) || 
00453           InVoronoi(b[0],a[1],-A1_dot_B1,Tba[1],A1_dot_B0,
00454                     -Tba[0],-Tab[1]))
00455         && 
00456 
00457         ((LB0_ux < 0) || 
00458           InVoronoi(a[1],b[0],-A0_dot_B0,-Tab[0],A1_dot_B0,
00459                     Tab[1],Tba[0])))
00460     {
00461       SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1],Tba[0]);
00462         
00463       S[0] = Tab[0] + Rab[0][0]*u;
00464       S[1] = Tab[1] + Rab[1][0]*u - t;
00465       S[2] = Tab[2] + Rab[2][0]*u;
00466       return sqrt(VdotV(S,S));
00467     }
00468   }
00469 
00470   PQP_REAL BLL_y, BLU_y, BUL_y, BUU_y;
00471 
00472   BLL_y = Tab[1];
00473   BLU_y = BLL_y + bA1_dot_B1;
00474   BUL_y = BLL_y + bA1_dot_B0;
00475   BUU_y = BLU_y + bA1_dot_B0;
00476 
00477   PQP_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
00478 
00479   if (ALL_x < AUL_x)
00480   {
00481     LA0_lx = ALL_x;
00482     LA0_ux = AUL_x;
00483     UA0_lx = ALU_x;
00484     UA0_ux = AUU_x;
00485   }
00486   else
00487   {
00488     LA0_lx = AUL_x;
00489     LA0_ux = ALL_x;
00490     UA0_lx = AUU_x;
00491     UA0_ux = ALU_x;
00492   }
00493 
00494   if (BLL_y < BLU_y)
00495   {
00496     LB1_ly = BLL_y;
00497     LB1_uy = BLU_y;
00498     UB1_ly = BUL_y;
00499     UB1_uy = BUU_y;
00500   }
00501   else
00502   {
00503     LB1_ly = BLU_y;
00504     LB1_uy = BLL_y;
00505     UB1_ly = BUU_y;
00506     UB1_uy = BUL_y;
00507   }
00508     
00509   // UA0, UB1
00510   
00511   if ((UA0_ux > b[0]) && (UB1_uy > a[1]))
00512   {
00513     if (((UA0_lx > b[0]) || 
00514           InVoronoi(b[1],a[0],A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0],
00515                     A0_dot_B1,aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0))
00516         &&
00517         
00518         ((UB1_ly > a[1]) || 
00519           InVoronoi(a[0],b[1],A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0,
00520                     A0_dot_B1,Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1)))
00521     {
00522       SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0] + bA0_dot_B0,
00523                 Tba[1] - aA1_dot_B1);
00524     
00525       S[0] = Tab[0] + Rab[0][0]*b[0] + Rab[0][1]*u - t;
00526       S[1] = Tab[1] + Rab[1][0]*b[0] + Rab[1][1]*u - a[1];
00527       S[2] = Tab[2] + Rab[2][0]*b[0] + Rab[2][1]*u;
00528       return sqrt(VdotV(S,S));
00529     }
00530   }
00531 
00532   // UA0, LB1
00533 
00534   if ((UA0_lx < 0) && (LB1_uy > a[1]))
00535   {
00536     if (((UA0_ux < 0) || 
00537           InVoronoi(b[1],a[0],-A0_dot_B0, Tba[0] - aA1_dot_B0,A0_dot_B1,
00538                     aA1_dot_B1 - Tba[1],-Tab[0]))
00539         &&
00540 
00541         ((LB1_ly > a[1]) || 
00542           InVoronoi(a[0],b[1],A1_dot_B1,Tab[1] - a[1],A0_dot_B1,Tab[0],
00543                     Tba[1] - aA1_dot_B1)))
00544     {
00545       SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0],Tba[1] - aA1_dot_B1);
00546 
00547       S[0] = Tab[0] + Rab[0][1]*u - t;
00548       S[1] = Tab[1] + Rab[1][1]*u - a[1];
00549       S[2] = Tab[2] + Rab[2][1]*u;
00550       return sqrt(VdotV(S,S));
00551     }
00552   }
00553 
00554   // LA0, UB1
00555 
00556   if ((LA0_ux > b[0]) && (UB1_ly < 0))
00557   {
00558     if (((LA0_lx > b[0]) || 
00559           InVoronoi(b[1],a[0],A0_dot_B0,-b[0] - Tba[0],A0_dot_B1,-Tba[1],
00560                     -bA0_dot_B0 - Tab[0]))
00561         &&
00562 
00563         ((UB1_uy < 0) || 
00564           InVoronoi(a[0],b[1],-A1_dot_B1, -Tab[1] - bA1_dot_B0,A0_dot_B1,
00565                     Tab[0] + bA0_dot_B0,Tba[1])))
00566     {
00567       SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0] + bA0_dot_B0,Tba[1]);
00568 
00569       S[0] = Tab[0] + Rab[0][0]*b[0] + Rab[0][1]*u - t;
00570       S[1] = Tab[1] + Rab[1][0]*b[0] + Rab[1][1]*u;
00571       S[2] = Tab[2] + Rab[2][0]*b[0] + Rab[2][1]*u;
00572       return sqrt(VdotV(S,S));
00573     }
00574   }
00575   
00576   // LA0, LB1
00577 
00578   if ((LA0_lx < 0) && (LB1_ly < 0))
00579   {
00580     if (((LA0_ux < 0) || 
00581           InVoronoi(b[1],a[0],-A0_dot_B0,Tba[0],A0_dot_B1,-Tba[1],
00582                     -Tab[0]))
00583         &&
00584         
00585         ((LB1_uy < 0) || 
00586           InVoronoi(a[0],b[1],-A1_dot_B1,-Tab[1],A0_dot_B1,
00587                     Tab[0],Tba[1])))
00588     {
00589       SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0],Tba[1]);
00590 
00591       S[0] = Tab[0] + Rab[0][1]*u - t;
00592       S[1] = Tab[1] + Rab[1][1]*u;
00593       S[2] = Tab[2] + Rab[2][1]*u;
00594       return sqrt(VdotV(S,S));
00595     }
00596   }
00597 
00598   PQP_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
00599 
00600   if (ALL_y < AUL_y)
00601   {
00602     LA0_ly = ALL_y;
00603     LA0_uy = AUL_y;
00604     UA0_ly = ALU_y;
00605     UA0_uy = AUU_y;
00606   }
00607   else
00608   {
00609     LA0_ly = AUL_y;
00610     LA0_uy = ALL_y;
00611     UA0_ly = AUU_y;
00612     UA0_uy = ALU_y;
00613   }
00614 
00615   if (BLL_y < BUL_y)
00616   {
00617     LB0_ly = BLL_y;
00618     LB0_uy = BUL_y;
00619     UB0_ly = BLU_y;
00620     UB0_uy = BUU_y;
00621   }
00622   else
00623   {
00624     LB0_ly = BUL_y;
00625     LB0_uy = BLL_y;
00626     UB0_ly = BUU_y;
00627     UB0_uy = BLU_y;
00628   }
00629 
00630   // UA0, UB0
00631 
00632   if ((UA0_uy > b[1]) && (UB0_uy > a[1]))
00633   {
00634     if (((UA0_ly > b[1]) || 
00635           InVoronoi(b[0],a[0],A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1],
00636                     A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1))
00637         &&
00638         
00639         ((UB0_ly > a[1]) || 
00640           InVoronoi(a[0],b[0],A1_dot_B0,Tab[1] - a[1] + bA1_dot_B1,A0_dot_B0,
00641                     Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0)))
00642     {
00643       SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0] + bA0_dot_B1,
00644                 Tba[0] - aA1_dot_B0);
00645       
00646       S[0] = Tab[0] + Rab[0][1]*b[1] + Rab[0][0]*u - t;
00647       S[1] = Tab[1] + Rab[1][1]*b[1] + Rab[1][0]*u - a[1];
00648       S[2] = Tab[2] + Rab[2][1]*b[1] + Rab[2][0]*u;
00649       return sqrt(VdotV(S,S));
00650     }
00651   }
00652 
00653   // UA0, LB0
00654 
00655   if ((UA0_ly < 0) && (LB0_uy > a[1]))
00656   {
00657     if (((UA0_uy < 0) || 
00658           InVoronoi(b[0],a[0],-A0_dot_B1,Tba[1] - aA1_dot_B1,A0_dot_B0,
00659                     aA1_dot_B0 - Tba[0],-Tab[0]))
00660         &&      
00661 
00662         ((LB0_ly > a[1]) || 
00663           InVoronoi(a[0],b[0],A1_dot_B0,Tab[1] - a[1],
00664                     A0_dot_B0,Tab[0],Tba[0] - aA1_dot_B0)))
00665     {
00666       SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0],Tba[0] - aA1_dot_B0);
00667 
00668       S[0] = Tab[0] + Rab[0][0]*u - t;
00669       S[1] = Tab[1] + Rab[1][0]*u - a[1];
00670       S[2] = Tab[2] + Rab[2][0]*u;
00671       return sqrt(VdotV(S,S));
00672     }
00673   }
00674 
00675   // LA0, UB0
00676 
00677   if ((LA0_uy > b[1]) && (UB0_ly < 0))
00678   {  
00679     if (((LA0_ly > b[1]) ||
00680           InVoronoi(b[0],a[0],A0_dot_B1,-Tba[1] - b[1], A0_dot_B0,-Tba[0],
00681                     -Tab[0] - bA0_dot_B1))
00682         &&
00683         
00684         ((UB0_uy < 0) ||
00685           InVoronoi(a[0],b[0],-A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0,
00686                     Tab[0] + bA0_dot_B1,Tba[0])))
00687     {
00688       SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0] + bA0_dot_B1,Tba[0]);
00689       
00690       S[0] = Tab[0] + Rab[0][1]*b[1] + Rab[0][0]*u - t;
00691       S[1] = Tab[1] + Rab[1][1]*b[1] + Rab[1][0]*u;
00692       S[2] = Tab[2] + Rab[2][1]*b[1] + Rab[2][0]*u;
00693       return sqrt(VdotV(S,S));
00694     }
00695   }
00696 
00697   // LA0, LB0
00698 
00699   if ((LA0_ly < 0) && (LB0_ly < 0))
00700   {   
00701     if (((LA0_uy < 0) || 
00702           InVoronoi(b[0],a[0],-A0_dot_B1,Tba[1],A0_dot_B0,
00703                     -Tba[0],-Tab[0]))
00704         &&
00705         
00706         ((LB0_uy < 0) || 
00707           InVoronoi(a[0],b[0],-A1_dot_B0,-Tab[1],A0_dot_B0,
00708                     Tab[0],Tba[0])))
00709     {
00710       SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0],Tba[0]);
00711 
00712       S[0] = Tab[0] + Rab[0][0]*u - t;
00713       S[1] = Tab[1] + Rab[1][0]*u;
00714       S[2] = Tab[2] + Rab[2][0]*u;
00715       return sqrt(VdotV(S,S));
00716     }
00717   }
00718 
00719   // no edges passed, take max separation along face normals
00720 
00721   PQP_REAL sep1, sep2;
00722  
00723   if (Tab[2] > 0.0)
00724   {
00725     sep1 = Tab[2];
00726     if (Rab[2][0] < 0.0) sep1 += b[0]*Rab[2][0];
00727     if (Rab[2][1] < 0.0) sep1 += b[1]*Rab[2][1];
00728   }
00729   else
00730   {
00731     sep1 = -Tab[2];
00732     if (Rab[2][0] > 0.0) sep1 -= b[0]*Rab[2][0];
00733     if (Rab[2][1] > 0.0) sep1 -= b[1]*Rab[2][1];
00734   }
00735   
00736   if (Tba[2] < 0)
00737   {
00738     sep2 = -Tba[2];
00739     if (Rab[0][2] < 0.0) sep2 += a[0]*Rab[0][2];
00740     if (Rab[1][2] < 0.0) sep2 += a[1]*Rab[1][2];
00741   }
00742   else
00743   {
00744     sep2 = Tba[2];
00745     if (Rab[0][2] > 0.0) sep2 -= a[0]*Rab[0][2];
00746     if (Rab[1][2] > 0.0) sep2 -= a[1]*Rab[1][2];
00747   }
00748 
00749   PQP_REAL sep = (sep1 > sep2? sep1 : sep2);
00750   return (sep > 0? sep : 0);
00751 }
00752 
00753 #endif


jskeus
Author(s): JSK Alumnis
autogenerated on Fri Aug 28 2015 11:15:08