libmove3d
3.13.0
|
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