FCL  0.6.0
Flexible Collision Library
RSS-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef FCL_BV_RSS_INL_H
39 #define FCL_BV_RSS_INL_H
40 
41 #include "fcl/math/bv/RSS.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class RSS<double>;
49 
50 //==============================================================================
51 extern template
52 void clipToRange(double& val, double a, double b);
53 
54 //==============================================================================
55 extern template
56 void segCoords(
57  double& t,
58  double& u,
59  double a,
60  double b,
61  double A_dot_B,
62  double A_dot_T,
63  double B_dot_T);
64 
65 //==============================================================================
66 extern template
67 bool inVoronoi(
68  double a,
69  double b,
70  double Anorm_dot_B,
71  double Anorm_dot_T,
72  double A_dot_B,
73  double A_dot_T,
74  double B_dot_T);
75 
76 //==============================================================================
77 extern template
78 double rectDistance(
79  const Matrix3<double>& Rab,
80  const Vector3<double>& Tab,
81  const double a[2],
82  const double b[2],
83  Vector3<double>* P,
84  Vector3<double>* Q);
85 
86 //==============================================================================
87 extern template
88 double rectDistance(
89  const Transform3<double>& tfab,
90  const double a[2],
91  const double b[2],
92  Vector3<double>* P,
93  Vector3<double>* Q);
94 
95 //==============================================================================
96 extern template
97 RSS<double> translate(const RSS<double>& bv, const Vector3<double>& t);
98 
99 //==============================================================================
100 template <typename S>
101 RSS<S>::RSS()
102  : axis(Matrix3<S>::Identity()), To(Vector3<S>::Zero())
103 {
104  // Do nothing
105 }
106 
107 //==============================================================================
108 template <typename S>
109 bool RSS<S>::overlap(const RSS<S>& other) const
110 {
111  Vector3<S> t = other.To - To;
112  Vector3<S> T(
113  axis.col(0).dot(t), axis.col(1).dot(t), axis.col(2).dot(t));
114  Matrix3<S> R = axis.transpose() * other.axis;
115 
116  S dist = rectDistance(R, T, l, other.l);
117  return (dist <= (r + other.r));
118 }
119 
120 //==============================================================================
121 template <typename S>
122 bool RSS<S>::overlap(const RSS<S>& other,
123  RSS<S>& /*overlap_part*/) const
124 {
125  return overlap(other);
126 }
127 
128 //==============================================================================
129 template <typename S>
130 bool RSS<S>::contain(const Vector3<S>& p) const
131 {
132  Vector3<S> local_p = p - To;
133  Vector3<S> proj(
134  axis.col(0).dot(local_p),
135  axis.col(1).dot(local_p),
136  axis.col(2).dot(local_p));
137  S abs_proj2 = fabs(proj[2]);
138 
140  if((proj[0] < l[0]) && (proj[0] > 0) && (proj[1] < l[1]) && (proj[1] > 0))
141  {
142  return (abs_proj2 < r);
143  }
144  else if((proj[0] < l[0]) && (proj[0] > 0) && ((proj[1] < 0) || (proj[1] > l[1])))
145  {
146  S y = (proj[1] > 0) ? l[1] : 0;
147  Vector3<S> v(proj[0], y, 0);
148  return ((proj - v).squaredNorm() < r * r);
149  }
150  else if((proj[1] < l[1]) && (proj[1] > 0) && ((proj[0] < 0) || (proj[0] > l[0])))
151  {
152  S x = (proj[0] > 0) ? l[0] : 0;
153  Vector3<S> v(x, proj[1], 0);
154  return ((proj - v).squaredNorm() < r * r);
155  }
156  else
157  {
158  S x = (proj[0] > 0) ? l[0] : 0;
159  S y = (proj[1] > 0) ? l[1] : 0;
160  Vector3<S> v(x, y, 0);
161  return ((proj - v).squaredNorm() < r * r);
162  }
163 }
164 
165 //==============================================================================
166 template <typename S>
167 RSS<S>& RSS<S>::operator +=(const Vector3<S>& p)
168 
169 {
170  Vector3<S> local_p = p - To;
171  Vector3<S> proj(
172  axis.col(0).dot(local_p),
173  axis.col(1).dot(local_p),
174  axis.col(2).dot(local_p));
175  S abs_proj2 = fabs(proj[2]);
176 
177  // projection is within the rectangle
178  if((proj[0] < l[0]) && (proj[0] > 0) && (proj[1] < l[1]) && (proj[1] > 0))
179  {
180  if(abs_proj2 < r)
181  ; // do nothing
182  else
183  {
184  r = 0.5 * (r + abs_proj2); // enlarge the r
185  // change RSS origin position
186  if(proj[2] > 0)
187  To[2] += 0.5 * (abs_proj2 - r);
188  else
189  To[2] -= 0.5 * (abs_proj2 - r);
190  }
191  }
192  else if((proj[0] < l[0]) && (proj[0] > 0) && ((proj[1] < 0) || (proj[1] > l[1])))
193  {
194  S y = (proj[1] > 0) ? l[1] : 0;
195  Vector3<S> v(proj[0], y, 0);
196  S new_r_sqr = (proj - v).squaredNorm();
197  if(new_r_sqr < r * r)
198  ; // do nothing
199  else
200  {
201  if(abs_proj2 < r)
202  {
203  S delta_y = - std::sqrt(r * r - proj[2] * proj[2]) + fabs(proj[1] - y);
204  l[1] += delta_y;
205  if(proj[1] < 0)
206  To[1] -= delta_y;
207  }
208  else
209  {
210  S delta_y = fabs(proj[1] - y);
211  l[1] += delta_y;
212  if(proj[1] < 0)
213  To[1] -= delta_y;
214 
215  if(proj[2] > 0)
216  To[2] += 0.5 * (abs_proj2 - r);
217  else
218  To[2] -= 0.5 * (abs_proj2 - r);
219  }
220  }
221  }
222  else if((proj[1] < l[1]) && (proj[1] > 0) && ((proj[0] < 0) || (proj[0] > l[0])))
223  {
224  S x = (proj[0] > 0) ? l[0] : 0;
225  Vector3<S> v(x, proj[1], 0);
226  S new_r_sqr = (proj - v).squaredNorm();
227  if(new_r_sqr < r * r)
228  ; // do nothing
229  else
230  {
231  if(abs_proj2 < r)
232  {
233  S delta_x = - std::sqrt(r * r - proj[2] * proj[2]) + fabs(proj[0] - x);
234  l[0] += delta_x;
235  if(proj[0] < 0)
236  To[0] -= delta_x;
237  }
238  else
239  {
240  S delta_x = fabs(proj[0] - x);
241  l[0] += delta_x;
242  if(proj[0] < 0)
243  To[0] -= delta_x;
244 
245  if(proj[2] > 0)
246  To[2] += 0.5 * (abs_proj2 - r);
247  else
248  To[2] -= 0.5 * (abs_proj2 - r);
249  }
250  }
251  }
252  else
253  {
254  S x = (proj[0] > 0) ? l[0] : 0;
255  S y = (proj[1] > 0) ? l[1] : 0;
256  Vector3<S> v(x, y, 0);
257  S new_r_sqr = (proj - v).squaredNorm();
258  if(new_r_sqr < r * r)
259  ; // do nothing
260  else
261  {
262  if(abs_proj2 < r)
263  {
264  S diag = std::sqrt(new_r_sqr - proj[2] * proj[2]);
265  S delta_diag = - std::sqrt(r * r - proj[2] * proj[2]) + diag;
266 
267  S delta_x = delta_diag / diag * fabs(proj[0] - x);
268  S delta_y = delta_diag / diag * fabs(proj[1] - y);
269  l[0] += delta_x;
270  l[1] += delta_y;
271 
272  if(proj[0] < 0 && proj[1] < 0)
273  {
274  To[0] -= delta_x;
275  To[1] -= delta_y;
276  }
277  }
278  else
279  {
280  S delta_x = fabs(proj[0] - x);
281  S delta_y = fabs(proj[1] - y);
282 
283  l[0] += delta_x;
284  l[1] += delta_y;
285 
286  if(proj[0] < 0 && proj[1] < 0)
287  {
288  To[0] -= delta_x;
289  To[1] -= delta_y;
290  }
291 
292  if(proj[2] > 0)
293  To[2] += 0.5 * (abs_proj2 - r);
294  else
295  To[2] -= 0.5 * (abs_proj2 - r);
296  }
297  }
298  }
299 
300  return *this;
301 }
302 
303 //==============================================================================
304 template <typename S>
305 RSS<S>& RSS<S>::operator +=(const RSS<S>& other)
306 {
307  *this = *this + other;
308  return *this;
309 }
310 
311 //==============================================================================
312 template <typename S>
313 RSS<S> RSS<S>::operator +(const RSS<S>& other) const
314 {
315  RSS<S> bv;
316 
317  Vector3<S> v[16];
318 
319  Vector3<S> d0_pos = other.axis.col(0) * (other.l[0] + other.r);
320  Vector3<S> d1_pos = other.axis.col(1) * (other.l[1] + other.r);
321 
322  Vector3<S> d0_neg = other.axis.col(0) * (-other.r);
323  Vector3<S> d1_neg = other.axis.col(1) * (-other.r);
324 
325  Vector3<S> d2_pos = other.axis.col(2) * other.r;
326  Vector3<S> d2_neg = other.axis.col(2) * (-other.r);
327 
328  v[0] = other.To + d0_pos + d1_pos + d2_pos;
329  v[1] = other.To + d0_pos + d1_pos + d2_neg;
330  v[2] = other.To + d0_pos + d1_neg + d2_pos;
331  v[3] = other.To + d0_pos + d1_neg + d2_neg;
332  v[4] = other.To + d0_neg + d1_pos + d2_pos;
333  v[5] = other.To + d0_neg + d1_pos + d2_neg;
334  v[6] = other.To + d0_neg + d1_neg + d2_pos;
335  v[7] = other.To + d0_neg + d1_neg + d2_neg;
336 
337  d0_pos.noalias() = axis.col(0) * (l[0] + r);
338  d1_pos.noalias() = axis.col(1) * (l[1] + r);
339  d0_neg.noalias() = axis.col(0) * (-r);
340  d1_neg.noalias() = axis.col(1) * (-r);
341  d2_pos.noalias() = axis.col(2) * r;
342  d2_neg.noalias() = axis.col(2) * (-r);
343 
344  v[8] = To + d0_pos + d1_pos + d2_pos;
345  v[9] = To + d0_pos + d1_pos + d2_neg;
346  v[10] = To + d0_pos + d1_neg + d2_pos;
347  v[11] = To + d0_pos + d1_neg + d2_neg;
348  v[12] = To + d0_neg + d1_pos + d2_pos;
349  v[13] = To + d0_neg + d1_pos + d2_neg;
350  v[14] = To + d0_neg + d1_neg + d2_pos;
351  v[15] = To + d0_neg + d1_neg + d2_neg;
352 
353 
354  Matrix3<S> M; // row first matrix
355  Matrix3<S> E; // row first eigen-vectors
356  Vector3<S> s(0, 0, 0);
357 
358  getCovariance<S>(v, nullptr, nullptr, nullptr, 16, M);
359  eigen_old(M, s, E);
360 
361  int min, mid, max;
362  if(s[0] > s[1]) { max = 0; min = 1; }
363  else { min = 0; max = 1; }
364  if(s[2] < s[min]) { mid = min; min = 2; }
365  else if(s[2] > s[max]) { mid = max; max = 2; }
366  else { mid = 2; }
367 
368  // column first matrix, as the axis in RSS
369  bv.axis.col(0) = E.col(max);
370  bv.axis.col(1) = E.col(mid);
371  bv.axis.col(2).noalias() = axis.col(0).cross(axis.col(1));
372 
373  // set rss origin, rectangle size and radius
374  getRadiusAndOriginAndRectangleSize<S>(v, nullptr, nullptr, nullptr, 16, bv.axis, bv.To, bv.l, bv.r);
375 
376  return bv;
377 }
378 
379 //==============================================================================
380 template <typename S>
381 S RSS<S>::width() const
382 {
383  return l[0] + 2 * r;
384 }
385 
386 //==============================================================================
387 template <typename S>
388 S RSS<S>::height() const
389 {
390  return l[1] + 2 * r;
391 }
392 
393 //==============================================================================
394 template <typename S>
395 S RSS<S>::depth() const
396 {
397  return 2 * r;
398 }
399 
400 //==============================================================================
401 template <typename S>
402 S RSS<S>::volume() const
403 {
404  return (l[0] * l[1] * 2 * r + 4 * constants<S>::pi() * r * r * r);
405 }
406 
407 //==============================================================================
408 template <typename S>
409 S RSS<S>::size() const
410 {
411  return (std::sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r);
412 }
413 
414 //==============================================================================
415 template <typename S>
416 const Vector3<S> RSS<S>::center() const
417 {
418  return To;
419 }
420 
421 //==============================================================================
422 template <typename S>
423 S RSS<S>::distance(
424  const RSS<S>& other,
425  Vector3<S>* P,
426  Vector3<S>* Q) const
427 {
428  Vector3<S> t = other.To - To;
429  Vector3<S> T(
430  axis.col(0).dot(t), axis.col(1).dot(t), axis.col(2).dot(t));
431  Matrix3<S> R = axis.transpose() * other.axis;
432 
433  S dist = rectDistance(R, T, l, other.l, P, Q);
434  dist -= (r + other.r);
435  return (dist < (S)0.0) ? (S)0.0 : dist;
436 }
437 
438 //==============================================================================
439 template <typename S>
440 void clipToRange(S& val, S a, S b)
441 {
442  if(val < a) val = a;
443  else if(val > b) val = b;
444 }
445 
446 //==============================================================================
447 template <typename S>
448 void segCoords(S& t, S& u, S a, S b, S A_dot_B, S A_dot_T, S B_dot_T)
449 {
450  S denom = 1 - A_dot_B * A_dot_B;
451 
452  if(denom == 0) t = 0;
453  else
454  {
455  t = (A_dot_T - B_dot_T * A_dot_B) / denom;
456  clipToRange(t, (S)0.0, a);
457  }
458 
459  u = t * A_dot_B - B_dot_T;
460  if(u < 0)
461  {
462  u = 0;
463  t = A_dot_T;
464  clipToRange(t, (S)0.0, a);
465  }
466  else if(u > b)
467  {
468  u = b;
469  t = u * A_dot_B + A_dot_T;
470  clipToRange(t, (S)0.0, a);
471  }
472 }
473 
474 //==============================================================================
475 template <typename S>
476 bool inVoronoi(S a, S b, S Anorm_dot_B, S Anorm_dot_T, S A_dot_B, S A_dot_T, S B_dot_T)
477 {
478  if(fabs(Anorm_dot_B) < 1e-7) return false;
479 
480  S t, u, v;
481 
482  u = -Anorm_dot_T / Anorm_dot_B;
483  clipToRange(u, (S)0.0, b);
484 
485  t = u * A_dot_B + A_dot_T;
486  clipToRange(t, (S)0.0, a);
487 
488  v = t * A_dot_B - B_dot_T;
489 
490  if(Anorm_dot_B > 0)
491  {
492  if(v > (u + 1e-7)) return true;
493  }
494  else
495  {
496  if(v < (u - 1e-7)) return true;
497  }
498  return false;
499 }
500 
501 //==============================================================================
502 template <typename S>
503 S rectDistance(const Matrix3<S>& Rab, Vector3<S> const& Tab, const S a[2], const S b[2], Vector3<S>* P, Vector3<S>* Q)
504 {
505  S A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
506 
507  A0_dot_B0 = Rab(0, 0);
508  A0_dot_B1 = Rab(0, 1);
509  A1_dot_B0 = Rab(1, 0);
510  A1_dot_B1 = Rab(1, 1);
511 
512  S aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
513  S bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
514 
515  aA0_dot_B0 = a[0] * A0_dot_B0;
516  aA0_dot_B1 = a[0] * A0_dot_B1;
517  aA1_dot_B0 = a[1] * A1_dot_B0;
518  aA1_dot_B1 = a[1] * A1_dot_B1;
519  bA0_dot_B0 = b[0] * A0_dot_B0;
520  bA1_dot_B0 = b[0] * A1_dot_B0;
521  bA0_dot_B1 = b[1] * A0_dot_B1;
522  bA1_dot_B1 = b[1] * A1_dot_B1;
523 
524  Vector3<S> Tba = Rab.transpose() * Tab;
525 
526  Vector3<S> D;
527  S t, u;
528 
529  // determine if any edge pair contains the closest points
530 
531  S ALL_x, ALU_x, AUL_x, AUU_x;
532  S BLL_x, BLU_x, BUL_x, BUU_x;
533  S LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
534 
535  ALL_x = -Tba[0];
536  ALU_x = ALL_x + aA1_dot_B0;
537  AUL_x = ALL_x + aA0_dot_B0;
538  AUU_x = ALU_x + aA0_dot_B0;
539 
540  if(ALL_x < ALU_x)
541  {
542  LA1_lx = ALL_x;
543  LA1_ux = ALU_x;
544  UA1_lx = AUL_x;
545  UA1_ux = AUU_x;
546  }
547  else
548  {
549  LA1_lx = ALU_x;
550  LA1_ux = ALL_x;
551  UA1_lx = AUU_x;
552  UA1_ux = AUL_x;
553  }
554 
555  BLL_x = Tab[0];
556  BLU_x = BLL_x + bA0_dot_B1;
557  BUL_x = BLL_x + bA0_dot_B0;
558  BUU_x = BLU_x + bA0_dot_B0;
559 
560  if(BLL_x < BLU_x)
561  {
562  LB1_lx = BLL_x;
563  LB1_ux = BLU_x;
564  UB1_lx = BUL_x;
565  UB1_ux = BUU_x;
566  }
567  else
568  {
569  LB1_lx = BLU_x;
570  LB1_ux = BLL_x;
571  UB1_lx = BUU_x;
572  UB1_ux = BUL_x;
573  }
574 
575  // UA1, UB1
576 
577  if((UA1_ux > b[0]) && (UB1_ux > a[0]))
578  {
579  if(((UA1_lx > b[0]) ||
580  inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0],
581  A1_dot_B1, aA0_dot_B1 - Tba[1],
582  -Tab[1] - bA1_dot_B0))
583  &&
584  ((UB1_lx > a[0]) ||
585  inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0],
586  A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1)))
587  {
588  segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0,
589  Tba[1] - aA0_dot_B1);
590 
591  D[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0] ;
592  D[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
593  D[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
594 
595  if(P && Q)
596  {
597  *P << a[0], t, 0;
598  *Q = D + (*P);
599  }
600 
601  return D.norm();
602  }
603  }
604 
605 
606  // UA1, LB1
607 
608  if((UA1_lx < 0) && (LB1_ux > a[0]))
609  {
610  if(((UA1_ux < 0) ||
611  inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
612  A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1]))
613  &&
614  ((LB1_lx > a[0]) ||
615  inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0],
616  A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1)))
617  {
618  segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1);
619 
620  D[0] = Tab[0] + Rab(0, 1) * u - a[0];
621  D[1] = Tab[1] + Rab(1, 1) * u - t;
622  D[2] = Tab[2] + Rab(2, 1) * u;
623 
624  if(P && Q)
625  {
626  *P << a[0], t, 0;
627  *Q = D + (*P);
628  }
629 
630  return D.norm();
631  }
632  }
633 
634  // LA1, UB1
635 
636  if((LA1_ux > b[0]) && (UB1_lx < 0))
637  {
638  if(((LA1_lx > b[0]) ||
639  inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0],
640  A1_dot_B1, -Tba[1], -Tab[1] - bA1_dot_B0))
641  &&
642  ((UB1_ux < 0) ||
643  inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0,
644  A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1])))
645  {
646  segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]);
647 
648  D[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u;
649  D[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
650  D[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
651 
652  if(P && Q)
653  {
654  *P << 0, t, 0;
655  *Q = D + (*P);
656  }
657 
658  return D.norm();
659  }
660  }
661 
662  // LA1, LB1
663 
664  if((LA1_lx < 0) && (LB1_lx < 0))
665  {
666  if (((LA1_ux < 0) ||
667  inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1,
668  -Tba[1], -Tab[1]))
669  &&
670  ((LB1_ux < 0) ||
671  inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1,
672  Tab[1], Tba[1])))
673  {
674  segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]);
675 
676  D[0] = Tab[0] + Rab(0, 1) * u;
677  D[1] = Tab[1] + Rab(1, 1) * u - t;
678  D[2] = Tab[2] + Rab(2, 1) * u;
679 
680  if(P && Q)
681  {
682  *P << 0, t, 0;
683  *Q = D + (*P);
684  }
685 
686  return D.norm();
687  }
688  }
689 
690  S ALL_y, ALU_y, AUL_y, AUU_y;
691 
692  ALL_y = -Tba[1];
693  ALU_y = ALL_y + aA1_dot_B1;
694  AUL_y = ALL_y + aA0_dot_B1;
695  AUU_y = ALU_y + aA0_dot_B1;
696 
697  S LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
698 
699  if(ALL_y < ALU_y)
700  {
701  LA1_ly = ALL_y;
702  LA1_uy = ALU_y;
703  UA1_ly = AUL_y;
704  UA1_uy = AUU_y;
705  }
706  else
707  {
708  LA1_ly = ALU_y;
709  LA1_uy = ALL_y;
710  UA1_ly = AUU_y;
711  UA1_uy = AUL_y;
712  }
713 
714  if(BLL_x < BUL_x)
715  {
716  LB0_lx = BLL_x;
717  LB0_ux = BUL_x;
718  UB0_lx = BLU_x;
719  UB0_ux = BUU_x;
720  }
721  else
722  {
723  LB0_lx = BUL_x;
724  LB0_ux = BLL_x;
725  UB0_lx = BUU_x;
726  UB0_ux = BLU_x;
727  }
728 
729  // UA1, UB0
730 
731  if((UA1_uy > b[1]) && (UB0_ux > a[0]))
732  {
733  if(((UA1_ly > b[1]) ||
734  inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1],
735  A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1))
736  &&
737  ((UB0_lx > a[0]) ||
738  inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1,
739  A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0)))
740  {
741  segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1,
742  Tba[0] - aA0_dot_B0);
743 
744  D[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0] ;
745  D[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
746  D[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
747 
748  if(P && Q)
749  {
750  *P << a[0], t, 0;
751  *Q = D + (*P);
752  }
753 
754  return D.norm();
755  }
756  }
757 
758  // UA1, LB0
759 
760  if((UA1_ly < 0) && (LB0_ux > a[0]))
761  {
762  if(((UA1_uy < 0) ||
763  inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0,
764  aA0_dot_B0 - Tba[0], -Tab[1]))
765  &&
766  ((LB0_lx > a[0]) ||
767  inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0],
768  A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0)))
769  {
770  segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0);
771 
772  D[0] = Tab[0] + Rab(0, 0) * u - a[0];
773  D[1] = Tab[1] + Rab(1, 0) * u - t;
774  D[2] = Tab[2] + Rab(2, 0) * u;
775 
776  if(P && Q)
777  {
778  *P << a[0], t, 0;
779  *Q = D + (*P);
780  }
781 
782  return D.norm();
783  }
784  }
785 
786  // LA1, UB0
787 
788  if((LA1_uy > b[1]) && (UB0_lx < 0))
789  {
790  if(((LA1_ly > b[1]) ||
791  inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1],
792  A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1))
793  &&
794 
795  ((UB0_ux < 0) ||
796  inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, A1_dot_B0,
797  Tab[1] + bA1_dot_B1, Tba[0])))
798  {
799  segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]);
800 
801  D[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u;
802  D[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
803  D[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
804 
805  if(P && Q)
806  {
807  *P << 0, t, 0;
808  *Q = D + (*P);
809  }
810 
811 
812  return D.norm();
813  }
814  }
815 
816  // LA1, LB0
817 
818  if((LA1_ly < 0) && (LB0_lx < 0))
819  {
820  if(((LA1_uy < 0) ||
821  inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0,
822  -Tba[0], -Tab[1]))
823  &&
824  ((LB0_ux < 0) ||
825  inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0,
826  Tab[1], Tba[0])))
827  {
828  segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]);
829 
830  D[0] = Tab[0] + Rab(0, 0) * u;
831  D[1] = Tab[1] + Rab(1, 0) * u - t;
832  D[2] = Tab[2] + Rab(2, 0) * u;
833 
834  if(P&& Q)
835  {
836  *P << 0, t, 0;
837  *Q = D + (*P);
838  }
839 
840  return D.norm();
841  }
842  }
843 
844  S BLL_y, BLU_y, BUL_y, BUU_y;
845 
846  BLL_y = Tab[1];
847  BLU_y = BLL_y + bA1_dot_B1;
848  BUL_y = BLL_y + bA1_dot_B0;
849  BUU_y = BLU_y + bA1_dot_B0;
850 
851  S LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
852 
853  if(ALL_x < AUL_x)
854  {
855  LA0_lx = ALL_x;
856  LA0_ux = AUL_x;
857  UA0_lx = ALU_x;
858  UA0_ux = AUU_x;
859  }
860  else
861  {
862  LA0_lx = AUL_x;
863  LA0_ux = ALL_x;
864  UA0_lx = AUU_x;
865  UA0_ux = ALU_x;
866  }
867 
868  if(BLL_y < BLU_y)
869  {
870  LB1_ly = BLL_y;
871  LB1_uy = BLU_y;
872  UB1_ly = BUL_y;
873  UB1_uy = BUU_y;
874  }
875  else
876  {
877  LB1_ly = BLU_y;
878  LB1_uy = BLL_y;
879  UB1_ly = BUU_y;
880  UB1_uy = BUL_y;
881  }
882 
883  // UA0, UB1
884 
885  if((UA0_ux > b[0]) && (UB1_uy > a[1]))
886  {
887  if(((UA0_lx > b[0]) ||
888  inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0],
889  A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0))
890  &&
891  ((UB1_ly > a[1]) ||
892  inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0,
893  A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1)))
894  {
895  segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0,
896  Tba[1] - aA1_dot_B1);
897 
898  D[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
899  D[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1];
900  D[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
901 
902  if(P && Q)
903  {
904  *P << t, a[1], 0;
905  *Q = D + (*P);
906  }
907 
908  return D.norm();
909  }
910  }
911 
912  // UA0, LB1
913 
914  if((UA0_lx < 0) && (LB1_uy > a[1]))
915  {
916  if(((UA0_ux < 0) ||
917  inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1,
918  aA1_dot_B1 - Tba[1], -Tab[0]))
919  &&
920  ((LB1_ly > a[1]) ||
921  inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0],
922  Tba[1] - aA1_dot_B1)))
923  {
924  segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1);
925 
926  D[0] = Tab[0] + Rab(0, 1) * u - t;
927  D[1] = Tab[1] + Rab(1, 1) * u - a[1];
928  D[2] = Tab[2] + Rab(2, 1) * u;
929 
930  if(P && Q)
931  {
932  *P << t, a[1], 0;
933  *Q = D + (*P);
934  }
935 
936  return D.norm();
937  }
938  }
939 
940  // LA0, UB1
941 
942  if((LA0_ux > b[0]) && (UB1_ly < 0))
943  {
944  if(((LA0_lx > b[0]) ||
945  inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1],
946  -bA0_dot_B0 - Tab[0]))
947  &&
948  ((UB1_uy < 0) ||
949  inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, A0_dot_B1,
950  Tab[0] + bA0_dot_B0, Tba[1])))
951  {
952  segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]);
953 
954  D[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
955  D[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u;
956  D[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
957 
958  if(P && Q)
959  {
960  *P << t, 0, 0;
961  *Q = D + (*P);
962  }
963 
964  return D.norm();
965  }
966  }
967 
968  // LA0, LB1
969 
970  if((LA0_lx < 0) && (LB1_ly < 0))
971  {
972  if(((LA0_ux < 0) ||
973  inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1],
974  -Tab[0]))
975  &&
976  ((LB1_uy < 0) ||
977  inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1,
978  Tab[0], Tba[1])))
979  {
980  segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]);
981 
982  D[0] = Tab[0] + Rab(0, 1) * u - t;
983  D[1] = Tab[1] + Rab(1, 1) * u;
984  D[2] = Tab[2] + Rab(2, 1) * u;
985 
986  if(P && Q)
987  {
988  *P << t, 0, 0;
989  *Q = D + (*P);
990  }
991 
992  return D.norm();
993  }
994  }
995 
996  S LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
997 
998  if(ALL_y < AUL_y)
999  {
1000  LA0_ly = ALL_y;
1001  LA0_uy = AUL_y;
1002  UA0_ly = ALU_y;
1003  UA0_uy = AUU_y;
1004  }
1005  else
1006  {
1007  LA0_ly = AUL_y;
1008  LA0_uy = ALL_y;
1009  UA0_ly = AUU_y;
1010  UA0_uy = ALU_y;
1011  }
1012 
1013  if(BLL_y < BUL_y)
1014  {
1015  LB0_ly = BLL_y;
1016  LB0_uy = BUL_y;
1017  UB0_ly = BLU_y;
1018  UB0_uy = BUU_y;
1019  }
1020  else
1021  {
1022  LB0_ly = BUL_y;
1023  LB0_uy = BLL_y;
1024  UB0_ly = BUU_y;
1025  UB0_uy = BLU_y;
1026  }
1027 
1028  // UA0, UB0
1029 
1030  if((UA0_uy > b[1]) && (UB0_uy > a[1]))
1031  {
1032  if(((UA0_ly > b[1]) ||
1033  inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1],
1034  A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1))
1035  &&
1036  ((UB0_ly > a[1]) ||
1037  inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0,
1038  Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0)))
1039  {
1040  segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1,
1041  Tba[0] - aA1_dot_B0);
1042 
1043  D[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
1044  D[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1];
1045  D[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
1046 
1047  if(P && Q)
1048  {
1049  *P << t, a[1], 0;
1050  *Q = D + (*P);
1051  }
1052 
1053  return D.norm();
1054  }
1055  }
1056 
1057  // UA0, LB0
1058 
1059  if((UA0_ly < 0) && (LB0_uy > a[1]))
1060  {
1061  if(((UA0_uy < 0) ||
1062  inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0,
1063  aA1_dot_B0 - Tba[0], -Tab[0]))
1064  &&
1065  ((LB0_ly > a[1]) ||
1066  inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1],
1067  A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0)))
1068  {
1069  segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0);
1070 
1071  D[0] = Tab[0] + Rab(0, 0) * u - t;
1072  D[1] = Tab[1] + Rab(1, 0) * u - a[1];
1073  D[2] = Tab[2] + Rab(2, 0) * u;
1074 
1075  if(P && Q)
1076  {
1077  *P << t, a[1], 0;
1078  *Q = D + (*P);
1079  }
1080 
1081  return D.norm();
1082  }
1083  }
1084 
1085  // LA0, UB0
1086 
1087  if((LA0_uy > b[1]) && (UB0_ly < 0))
1088  {
1089  if(((LA0_ly > b[1]) ||
1090  inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0],
1091  -Tab[0] - bA0_dot_B1))
1092  &&
1093 
1094  ((UB0_uy < 0) ||
1095  inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0,
1096  Tab[0] + bA0_dot_B1, Tba[0])))
1097  {
1098  segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]);
1099 
1100  D[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
1101  D[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u;
1102  D[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
1103 
1104  if(P && Q)
1105  {
1106  *P << t, 0, 0;
1107  *Q = D + (*P);
1108  }
1109 
1110  return D.norm();
1111  }
1112  }
1113 
1114  // LA0, LB0
1115 
1116  if((LA0_ly < 0) && (LB0_ly < 0))
1117  {
1118  if(((LA0_uy < 0) ||
1119  inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
1120  -Tba[0], -Tab[0]))
1121  &&
1122  ((LB0_uy < 0) ||
1123  inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0,
1124  Tab[0], Tba[0])))
1125  {
1126  segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]);
1127 
1128  D[0] = Tab[0] + Rab(0, 0) * u - t;
1129  D[1] = Tab[1] + Rab(1, 0) * u;
1130  D[2] = Tab[2] + Rab(2, 0) * u;
1131 
1132  if(P && Q)
1133  {
1134  *P << t, 0, 0;
1135  *Q = D + (*P);
1136  }
1137 
1138  return D.norm();
1139  }
1140  }
1141 
1142  // no edges passed, take max separation along face normals
1143 
1144  S sep1, sep2;
1145 
1146  if(Tab[2] > 0.0)
1147  {
1148  sep1 = Tab[2];
1149  if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0);
1150  if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1);
1151  }
1152  else
1153  {
1154  sep1 = -Tab[2];
1155  if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0);
1156  if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1);
1157  }
1158 
1159  if(Tba[2] < 0)
1160  {
1161  sep2 = -Tba[2];
1162  if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2);
1163  if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2);
1164  }
1165  else
1166  {
1167  sep2 = Tba[2];
1168  if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2);
1169  if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2);
1170  }
1171 
1172  if(sep1 >= sep2 && sep1 >= 0)
1173  {
1174  if(Tab[2] > 0)
1175  D << 0, 0, sep1;
1176  else
1177  D << 0, 0, -sep1;
1178 
1179  if(P && Q)
1180  {
1181  *Q = D;
1182  P->setZero();
1183  }
1184  }
1185 
1186  if(sep2 >= sep1 && sep2 >= 0)
1187  {
1188  Vector3<S> Q_(Tab[0], Tab[1], Tab[2]);
1189  Vector3<S> P_;
1190  if(Tba[2] < 0)
1191  {
1192  P_[0] = Rab(0, 2) * sep2 + Tab[0];
1193  P_[1] = Rab(1, 2) * sep2 + Tab[1];
1194  P_[2] = Rab(2, 2) * sep2 + Tab[2];
1195  }
1196  else
1197  {
1198  P_[0] = -Rab(0, 2) * sep2 + Tab[0];
1199  P_[1] = -Rab(1, 2) * sep2 + Tab[1];
1200  P_[2] = -Rab(2, 2) * sep2 + Tab[2];
1201  }
1202 
1203  D = Q_ - P_;
1204 
1205  if(P && Q)
1206  {
1207  *P = P_;
1208  *Q = Q_;
1209  }
1210  }
1211 
1212  S sep = (sep1 > sep2 ? sep1 : sep2);
1213  return (sep > 0 ? sep : 0);
1214 }
1215 
1216 //==============================================================================
1217 template <typename S>
1218 S rectDistance(
1219  const Transform3<S>& tfab,
1220  const S a[2],
1221  const S b[2],
1222  Vector3<S>* P,
1223  Vector3<S>* Q)
1224 {
1225  S A0_dot_B0 = tfab.linear()(0, 0);
1226  S A0_dot_B1 = tfab.linear()(0, 1);
1227  S A1_dot_B0 = tfab.linear()(1, 0);
1228  S A1_dot_B1 = tfab.linear()(1, 1);
1229 
1230  S aA0_dot_B0 = a[0] * A0_dot_B0;
1231  S aA0_dot_B1 = a[0] * A0_dot_B1;
1232  S aA1_dot_B0 = a[1] * A1_dot_B0;
1233  S aA1_dot_B1 = a[1] * A1_dot_B1;
1234  S bA0_dot_B0 = b[0] * A0_dot_B0;
1235  S bA1_dot_B0 = b[0] * A1_dot_B0;
1236  S bA0_dot_B1 = b[1] * A0_dot_B1;
1237  S bA1_dot_B1 = b[1] * A1_dot_B1;
1238 
1239  Vector3<S> Tba = tfab.linear().transpose() * tfab.translation();
1240 
1241  Vector3<S> D;
1242  S t, u;
1243 
1244  // determine if any edge pair contains the closest points
1245 
1246  S LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
1247 
1248  S ALL_x = -Tba[0];
1249  S ALU_x = ALL_x + aA1_dot_B0;
1250  S AUL_x = ALL_x + aA0_dot_B0;
1251  S AUU_x = ALU_x + aA0_dot_B0;
1252 
1253  if(ALL_x < ALU_x)
1254  {
1255  LA1_lx = ALL_x;
1256  LA1_ux = ALU_x;
1257  UA1_lx = AUL_x;
1258  UA1_ux = AUU_x;
1259  }
1260  else
1261  {
1262  LA1_lx = ALU_x;
1263  LA1_ux = ALL_x;
1264  UA1_lx = AUU_x;
1265  UA1_ux = AUL_x;
1266  }
1267 
1268  S BLL_x = tfab.translation()[0];
1269  S BLU_x = BLL_x + bA0_dot_B1;
1270  S BUL_x = BLL_x + bA0_dot_B0;
1271  S BUU_x = BLU_x + bA0_dot_B0;
1272 
1273  if(BLL_x < BLU_x)
1274  {
1275  LB1_lx = BLL_x;
1276  LB1_ux = BLU_x;
1277  UB1_lx = BUL_x;
1278  UB1_ux = BUU_x;
1279  }
1280  else
1281  {
1282  LB1_lx = BLU_x;
1283  LB1_ux = BLL_x;
1284  UB1_lx = BUU_x;
1285  UB1_ux = BUL_x;
1286  }
1287 
1288  // UA1, UB1
1289 
1290  if((UA1_ux > b[0]) && (UB1_ux > a[0]))
1291  {
1292  if(((UA1_lx > b[0]) ||
1293  inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0],
1294  A1_dot_B1, aA0_dot_B1 - Tba[1],
1295  -tfab.translation()[1] - bA1_dot_B0))
1296  &&
1297  ((UB1_lx > a[0]) ||
1298  inVoronoi(a[1], b[1], A0_dot_B1, tfab.translation()[0] + bA0_dot_B0 - a[0],
1299  A1_dot_B1, tfab.translation()[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1)))
1300  {
1301  segCoords(t, u, a[1], b[1], A1_dot_B1, tfab.translation()[1] + bA1_dot_B0,
1302  Tba[1] - aA0_dot_B1);
1303 
1304  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * b[0] + tfab.linear()(0, 1) * u - a[0] ;
1305  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * b[0] + tfab.linear()(1, 1) * u - t;
1306  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * b[0] + tfab.linear()(2, 1) * u;
1307 
1308  if(P && Q)
1309  {
1310  *P << a[0], t, 0;
1311  *Q = D + (*P);
1312  }
1313 
1314  return D.norm();
1315  }
1316  }
1317 
1318 
1319  // UA1, LB1
1320 
1321  if((UA1_lx < 0) && (LB1_ux > a[0]))
1322  {
1323  if(((UA1_ux < 0) ||
1324  inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
1325  A1_dot_B1, aA0_dot_B1 - Tba[1], -tfab.translation()[1]))
1326  &&
1327  ((LB1_lx > a[0]) ||
1328  inVoronoi(a[1], b[1], A0_dot_B1, tfab.translation()[0] - a[0],
1329  A1_dot_B1, tfab.translation()[1], Tba[1] - aA0_dot_B1)))
1330  {
1331  segCoords(t, u, a[1], b[1], A1_dot_B1, tfab.translation()[1], Tba[1] - aA0_dot_B1);
1332 
1333  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * u - a[0];
1334  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * u - t;
1335  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * u;
1336 
1337  if(P && Q)
1338  {
1339  *P << a[0], t, 0;
1340  *Q = D + (*P);
1341  }
1342 
1343  return D.norm();
1344  }
1345  }
1346 
1347  // LA1, UB1
1348 
1349  if((LA1_ux > b[0]) && (UB1_lx < 0))
1350  {
1351  if(((LA1_lx > b[0]) ||
1352  inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0],
1353  A1_dot_B1, -Tba[1], -tfab.translation()[1] - bA1_dot_B0))
1354  &&
1355  ((UB1_ux < 0) ||
1356  inVoronoi(a[1], b[1], -A0_dot_B1, -tfab.translation()[0] - bA0_dot_B0,
1357  A1_dot_B1, tfab.translation()[1] + bA1_dot_B0, Tba[1])))
1358  {
1359  segCoords(t, u, a[1], b[1], A1_dot_B1, tfab.translation()[1] + bA1_dot_B0, Tba[1]);
1360 
1361  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * b[0] + tfab.linear()(0, 1) * u;
1362  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * b[0] + tfab.linear()(1, 1) * u - t;
1363  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * b[0] + tfab.linear()(2, 1) * u;
1364 
1365  if(P && Q)
1366  {
1367  *P << 0, t, 0;
1368  *Q = D + (*P);
1369  }
1370 
1371  return D.norm();
1372  }
1373  }
1374 
1375  // LA1, LB1
1376 
1377  if((LA1_lx < 0) && (LB1_lx < 0))
1378  {
1379  if (((LA1_ux < 0) ||
1380  inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1,
1381  -Tba[1], -tfab.translation()[1]))
1382  &&
1383  ((LB1_ux < 0) ||
1384  inVoronoi(a[1], b[1], -A0_dot_B1, -tfab.translation()[0], A1_dot_B1,
1385  tfab.translation()[1], Tba[1])))
1386  {
1387  segCoords(t, u, a[1], b[1], A1_dot_B1, tfab.translation()[1], Tba[1]);
1388 
1389  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * u;
1390  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * u - t;
1391  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * u;
1392 
1393  if(P && Q)
1394  {
1395  *P << 0, t, 0;
1396  *Q = D + (*P);
1397  }
1398 
1399  return D.norm();
1400  }
1401  }
1402 
1403  S ALL_y, ALU_y, AUL_y, AUU_y;
1404 
1405  ALL_y = -Tba[1];
1406  ALU_y = ALL_y + aA1_dot_B1;
1407  AUL_y = ALL_y + aA0_dot_B1;
1408  AUU_y = ALU_y + aA0_dot_B1;
1409 
1410  S LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
1411 
1412  if(ALL_y < ALU_y)
1413  {
1414  LA1_ly = ALL_y;
1415  LA1_uy = ALU_y;
1416  UA1_ly = AUL_y;
1417  UA1_uy = AUU_y;
1418  }
1419  else
1420  {
1421  LA1_ly = ALU_y;
1422  LA1_uy = ALL_y;
1423  UA1_ly = AUU_y;
1424  UA1_uy = AUL_y;
1425  }
1426 
1427  if(BLL_x < BUL_x)
1428  {
1429  LB0_lx = BLL_x;
1430  LB0_ux = BUL_x;
1431  UB0_lx = BLU_x;
1432  UB0_ux = BUU_x;
1433  }
1434  else
1435  {
1436  LB0_lx = BUL_x;
1437  LB0_ux = BLL_x;
1438  UB0_lx = BUU_x;
1439  UB0_ux = BLU_x;
1440  }
1441 
1442  // UA1, UB0
1443 
1444  if((UA1_uy > b[1]) && (UB0_ux > a[0]))
1445  {
1446  if(((UA1_ly > b[1]) ||
1447  inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1],
1448  A1_dot_B0, aA0_dot_B0 - Tba[0], -tfab.translation()[1] - bA1_dot_B1))
1449  &&
1450  ((UB0_lx > a[0]) ||
1451  inVoronoi(a[1], b[0], A0_dot_B0, tfab.translation()[0] - a[0] + bA0_dot_B1,
1452  A1_dot_B0, tfab.translation()[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0)))
1453  {
1454  segCoords(t, u, a[1], b[0], A1_dot_B0, tfab.translation()[1] + bA1_dot_B1,
1455  Tba[0] - aA0_dot_B0);
1456 
1457  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * b[1] + tfab.linear()(0, 0) * u - a[0] ;
1458  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * b[1] + tfab.linear()(1, 0) * u - t;
1459  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * b[1] + tfab.linear()(2, 0) * u;
1460 
1461  if(P && Q)
1462  {
1463  *P << a[0], t, 0;
1464  *Q = D + (*P);
1465  }
1466 
1467  return D.norm();
1468  }
1469  }
1470 
1471  // UA1, LB0
1472 
1473  if((UA1_ly < 0) && (LB0_ux > a[0]))
1474  {
1475  if(((UA1_uy < 0) ||
1476  inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0,
1477  aA0_dot_B0 - Tba[0], -tfab.translation()[1]))
1478  &&
1479  ((LB0_lx > a[0]) ||
1480  inVoronoi(a[1], b[0], A0_dot_B0, tfab.translation()[0] - a[0],
1481  A1_dot_B0, tfab.translation()[1], Tba[0] - aA0_dot_B0)))
1482  {
1483  segCoords(t, u, a[1], b[0], A1_dot_B0, tfab.translation()[1], Tba[0] - aA0_dot_B0);
1484 
1485  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * u - a[0];
1486  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * u - t;
1487  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * u;
1488 
1489  if(P && Q)
1490  {
1491  *P << a[0], t, 0;
1492  *Q = D + (*P);
1493  }
1494 
1495  return D.norm();
1496  }
1497  }
1498 
1499  // LA1, UB0
1500 
1501  if((LA1_uy > b[1]) && (UB0_lx < 0))
1502  {
1503  if(((LA1_ly > b[1]) ||
1504  inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1],
1505  A1_dot_B0, -Tba[0], -tfab.translation()[1] - bA1_dot_B1))
1506  &&
1507 
1508  ((UB0_ux < 0) ||
1509  inVoronoi(a[1], b[0], -A0_dot_B0, -tfab.translation()[0] - bA0_dot_B1, A1_dot_B0,
1510  tfab.translation()[1] + bA1_dot_B1, Tba[0])))
1511  {
1512  segCoords(t, u, a[1], b[0], A1_dot_B0, tfab.translation()[1] + bA1_dot_B1, Tba[0]);
1513 
1514  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * b[1] + tfab.linear()(0, 0) * u;
1515  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * b[1] + tfab.linear()(1, 0) * u - t;
1516  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * b[1] + tfab.linear()(2, 0) * u;
1517 
1518  if(P && Q)
1519  {
1520  *P << 0, t, 0;
1521  *Q = D + (*P);
1522  }
1523 
1524 
1525  return D.norm();
1526  }
1527  }
1528 
1529  // LA1, LB0
1530 
1531  if((LA1_ly < 0) && (LB0_lx < 0))
1532  {
1533  if(((LA1_uy < 0) ||
1534  inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0,
1535  -Tba[0], -tfab.translation()[1]))
1536  &&
1537  ((LB0_ux < 0) ||
1538  inVoronoi(a[1], b[0], -A0_dot_B0, -tfab.translation()[0], A1_dot_B0,
1539  tfab.translation()[1], Tba[0])))
1540  {
1541  segCoords(t, u, a[1], b[0], A1_dot_B0, tfab.translation()[1], Tba[0]);
1542 
1543  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * u;
1544  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * u - t;
1545  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * u;
1546 
1547  if(P&& Q)
1548  {
1549  *P << 0, t, 0;
1550  *Q = D + (*P);
1551  }
1552 
1553  return D.norm();
1554  }
1555  }
1556 
1557  S BLL_y, BLU_y, BUL_y, BUU_y;
1558 
1559  BLL_y = tfab.translation()[1];
1560  BLU_y = BLL_y + bA1_dot_B1;
1561  BUL_y = BLL_y + bA1_dot_B0;
1562  BUU_y = BLU_y + bA1_dot_B0;
1563 
1564  S LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
1565 
1566  if(ALL_x < AUL_x)
1567  {
1568  LA0_lx = ALL_x;
1569  LA0_ux = AUL_x;
1570  UA0_lx = ALU_x;
1571  UA0_ux = AUU_x;
1572  }
1573  else
1574  {
1575  LA0_lx = AUL_x;
1576  LA0_ux = ALL_x;
1577  UA0_lx = AUU_x;
1578  UA0_ux = ALU_x;
1579  }
1580 
1581  if(BLL_y < BLU_y)
1582  {
1583  LB1_ly = BLL_y;
1584  LB1_uy = BLU_y;
1585  UB1_ly = BUL_y;
1586  UB1_uy = BUU_y;
1587  }
1588  else
1589  {
1590  LB1_ly = BLU_y;
1591  LB1_uy = BLL_y;
1592  UB1_ly = BUU_y;
1593  UB1_uy = BUL_y;
1594  }
1595 
1596  // UA0, UB1
1597 
1598  if((UA0_ux > b[0]) && (UB1_uy > a[1]))
1599  {
1600  if(((UA0_lx > b[0]) ||
1601  inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0],
1602  A0_dot_B1, aA1_dot_B1 - Tba[1], -tfab.translation()[0] - bA0_dot_B0))
1603  &&
1604  ((UB1_ly > a[1]) ||
1605  inVoronoi(a[0], b[1], A1_dot_B1, tfab.translation()[1] - a[1] + bA1_dot_B0,
1606  A0_dot_B1, tfab.translation()[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1)))
1607  {
1608  segCoords(t, u, a[0], b[1], A0_dot_B1, tfab.translation()[0] + bA0_dot_B0,
1609  Tba[1] - aA1_dot_B1);
1610 
1611  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * b[0] + tfab.linear()(0, 1) * u - t;
1612  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * b[0] + tfab.linear()(1, 1) * u - a[1];
1613  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * b[0] + tfab.linear()(2, 1) * u;
1614 
1615  if(P && Q)
1616  {
1617  *P << t, a[1], 0;
1618  *Q = D + (*P);
1619  }
1620 
1621  return D.norm();
1622  }
1623  }
1624 
1625  // UA0, LB1
1626 
1627  if((UA0_lx < 0) && (LB1_uy > a[1]))
1628  {
1629  if(((UA0_ux < 0) ||
1630  inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1,
1631  aA1_dot_B1 - Tba[1], -tfab.translation()[0]))
1632  &&
1633  ((LB1_ly > a[1]) ||
1634  inVoronoi(a[0], b[1], A1_dot_B1, tfab.translation()[1] - a[1], A0_dot_B1, tfab.translation()[0],
1635  Tba[1] - aA1_dot_B1)))
1636  {
1637  segCoords(t, u, a[0], b[1], A0_dot_B1, tfab.translation()[0], Tba[1] - aA1_dot_B1);
1638 
1639  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * u - t;
1640  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * u - a[1];
1641  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * u;
1642 
1643  if(P && Q)
1644  {
1645  *P << t, a[1], 0;
1646  *Q = D + (*P);
1647  }
1648 
1649  return D.norm();
1650  }
1651  }
1652 
1653  // LA0, UB1
1654 
1655  if((LA0_ux > b[0]) && (UB1_ly < 0))
1656  {
1657  if(((LA0_lx > b[0]) ||
1658  inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1],
1659  -bA0_dot_B0 - tfab.translation()[0]))
1660  &&
1661  ((UB1_uy < 0) ||
1662  inVoronoi(a[0], b[1], -A1_dot_B1, -tfab.translation()[1] - bA1_dot_B0, A0_dot_B1,
1663  tfab.translation()[0] + bA0_dot_B0, Tba[1])))
1664  {
1665  segCoords(t, u, a[0], b[1], A0_dot_B1, tfab.translation()[0] + bA0_dot_B0, Tba[1]);
1666 
1667  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * b[0] + tfab.linear()(0, 1) * u - t;
1668  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * b[0] + tfab.linear()(1, 1) * u;
1669  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * b[0] + tfab.linear()(2, 1) * u;
1670 
1671  if(P && Q)
1672  {
1673  *P << t, 0, 0;
1674  *Q = D + (*P);
1675  }
1676 
1677  return D.norm();
1678  }
1679  }
1680 
1681  // LA0, LB1
1682 
1683  if((LA0_lx < 0) && (LB1_ly < 0))
1684  {
1685  if(((LA0_ux < 0) ||
1686  inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1],
1687  -tfab.translation()[0]))
1688  &&
1689  ((LB1_uy < 0) ||
1690  inVoronoi(a[0], b[1], -A1_dot_B1, -tfab.translation()[1], A0_dot_B1,
1691  tfab.translation()[0], Tba[1])))
1692  {
1693  segCoords(t, u, a[0], b[1], A0_dot_B1, tfab.translation()[0], Tba[1]);
1694 
1695  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * u - t;
1696  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * u;
1697  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * u;
1698 
1699  if(P && Q)
1700  {
1701  *P << t, 0, 0;
1702  *Q = D + (*P);
1703  }
1704 
1705  return D.norm();
1706  }
1707  }
1708 
1709  S LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
1710 
1711  if(ALL_y < AUL_y)
1712  {
1713  LA0_ly = ALL_y;
1714  LA0_uy = AUL_y;
1715  UA0_ly = ALU_y;
1716  UA0_uy = AUU_y;
1717  }
1718  else
1719  {
1720  LA0_ly = AUL_y;
1721  LA0_uy = ALL_y;
1722  UA0_ly = AUU_y;
1723  UA0_uy = ALU_y;
1724  }
1725 
1726  if(BLL_y < BUL_y)
1727  {
1728  LB0_ly = BLL_y;
1729  LB0_uy = BUL_y;
1730  UB0_ly = BLU_y;
1731  UB0_uy = BUU_y;
1732  }
1733  else
1734  {
1735  LB0_ly = BUL_y;
1736  LB0_uy = BLL_y;
1737  UB0_ly = BUU_y;
1738  UB0_uy = BLU_y;
1739  }
1740 
1741  // UA0, UB0
1742 
1743  if((UA0_uy > b[1]) && (UB0_uy > a[1]))
1744  {
1745  if(((UA0_ly > b[1]) ||
1746  inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1],
1747  A0_dot_B0, aA1_dot_B0 - Tba[0], -tfab.translation()[0] - bA0_dot_B1))
1748  &&
1749  ((UB0_ly > a[1]) ||
1750  inVoronoi(a[0], b[0], A1_dot_B0, tfab.translation()[1] - a[1] + bA1_dot_B1, A0_dot_B0,
1751  tfab.translation()[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0)))
1752  {
1753  segCoords(t, u, a[0], b[0], A0_dot_B0, tfab.translation()[0] + bA0_dot_B1,
1754  Tba[0] - aA1_dot_B0);
1755 
1756  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * b[1] + tfab.linear()(0, 0) * u - t;
1757  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * b[1] + tfab.linear()(1, 0) * u - a[1];
1758  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * b[1] + tfab.linear()(2, 0) * u;
1759 
1760  if(P && Q)
1761  {
1762  *P << t, a[1], 0;
1763  *Q = D + (*P);
1764  }
1765 
1766  return D.norm();
1767  }
1768  }
1769 
1770  // UA0, LB0
1771 
1772  if((UA0_ly < 0) && (LB0_uy > a[1]))
1773  {
1774  if(((UA0_uy < 0) ||
1775  inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0,
1776  aA1_dot_B0 - Tba[0], -tfab.translation()[0]))
1777  &&
1778  ((LB0_ly > a[1]) ||
1779  inVoronoi(a[0], b[0], A1_dot_B0, tfab.translation()[1] - a[1],
1780  A0_dot_B0, tfab.translation()[0], Tba[0] - aA1_dot_B0)))
1781  {
1782  segCoords(t, u, a[0], b[0], A0_dot_B0, tfab.translation()[0], Tba[0] - aA1_dot_B0);
1783 
1784  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * u - t;
1785  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * u - a[1];
1786  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * u;
1787 
1788  if(P && Q)
1789  {
1790  *P << t, a[1], 0;
1791  *Q = D + (*P);
1792  }
1793 
1794  return D.norm();
1795  }
1796  }
1797 
1798  // LA0, UB0
1799 
1800  if((LA0_uy > b[1]) && (UB0_ly < 0))
1801  {
1802  if(((LA0_ly > b[1]) ||
1803  inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0],
1804  -tfab.translation()[0] - bA0_dot_B1))
1805  &&
1806 
1807  ((UB0_uy < 0) ||
1808  inVoronoi(a[0], b[0], -A1_dot_B0, -tfab.translation()[1] - bA1_dot_B1, A0_dot_B0,
1809  tfab.translation()[0] + bA0_dot_B1, Tba[0])))
1810  {
1811  segCoords(t, u, a[0], b[0], A0_dot_B0, tfab.translation()[0] + bA0_dot_B1, Tba[0]);
1812 
1813  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * b[1] + tfab.linear()(0, 0) * u - t;
1814  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * b[1] + tfab.linear()(1, 0) * u;
1815  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * b[1] + tfab.linear()(2, 0) * u;
1816 
1817  if(P && Q)
1818  {
1819  *P << t, 0, 0;
1820  *Q = D + (*P);
1821  }
1822 
1823  return D.norm();
1824  }
1825  }
1826 
1827  // LA0, LB0
1828 
1829  if((LA0_ly < 0) && (LB0_ly < 0))
1830  {
1831  if(((LA0_uy < 0) ||
1832  inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
1833  -Tba[0], -tfab.translation()[0]))
1834  &&
1835  ((LB0_uy < 0) ||
1836  inVoronoi(a[0], b[0], -A1_dot_B0, -tfab.translation()[1], A0_dot_B0,
1837  tfab.translation()[0], Tba[0])))
1838  {
1839  segCoords(t, u, a[0], b[0], A0_dot_B0, tfab.translation()[0], Tba[0]);
1840 
1841  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * u - t;
1842  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * u;
1843  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * u;
1844 
1845  if(P && Q)
1846  {
1847  *P << t, 0, 0;
1848  *Q = D + (*P);
1849  }
1850 
1851  return D.norm();
1852  }
1853  }
1854 
1855  // no edges passed, take max separation along face normals
1856 
1857  S sep1, sep2;
1858 
1859  if(tfab.translation()[2] > 0.0)
1860  {
1861  sep1 = tfab.translation()[2];
1862  if (tfab.linear()(2, 0) < 0.0) sep1 += b[0] * tfab.linear()(2, 0);
1863  if (tfab.linear()(2, 1) < 0.0) sep1 += b[1] * tfab.linear()(2, 1);
1864  }
1865  else
1866  {
1867  sep1 = -tfab.translation()[2];
1868  if (tfab.linear()(2, 0) > 0.0) sep1 -= b[0] * tfab.linear()(2, 0);
1869  if (tfab.linear()(2, 1) > 0.0) sep1 -= b[1] * tfab.linear()(2, 1);
1870  }
1871 
1872  if(Tba[2] < 0)
1873  {
1874  sep2 = -Tba[2];
1875  if (tfab.linear()(0, 2) < 0.0) sep2 += a[0] * tfab.linear()(0, 2);
1876  if (tfab.linear()(1, 2) < 0.0) sep2 += a[1] * tfab.linear()(1, 2);
1877  }
1878  else
1879  {
1880  sep2 = Tba[2];
1881  if (tfab.linear()(0, 2) > 0.0) sep2 -= a[0] * tfab.linear()(0, 2);
1882  if (tfab.linear()(1, 2) > 0.0) sep2 -= a[1] * tfab.linear()(1, 2);
1883  }
1884 
1885  if(sep1 >= sep2 && sep1 >= 0)
1886  {
1887  if(tfab.translation()[2] > 0)
1888  D << 0, 0, sep1;
1889  else
1890  D << 0, 0, -sep1;
1891 
1892  if(P && Q)
1893  {
1894  *Q = D;
1895  P->setZero();
1896  }
1897  }
1898 
1899  if(sep2 >= sep1 && sep2 >= 0)
1900  {
1901  Vector3<S> Q_(tfab.translation());
1902  Vector3<S> P_;
1903  if(Tba[2] < 0)
1904  {
1905  P_.noalias() = tfab.linear().col(2) * sep2;
1906  P_.noalias() += tfab.translation();
1907  }
1908  else
1909  {
1910  P_.noalias() = tfab.linear().col(2) * -sep2;
1911  P_.noalias() += tfab.translation();
1912  }
1913 
1914  D = Q_ - P_;
1915 
1916  if(P && Q)
1917  {
1918  *P = P_;
1919  *Q = Q_;
1920  }
1921  }
1922 
1923  S sep = (sep1 > sep2 ? sep1 : sep2);
1924  return (sep > 0 ? sep : 0);
1925 }
1926 
1927 //==============================================================================
1928 template <typename S, typename DerivedA, typename DerivedB>
1929 bool overlap(
1930  const Eigen::MatrixBase<DerivedA>& R0,
1931  const Eigen::MatrixBase<DerivedB>& T0,
1932  const RSS<S>& b1,
1933  const RSS<S>& b2)
1934 {
1935  Matrix3<S> R0b2 = R0 * b2.axis;
1936  Matrix3<S> R = b1.axis.transpose() * R0b2;
1937 
1938  Vector3<S> Ttemp = R0 * b2.To + T0 - b1.To;
1939  Vector3<S> T = Ttemp.transpose() * b1.axis;
1940 
1941  S dist = rectDistance(R, T, b1.l, b2.l);
1942  return (dist <= (b1.r + b2.r));
1943 }
1944 
1945 //==============================================================================
1946 template <typename S, typename DerivedA, typename DerivedB>
1947 S distance(
1948  const Eigen::MatrixBase<DerivedA>& R0,
1949  const Eigen::MatrixBase<DerivedB>& T0,
1950  const RSS<S>& b1,
1951  const RSS<S>& b2,
1952  Vector3<S>* P,
1953  Vector3<S>* Q)
1954 {
1955  Matrix3<S> R0b2 = R0 * b2.axis;
1956  Matrix3<S> R = b1.axis.transpose() * R0b2;
1957 
1958  Vector3<S> Ttemp = R0 * b2.To + T0 - b1.To;
1959  Vector3<S> T = Ttemp.transpose() * b1.axis;
1960 
1961  S dist = rectDistance(R, T, b1.l, b2.l, P, Q);
1962  dist -= (b1.r + b2.r);
1963  return (dist < (S)0.0) ? (S)0.0 : dist;
1964 }
1965 
1966 //==============================================================================
1967 template <typename S>
1968 RSS<S> translate(const RSS<S>& bv, const Vector3<S>& t)
1969 {
1970  RSS<S> res(bv);
1971  res.To += t;
1972  return res;
1973 }
1974 
1975 } // namespace fcl
1976 
1977 #endif
fcl::RSS::RSS
RSS()
Constructor.
Definition: RSS-inl.h:101
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::RSS::height
S height() const
Height of the RSS.
Definition: RSS-inl.h:388
fcl::RSS::center
const Vector3< S > center() const
The RSS center.
Definition: RSS-inl.h:416
fcl::RSS::axis
Matrix3< S > axis
Orientation of RSS. The axes of the rotation matrix are the principle directions of the box...
Definition: RSS.h:60
fcl::RSS::To
Vector3< S > To
Origin of the rectangle in RSS.
Definition: RSS.h:63
fcl::translate
AABB< S > translate(const AABB< S > &aabb, const Eigen::MatrixBase< Derived > &t)
translate the center of AABB by t
Definition: AABB-inl.h:345
fcl::RSS::size
S size() const
Size of the RSS (used in BV_Splitter to order two RSSs)
Definition: RSS-inl.h:409
fcl::RSS::contain
bool contain(const Vector3< S > &p) const
Check whether the RSS contains a point.
Definition: RSS-inl.h:130
fcl::RSS::width
S width() const
Width of the RSS.
Definition: RSS-inl.h:381
fcl::RSS::l
S l[2]
Side lengths of rectangle.
Definition: RSS.h:66
fcl::RSS::overlap
bool overlap(const RSS< S > &other) const
Check collision between two RSS.
Definition: RSS-inl.h:109
fcl::RSS::volume
S volume() const
Volume of the RSS.
Definition: RSS-inl.h:402
fcl::RSS::depth
S depth() const
Depth of the RSS.
Definition: RSS-inl.h:395
fcl::RSS::r
S r
Radius of sphere summed with rectangle to form RSS.
Definition: RSS.h:69
fcl::constants
Definition: constants.h:46
fcl::RSS::operator+=
RSS< S > & operator+=(const Vector3< S > &p)
A simple way to merge the RSS and a point, not compact.
Definition: RSS-inl.h:167
fcl::RSS::operator+
RSS< S > operator+(const RSS< S > &other) const
Return the merged RSS of current RSS and the other one.
Definition: RSS-inl.h:313
fcl::RSS::distance
S distance(const RSS< S > &other, Vector3< S > *P=nullptr, Vector3< S > *Q=nullptr) const
the distance between two RSS; P and Q, if not nullptr, return the nearest points
Definition: RSS-inl.h:423