pqp
 All Classes
RectDist.h
1 /*************************************************************************\
2 
3  Copyright 1999 The University of North Carolina at Chapel Hill.
4  All Rights Reserved.
5 
6  Permission to use, copy, modify and distribute this software and its
7  documentation for educational, research and non-profit purposes, without
8  fee, and without a written agreement is hereby granted, provided that the
9  above copyright notice and the following three paragraphs appear in all
10  copies.
11 
12  IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
13  LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
14  CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
15  USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
16  OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
17  DAMAGES.
18 
19  THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
20  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
22  PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
23  NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
24  UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
25 
26  The authors may be contacted via:
27 
28  US Mail: E. Larsen
29  Department of Computer Science
30  Sitterson Hall, CB #3175
31  University of N. Carolina
32  Chapel Hill, NC 27599-3175
33 
34  Phone: (919)962-1749
35 
36  EMail: geom@cs.unc.edu
37 
38 
39 \**************************************************************************/
40 
41 #ifndef PQP_RECTDIST_H
42 #define PQP_RECTDIST_H
43 
44 #include <math.h>
45 #include "MatVec.h"
46 #include "PQP_Compile.h"
47 
48 // ClipToRange
49 //
50 // clips val between a and b
51 
52 inline
53 void
54 ClipToRange(PQP_REAL &val, const PQP_REAL &a, const PQP_REAL &b)
55 {
56  if (val < a) val = a;
57  else if (val > b) val = b;
58 }
59 
60 // SegCoords
61 //
62 // finds the parameters t & u corresponding to the two closest points
63 // on a pair of line segments
64 //
65 // The first segment is defined as
66 //
67 // Pa + A*t, 0 <= t <= a,
68 //
69 // where "Pa" is one endpoint of the segment, "A" is a unit vector
70 // pointing to the other endpoint, and t is a scalar that produces
71 // all the points between the two endpoints. Since "A" is a unit
72 // vector, "a" is the segment's length.
73 //
74 // The second segment is
75 //
76 // Pb + B*u, 0 <= u <= b
77 //
78 // In my application, many of the terms needed by the algorithm
79 // are already computed for other purposes, so I pass these terms to
80 // the function instead of complete specifications of each segment.
81 // "T" in the dot products is the vector between Pa and Pb.
82 //
83 // The algorithm is from
84 //
85 // Vladimir J. Lumelsky,
86 // On fast computation of distance between line segments.
87 // In Information Processing Letters, no. 21, pages 55-61, 1985.
88 
89 inline
90 void
91 SegCoords(PQP_REAL& t, PQP_REAL& u,
92  const PQP_REAL& a, const PQP_REAL& b,
93  const PQP_REAL& A_dot_B,
94  const PQP_REAL& A_dot_T,
95  const PQP_REAL& B_dot_T)
96 {
97  PQP_REAL denom = 1 - (A_dot_B)*(A_dot_B);
98 
99  if (denom == 0) t = 0;
100  else
101  {
102  t = (A_dot_T - B_dot_T*A_dot_B)/denom;
103  ClipToRange(t,0,a);
104  }
105 
106  u = t*A_dot_B - B_dot_T;
107  if (u < 0)
108  {
109  u = 0;
110  t = A_dot_T;
111  ClipToRange(t,0,a);
112  }
113  else if (u > b)
114  {
115  u = b;
116  t = u*A_dot_B + A_dot_T;
117  ClipToRange(t,0,a);
118  }
119 }
120 
121 // InVoronoi
122 //
123 // returns whether the nearest point on rectangle edge
124 // Pb + B*u, 0 <= u <= b, to the rectangle edge,
125 // Pa + A*t, 0 <= t <= a, is within the half space
126 // determined by the point Pa and the direction Anorm.
127 //
128 // A,B, and Anorm are unit vectors.
129 // T is the vector between Pa and Pb.
130 
131 inline
132 int
133 InVoronoi(const PQP_REAL &a,
134  const PQP_REAL &b,
135  const PQP_REAL &Anorm_dot_B,
136  const PQP_REAL &Anorm_dot_T,
137  const PQP_REAL &A_dot_B,
138  const PQP_REAL &A_dot_T,
139  const PQP_REAL &B_dot_T)
140 {
141  if (myfabs(Anorm_dot_B) < 1e-7) return 0;
142 
143  PQP_REAL t, u, v;
144 
145  u = -Anorm_dot_T / Anorm_dot_B;
146  ClipToRange(u,0,b);
147 
148  t = u*A_dot_B + A_dot_T;
149  ClipToRange(t,0,a);
150 
151  v = t*A_dot_B - B_dot_T;
152 
153  if (Anorm_dot_B > 0)
154  {
155  if (v > (u + 1e-7)) return 1;
156  }
157  else
158  {
159  if (v < (u - 1e-7)) return 1;
160  }
161  return 0;
162 }
163 
164 
165 // RectDist
166 //
167 // Finds the distance between two rectangles A and B. A is assumed
168 // to have its corner on the origin, one side aligned with
169 // x, the other side aligned with y, and its normal aligned with z.
170 //
171 // [Rab,Tab] gives the orientation and corner position of rectangle B
172 //
173 // a[2] are the side lengths of A, b[2] are the side lengths of B
174 
175 inline
176 PQP_REAL
177 RectDist(PQP_REAL Rab[3][3], PQP_REAL Tab[3],
178  PQP_REAL a[2], PQP_REAL b[2])
179 {
180  PQP_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
181 
182  A0_dot_B0 = Rab[0][0];
183  A0_dot_B1 = Rab[0][1];
184  A1_dot_B0 = Rab[1][0];
185  A1_dot_B1 = Rab[1][1];
186 
187  PQP_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
188  PQP_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
189 
190  aA0_dot_B0 = a[0]*A0_dot_B0;
191  aA0_dot_B1 = a[0]*A0_dot_B1;
192  aA1_dot_B0 = a[1]*A1_dot_B0;
193  aA1_dot_B1 = a[1]*A1_dot_B1;
194  bA0_dot_B0 = b[0]*A0_dot_B0;
195  bA1_dot_B0 = b[0]*A1_dot_B0;
196  bA0_dot_B1 = b[1]*A0_dot_B1;
197  bA1_dot_B1 = b[1]*A1_dot_B1;
198 
199  PQP_REAL Tba[3];
200  MTxV(Tba,Rab,Tab);
201 
202  PQP_REAL S[3], t, u;
203 
204  // determine if any edge pair contains the closest points
205 
206  PQP_REAL ALL_x, ALU_x, AUL_x, AUU_x;
207  PQP_REAL BLL_x, BLU_x, BUL_x, BUU_x;
208  PQP_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
209 
210  ALL_x = -Tba[0];
211  ALU_x = ALL_x + aA1_dot_B0;
212  AUL_x = ALL_x + aA0_dot_B0;
213  AUU_x = ALU_x + aA0_dot_B0;
214 
215  if (ALL_x < ALU_x)
216  {
217  LA1_lx = ALL_x;
218  LA1_ux = ALU_x;
219  UA1_lx = AUL_x;
220  UA1_ux = AUU_x;
221  }
222  else
223  {
224  LA1_lx = ALU_x;
225  LA1_ux = ALL_x;
226  UA1_lx = AUU_x;
227  UA1_ux = AUL_x;
228  }
229 
230  BLL_x = Tab[0];
231  BLU_x = BLL_x + bA0_dot_B1;
232  BUL_x = BLL_x + bA0_dot_B0;
233  BUU_x = BLU_x + bA0_dot_B0;
234 
235  if (BLL_x < BLU_x)
236  {
237  LB1_lx = BLL_x;
238  LB1_ux = BLU_x;
239  UB1_lx = BUL_x;
240  UB1_ux = BUU_x;
241  }
242  else
243  {
244  LB1_lx = BLU_x;
245  LB1_ux = BLL_x;
246  UB1_lx = BUU_x;
247  UB1_ux = BUL_x;
248  }
249 
250  // UA1, UB1
251 
252  if ((UA1_ux > b[0]) && (UB1_ux > a[0]))
253  {
254  if (((UA1_lx > b[0]) ||
255  InVoronoi(b[1],a[1],A1_dot_B0,aA0_dot_B0 - b[0] - Tba[0],
256  A1_dot_B1, aA0_dot_B1 - Tba[1],
257  -Tab[1] - bA1_dot_B0))
258  &&
259 
260  ((UB1_lx > a[0]) ||
261  InVoronoi(a[1],b[1],A0_dot_B1,Tab[0] + bA0_dot_B0 - a[0],
262  A1_dot_B1,Tab[1] + bA1_dot_B0,Tba[1] - aA0_dot_B1)))
263  {
264  SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1] + bA1_dot_B0,
265  Tba[1] - aA0_dot_B1);
266 
267  S[0] = Tab[0] + Rab[0][0]*b[0] + Rab[0][1]*u - a[0] ;
268  S[1] = Tab[1] + Rab[1][0]*b[0] + Rab[1][1]*u - t;
269  S[2] = Tab[2] + Rab[2][0]*b[0] + Rab[2][1]*u;
270  return sqrt(VdotV(S,S));
271  }
272  }
273 
274 
275  // UA1, LB1
276 
277  if ((UA1_lx < 0) && (LB1_ux > a[0]))
278  {
279  if (((UA1_ux < 0) ||
280  InVoronoi(b[1],a[1],-A1_dot_B0,Tba[0] - aA0_dot_B0,
281  A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1]))
282  &&
283 
284  ((LB1_lx > a[0]) ||
285  InVoronoi(a[1],b[1],A0_dot_B1,Tab[0] - a[0],
286  A1_dot_B1,Tab[1],Tba[1] - aA0_dot_B1)))
287  {
288  SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1],Tba[1] - aA0_dot_B1);
289 
290  S[0] = Tab[0] + Rab[0][1]*u - a[0];
291  S[1] = Tab[1] + Rab[1][1]*u - t;
292  S[2] = Tab[2] + Rab[2][1]*u;
293  return sqrt(VdotV(S,S));
294  }
295  }
296 
297  // LA1, UB1
298 
299  if ((LA1_ux > b[0]) && (UB1_lx < 0))
300  {
301  if (((LA1_lx > b[0]) ||
302  InVoronoi(b[1],a[1],A1_dot_B0,-Tba[0] - b[0],
303  A1_dot_B1,-Tba[1], -Tab[1] - bA1_dot_B0))
304  &&
305 
306  ((UB1_ux < 0) ||
307  InVoronoi(a[1],b[1],-A0_dot_B1, -Tab[0] - bA0_dot_B0,
308  A1_dot_B1, Tab[1] + bA1_dot_B0,Tba[1])))
309  {
310 
311  SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1] + bA1_dot_B0,Tba[1]);
312 
313  S[0] = Tab[0] + Rab[0][0]*b[0] + Rab[0][1]*u;
314  S[1] = Tab[1] + Rab[1][0]*b[0] + Rab[1][1]*u - t;
315  S[2] = Tab[2] + Rab[2][0]*b[0] + Rab[2][1]*u;
316  return sqrt(VdotV(S,S));
317  }
318  }
319 
320  // LA1, LB1
321 
322  if ((LA1_lx < 0) && (LB1_lx < 0))
323  {
324  if (((LA1_ux < 0) ||
325  InVoronoi(b[1],a[1],-A1_dot_B0,Tba[0],A1_dot_B1,
326  -Tba[1],-Tab[1]))
327  &&
328 
329  ((LB1_ux < 0) ||
330  InVoronoi(a[1],b[1],-A0_dot_B1,-Tab[0],A1_dot_B1,
331  Tab[1], Tba[1])))
332  {
333  SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1],Tba[1]);
334 
335  S[0] = Tab[0] + Rab[0][1]*u;
336  S[1] = Tab[1] + Rab[1][1]*u - t;
337  S[2] = Tab[2] + Rab[2][1]*u;
338  return sqrt(VdotV(S,S));
339  }
340  }
341 
342  PQP_REAL ALL_y, ALU_y, AUL_y, AUU_y;
343 
344  ALL_y = -Tba[1];
345  ALU_y = ALL_y + aA1_dot_B1;
346  AUL_y = ALL_y + aA0_dot_B1;
347  AUU_y = ALU_y + aA0_dot_B1;
348 
349  PQP_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
350 
351  if (ALL_y < ALU_y)
352  {
353  LA1_ly = ALL_y;
354  LA1_uy = ALU_y;
355  UA1_ly = AUL_y;
356  UA1_uy = AUU_y;
357  }
358  else
359  {
360  LA1_ly = ALU_y;
361  LA1_uy = ALL_y;
362  UA1_ly = AUU_y;
363  UA1_uy = AUL_y;
364  }
365 
366  if (BLL_x < BUL_x)
367  {
368  LB0_lx = BLL_x;
369  LB0_ux = BUL_x;
370  UB0_lx = BLU_x;
371  UB0_ux = BUU_x;
372  }
373  else
374  {
375  LB0_lx = BUL_x;
376  LB0_ux = BLL_x;
377  UB0_lx = BUU_x;
378  UB0_ux = BLU_x;
379  }
380 
381  // UA1, UB0
382 
383  if ((UA1_uy > b[1]) && (UB0_ux > a[0]))
384  {
385  if (((UA1_ly > b[1]) ||
386  InVoronoi(b[0],a[1],A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1],
387  A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1))
388  &&
389 
390  ((UB0_lx > a[0]) ||
391  InVoronoi(a[1],b[0],A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1,
392  A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0)))
393  {
394  SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1] + bA1_dot_B1,
395  Tba[0] - aA0_dot_B0);
396 
397  S[0] = Tab[0] + Rab[0][1]*b[1] + Rab[0][0]*u - a[0] ;
398  S[1] = Tab[1] + Rab[1][1]*b[1] + Rab[1][0]*u - t;
399  S[2] = Tab[2] + Rab[2][1]*b[1] + Rab[2][0]*u;
400  return sqrt(VdotV(S,S));
401  }
402  }
403 
404  // UA1, LB0
405 
406  if ((UA1_ly < 0) && (LB0_ux > a[0]))
407  {
408  if (((UA1_uy < 0) ||
409  InVoronoi(b[0],a[1],-A1_dot_B1, Tba[1] - aA0_dot_B1,A1_dot_B0,
410  aA0_dot_B0 - Tba[0], -Tab[1]))
411  &&
412 
413  ((LB0_lx > a[0]) ||
414  InVoronoi(a[1],b[0],A0_dot_B0,Tab[0] - a[0],
415  A1_dot_B0,Tab[1],Tba[0] - aA0_dot_B0)))
416  {
417  SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1],Tba[0] - aA0_dot_B0);
418 
419  S[0] = Tab[0] + Rab[0][0]*u - a[0];
420  S[1] = Tab[1] + Rab[1][0]*u - t;
421  S[2] = Tab[2] + Rab[2][0]*u;
422  return sqrt(VdotV(S,S));
423  }
424  }
425 
426  // LA1, UB0
427 
428  if ((LA1_uy > b[1]) && (UB0_lx < 0))
429  {
430  if (((LA1_ly > b[1]) ||
431  InVoronoi(b[0],a[1],A1_dot_B1,-Tba[1] - b[1],
432  A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1))
433  &&
434 
435  ((UB0_ux < 0) ||
436  InVoronoi(a[1],b[0],-A0_dot_B0, -Tab[0] - bA0_dot_B1,A1_dot_B0,
437  Tab[1] + bA1_dot_B1,Tba[0])))
438  {
439  SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1] + bA1_dot_B1,Tba[0]);
440 
441  S[0] = Tab[0] + Rab[0][1]*b[1] + Rab[0][0]*u;
442  S[1] = Tab[1] + Rab[1][1]*b[1] + Rab[1][0]*u - t;
443  S[2] = Tab[2] + Rab[2][1]*b[1] + Rab[2][0]*u;
444  return sqrt(VdotV(S,S));
445  }
446  }
447 
448  // LA1, LB0
449 
450  if ((LA1_ly < 0) && (LB0_lx < 0))
451  {
452  if (((LA1_uy < 0) ||
453  InVoronoi(b[0],a[1],-A1_dot_B1,Tba[1],A1_dot_B0,
454  -Tba[0],-Tab[1]))
455  &&
456 
457  ((LB0_ux < 0) ||
458  InVoronoi(a[1],b[0],-A0_dot_B0,-Tab[0],A1_dot_B0,
459  Tab[1],Tba[0])))
460  {
461  SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1],Tba[0]);
462 
463  S[0] = Tab[0] + Rab[0][0]*u;
464  S[1] = Tab[1] + Rab[1][0]*u - t;
465  S[2] = Tab[2] + Rab[2][0]*u;
466  return sqrt(VdotV(S,S));
467  }
468  }
469 
470  PQP_REAL BLL_y, BLU_y, BUL_y, BUU_y;
471 
472  BLL_y = Tab[1];
473  BLU_y = BLL_y + bA1_dot_B1;
474  BUL_y = BLL_y + bA1_dot_B0;
475  BUU_y = BLU_y + bA1_dot_B0;
476 
477  PQP_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
478 
479  if (ALL_x < AUL_x)
480  {
481  LA0_lx = ALL_x;
482  LA0_ux = AUL_x;
483  UA0_lx = ALU_x;
484  UA0_ux = AUU_x;
485  }
486  else
487  {
488  LA0_lx = AUL_x;
489  LA0_ux = ALL_x;
490  UA0_lx = AUU_x;
491  UA0_ux = ALU_x;
492  }
493 
494  if (BLL_y < BLU_y)
495  {
496  LB1_ly = BLL_y;
497  LB1_uy = BLU_y;
498  UB1_ly = BUL_y;
499  UB1_uy = BUU_y;
500  }
501  else
502  {
503  LB1_ly = BLU_y;
504  LB1_uy = BLL_y;
505  UB1_ly = BUU_y;
506  UB1_uy = BUL_y;
507  }
508 
509  // UA0, UB1
510 
511  if ((UA0_ux > b[0]) && (UB1_uy > a[1]))
512  {
513  if (((UA0_lx > b[0]) ||
514  InVoronoi(b[1],a[0],A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0],
515  A0_dot_B1,aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0))
516  &&
517 
518  ((UB1_ly > a[1]) ||
519  InVoronoi(a[0],b[1],A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0,
520  A0_dot_B1,Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1)))
521  {
522  SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0] + bA0_dot_B0,
523  Tba[1] - aA1_dot_B1);
524 
525  S[0] = Tab[0] + Rab[0][0]*b[0] + Rab[0][1]*u - t;
526  S[1] = Tab[1] + Rab[1][0]*b[0] + Rab[1][1]*u - a[1];
527  S[2] = Tab[2] + Rab[2][0]*b[0] + Rab[2][1]*u;
528  return sqrt(VdotV(S,S));
529  }
530  }
531 
532  // UA0, LB1
533 
534  if ((UA0_lx < 0) && (LB1_uy > a[1]))
535  {
536  if (((UA0_ux < 0) ||
537  InVoronoi(b[1],a[0],-A0_dot_B0, Tba[0] - aA1_dot_B0,A0_dot_B1,
538  aA1_dot_B1 - Tba[1],-Tab[0]))
539  &&
540 
541  ((LB1_ly > a[1]) ||
542  InVoronoi(a[0],b[1],A1_dot_B1,Tab[1] - a[1],A0_dot_B1,Tab[0],
543  Tba[1] - aA1_dot_B1)))
544  {
545  SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0],Tba[1] - aA1_dot_B1);
546 
547  S[0] = Tab[0] + Rab[0][1]*u - t;
548  S[1] = Tab[1] + Rab[1][1]*u - a[1];
549  S[2] = Tab[2] + Rab[2][1]*u;
550  return sqrt(VdotV(S,S));
551  }
552  }
553 
554  // LA0, UB1
555 
556  if ((LA0_ux > b[0]) && (UB1_ly < 0))
557  {
558  if (((LA0_lx > b[0]) ||
559  InVoronoi(b[1],a[0],A0_dot_B0,-b[0] - Tba[0],A0_dot_B1,-Tba[1],
560  -bA0_dot_B0 - Tab[0]))
561  &&
562 
563  ((UB1_uy < 0) ||
564  InVoronoi(a[0],b[1],-A1_dot_B1, -Tab[1] - bA1_dot_B0,A0_dot_B1,
565  Tab[0] + bA0_dot_B0,Tba[1])))
566  {
567  SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0] + bA0_dot_B0,Tba[1]);
568 
569  S[0] = Tab[0] + Rab[0][0]*b[0] + Rab[0][1]*u - t;
570  S[1] = Tab[1] + Rab[1][0]*b[0] + Rab[1][1]*u;
571  S[2] = Tab[2] + Rab[2][0]*b[0] + Rab[2][1]*u;
572  return sqrt(VdotV(S,S));
573  }
574  }
575 
576  // LA0, LB1
577 
578  if ((LA0_lx < 0) && (LB1_ly < 0))
579  {
580  if (((LA0_ux < 0) ||
581  InVoronoi(b[1],a[0],-A0_dot_B0,Tba[0],A0_dot_B1,-Tba[1],
582  -Tab[0]))
583  &&
584 
585  ((LB1_uy < 0) ||
586  InVoronoi(a[0],b[1],-A1_dot_B1,-Tab[1],A0_dot_B1,
587  Tab[0],Tba[1])))
588  {
589  SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0],Tba[1]);
590 
591  S[0] = Tab[0] + Rab[0][1]*u - t;
592  S[1] = Tab[1] + Rab[1][1]*u;
593  S[2] = Tab[2] + Rab[2][1]*u;
594  return sqrt(VdotV(S,S));
595  }
596  }
597 
598  PQP_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
599 
600  if (ALL_y < AUL_y)
601  {
602  LA0_ly = ALL_y;
603  LA0_uy = AUL_y;
604  UA0_ly = ALU_y;
605  UA0_uy = AUU_y;
606  }
607  else
608  {
609  LA0_ly = AUL_y;
610  LA0_uy = ALL_y;
611  UA0_ly = AUU_y;
612  UA0_uy = ALU_y;
613  }
614 
615  if (BLL_y < BUL_y)
616  {
617  LB0_ly = BLL_y;
618  LB0_uy = BUL_y;
619  UB0_ly = BLU_y;
620  UB0_uy = BUU_y;
621  }
622  else
623  {
624  LB0_ly = BUL_y;
625  LB0_uy = BLL_y;
626  UB0_ly = BUU_y;
627  UB0_uy = BLU_y;
628  }
629 
630  // UA0, UB0
631 
632  if ((UA0_uy > b[1]) && (UB0_uy > a[1]))
633  {
634  if (((UA0_ly > b[1]) ||
635  InVoronoi(b[0],a[0],A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1],
636  A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1))
637  &&
638 
639  ((UB0_ly > a[1]) ||
640  InVoronoi(a[0],b[0],A1_dot_B0,Tab[1] - a[1] + bA1_dot_B1,A0_dot_B0,
641  Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0)))
642  {
643  SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0] + bA0_dot_B1,
644  Tba[0] - aA1_dot_B0);
645 
646  S[0] = Tab[0] + Rab[0][1]*b[1] + Rab[0][0]*u - t;
647  S[1] = Tab[1] + Rab[1][1]*b[1] + Rab[1][0]*u - a[1];
648  S[2] = Tab[2] + Rab[2][1]*b[1] + Rab[2][0]*u;
649  return sqrt(VdotV(S,S));
650  }
651  }
652 
653  // UA0, LB0
654 
655  if ((UA0_ly < 0) && (LB0_uy > a[1]))
656  {
657  if (((UA0_uy < 0) ||
658  InVoronoi(b[0],a[0],-A0_dot_B1,Tba[1] - aA1_dot_B1,A0_dot_B0,
659  aA1_dot_B0 - Tba[0],-Tab[0]))
660  &&
661 
662  ((LB0_ly > a[1]) ||
663  InVoronoi(a[0],b[0],A1_dot_B0,Tab[1] - a[1],
664  A0_dot_B0,Tab[0],Tba[0] - aA1_dot_B0)))
665  {
666  SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0],Tba[0] - aA1_dot_B0);
667 
668  S[0] = Tab[0] + Rab[0][0]*u - t;
669  S[1] = Tab[1] + Rab[1][0]*u - a[1];
670  S[2] = Tab[2] + Rab[2][0]*u;
671  return sqrt(VdotV(S,S));
672  }
673  }
674 
675  // LA0, UB0
676 
677  if ((LA0_uy > b[1]) && (UB0_ly < 0))
678  {
679  if (((LA0_ly > b[1]) ||
680  InVoronoi(b[0],a[0],A0_dot_B1,-Tba[1] - b[1], A0_dot_B0,-Tba[0],
681  -Tab[0] - bA0_dot_B1))
682  &&
683 
684  ((UB0_uy < 0) ||
685  InVoronoi(a[0],b[0],-A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0,
686  Tab[0] + bA0_dot_B1,Tba[0])))
687  {
688  SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0] + bA0_dot_B1,Tba[0]);
689 
690  S[0] = Tab[0] + Rab[0][1]*b[1] + Rab[0][0]*u - t;
691  S[1] = Tab[1] + Rab[1][1]*b[1] + Rab[1][0]*u;
692  S[2] = Tab[2] + Rab[2][1]*b[1] + Rab[2][0]*u;
693  return sqrt(VdotV(S,S));
694  }
695  }
696 
697  // LA0, LB0
698 
699  if ((LA0_ly < 0) && (LB0_ly < 0))
700  {
701  if (((LA0_uy < 0) ||
702  InVoronoi(b[0],a[0],-A0_dot_B1,Tba[1],A0_dot_B0,
703  -Tba[0],-Tab[0]))
704  &&
705 
706  ((LB0_uy < 0) ||
707  InVoronoi(a[0],b[0],-A1_dot_B0,-Tab[1],A0_dot_B0,
708  Tab[0],Tba[0])))
709  {
710  SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0],Tba[0]);
711 
712  S[0] = Tab[0] + Rab[0][0]*u - t;
713  S[1] = Tab[1] + Rab[1][0]*u;
714  S[2] = Tab[2] + Rab[2][0]*u;
715  return sqrt(VdotV(S,S));
716  }
717  }
718 
719  // no edges passed, take max separation along face normals
720 
721  PQP_REAL sep1, sep2;
722 
723  if (Tab[2] > 0.0)
724  {
725  sep1 = Tab[2];
726  if (Rab[2][0] < 0.0) sep1 += b[0]*Rab[2][0];
727  if (Rab[2][1] < 0.0) sep1 += b[1]*Rab[2][1];
728  }
729  else
730  {
731  sep1 = -Tab[2];
732  if (Rab[2][0] > 0.0) sep1 -= b[0]*Rab[2][0];
733  if (Rab[2][1] > 0.0) sep1 -= b[1]*Rab[2][1];
734  }
735 
736  if (Tba[2] < 0)
737  {
738  sep2 = -Tba[2];
739  if (Rab[0][2] < 0.0) sep2 += a[0]*Rab[0][2];
740  if (Rab[1][2] < 0.0) sep2 += a[1]*Rab[1][2];
741  }
742  else
743  {
744  sep2 = Tba[2];
745  if (Rab[0][2] > 0.0) sep2 -= a[0]*Rab[0][2];
746  if (Rab[1][2] > 0.0) sep2 -= a[1]*Rab[1][2];
747  }
748 
749  PQP_REAL sep = (sep1 > sep2? sep1 : sep2);
750  return (sep > 0? sep : 0);
751 }
752 
753 #endif