41 #ifndef PQP_RECTDIST_H
42 #define PQP_RECTDIST_H
46 #include "PQP_Compile.h"
54 ClipToRange(PQP_REAL &val,
const PQP_REAL &a,
const PQP_REAL &b)
57 else if (val > b) val = b;
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)
97 PQP_REAL denom = 1 - (A_dot_B)*(A_dot_B);
99 if (denom == 0) t = 0;
102 t = (A_dot_T - B_dot_T*A_dot_B)/denom;
106 u = t*A_dot_B - B_dot_T;
116 t = u*A_dot_B + A_dot_T;
133 InVoronoi(
const PQP_REAL &a,
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)
141 if (myfabs(Anorm_dot_B) < 1e-7)
return 0;
145 u = -Anorm_dot_T / Anorm_dot_B;
148 t = u*A_dot_B + A_dot_T;
151 v = t*A_dot_B - B_dot_T;
155 if (v > (u + 1e-7))
return 1;
159 if (v < (u - 1e-7))
return 1;
177 RectDist(PQP_REAL Rab[3][3], PQP_REAL Tab[3],
178 PQP_REAL a[2], PQP_REAL b[2])
180 PQP_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
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];
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;
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;
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;
211 ALU_x = ALL_x + aA1_dot_B0;
212 AUL_x = ALL_x + aA0_dot_B0;
213 AUU_x = ALU_x + aA0_dot_B0;
231 BLU_x = BLL_x + bA0_dot_B1;
232 BUL_x = BLL_x + bA0_dot_B0;
233 BUU_x = BLU_x + bA0_dot_B0;
252 if ((UA1_ux > b[0]) && (UB1_ux > a[0]))
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))
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)))
264 SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1] + bA1_dot_B0,
265 Tba[1] - aA0_dot_B1);
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));
277 if ((UA1_lx < 0) && (LB1_ux > a[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]))
285 InVoronoi(a[1],b[1],A0_dot_B1,Tab[0] - a[0],
286 A1_dot_B1,Tab[1],Tba[1] - aA0_dot_B1)))
288 SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1],Tba[1] - aA0_dot_B1);
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));
299 if ((LA1_ux > b[0]) && (UB1_lx < 0))
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))
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])))
311 SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1] + bA1_dot_B0,Tba[1]);
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));
322 if ((LA1_lx < 0) && (LB1_lx < 0))
325 InVoronoi(b[1],a[1],-A1_dot_B0,Tba[0],A1_dot_B1,
330 InVoronoi(a[1],b[1],-A0_dot_B1,-Tab[0],A1_dot_B1,
333 SegCoords(t,u,a[1],b[1],A1_dot_B1,Tab[1],Tba[1]);
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));
342 PQP_REAL ALL_y, ALU_y, AUL_y, AUU_y;
345 ALU_y = ALL_y + aA1_dot_B1;
346 AUL_y = ALL_y + aA0_dot_B1;
347 AUU_y = ALU_y + aA0_dot_B1;
349 PQP_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
383 if ((UA1_uy > b[1]) && (UB0_ux > a[0]))
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))
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)))
394 SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1] + bA1_dot_B1,
395 Tba[0] - aA0_dot_B0);
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));
406 if ((UA1_ly < 0) && (LB0_ux > a[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]))
414 InVoronoi(a[1],b[0],A0_dot_B0,Tab[0] - a[0],
415 A1_dot_B0,Tab[1],Tba[0] - aA0_dot_B0)))
417 SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1],Tba[0] - aA0_dot_B0);
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));
428 if ((LA1_uy > b[1]) && (UB0_lx < 0))
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))
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])))
439 SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1] + bA1_dot_B1,Tba[0]);
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));
450 if ((LA1_ly < 0) && (LB0_lx < 0))
453 InVoronoi(b[0],a[1],-A1_dot_B1,Tba[1],A1_dot_B0,
458 InVoronoi(a[1],b[0],-A0_dot_B0,-Tab[0],A1_dot_B0,
461 SegCoords(t,u,a[1],b[0],A1_dot_B0,Tab[1],Tba[0]);
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));
470 PQP_REAL BLL_y, BLU_y, BUL_y, BUU_y;
473 BLU_y = BLL_y + bA1_dot_B1;
474 BUL_y = BLL_y + bA1_dot_B0;
475 BUU_y = BLU_y + bA1_dot_B0;
477 PQP_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
511 if ((UA0_ux > b[0]) && (UB1_uy > a[1]))
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))
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)))
522 SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0] + bA0_dot_B0,
523 Tba[1] - aA1_dot_B1);
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));
534 if ((UA0_lx < 0) && (LB1_uy > a[1]))
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]))
542 InVoronoi(a[0],b[1],A1_dot_B1,Tab[1] - a[1],A0_dot_B1,Tab[0],
543 Tba[1] - aA1_dot_B1)))
545 SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0],Tba[1] - aA1_dot_B1);
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));
556 if ((LA0_ux > b[0]) && (UB1_ly < 0))
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]))
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])))
567 SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0] + bA0_dot_B0,Tba[1]);
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));
578 if ((LA0_lx < 0) && (LB1_ly < 0))
581 InVoronoi(b[1],a[0],-A0_dot_B0,Tba[0],A0_dot_B1,-Tba[1],
586 InVoronoi(a[0],b[1],-A1_dot_B1,-Tab[1],A0_dot_B1,
589 SegCoords(t,u,a[0],b[1],A0_dot_B1,Tab[0],Tba[1]);
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));
598 PQP_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
632 if ((UA0_uy > b[1]) && (UB0_uy > a[1]))
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))
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)))
643 SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0] + bA0_dot_B1,
644 Tba[0] - aA1_dot_B0);
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));
655 if ((UA0_ly < 0) && (LB0_uy > a[1]))
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]))
663 InVoronoi(a[0],b[0],A1_dot_B0,Tab[1] - a[1],
664 A0_dot_B0,Tab[0],Tba[0] - aA1_dot_B0)))
666 SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0],Tba[0] - aA1_dot_B0);
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));
677 if ((LA0_uy > b[1]) && (UB0_ly < 0))
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))
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])))
688 SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0] + bA0_dot_B1,Tba[0]);
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));
699 if ((LA0_ly < 0) && (LB0_ly < 0))
702 InVoronoi(b[0],a[0],-A0_dot_B1,Tba[1],A0_dot_B0,
707 InVoronoi(a[0],b[0],-A1_dot_B0,-Tab[1],A0_dot_B0,
710 SegCoords(t,u,a[0],b[0],A0_dot_B0,Tab[0],Tba[0]);
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));
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];
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];
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];
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];
749 PQP_REAL sep = (sep1 > sep2? sep1 : sep2);
750 return (sep > 0? sep : 0);