RSS-inl.h
Go to the documentation of this file.
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 FCL_EXPORT 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  Vector3<S> p_ToCenter_T;
419  p_ToCenter_T << l[0] * 0.5, l[1] * 0.5, 0.0;
420  return p_FoTo_F() + R_FT() * p_ToCenter_T;
421 }
422 
423 template <typename S>
424 void RSS<S>::setToFromCenter(const Vector3<S>& p_FoCenter_F)
425 {
426  Vector3<S> p_ToCenter_T;
427  p_ToCenter_T << l[0] * 0.5, l[1] * 0.5, 0.0;
428  p_FoTo_F() = p_FoCenter_F - R_FT() * p_ToCenter_T;
429 }
430 
431 //==============================================================================
432 template <typename S>
433 S RSS<S>::distance(
434  const RSS<S>& other,
435  Vector3<S>* P,
436  Vector3<S>* Q) const
437 {
438  Vector3<S> t = other.To - To;
439  Vector3<S> T(
440  axis.col(0).dot(t), axis.col(1).dot(t), axis.col(2).dot(t));
441  Matrix3<S> R = axis.transpose() * other.axis;
442 
443  S dist = rectDistance(R, T, l, other.l, P, Q);
444  dist -= (r + other.r);
445  return (dist < (S)0.0) ? (S)0.0 : dist;
446 }
447 
448 //==============================================================================
449 template <typename S>
450 void clipToRange(S& val, S a, S b)
451 {
452  if(val < a) val = a;
453  else if(val > b) val = b;
454 }
455 
456 //==============================================================================
457 template <typename S>
458 void segCoords(S& t, S& u, S a, S b, S A_dot_B, S A_dot_T, S B_dot_T)
459 {
460  S denom = 1 - A_dot_B * A_dot_B;
461 
462  if(denom == 0) t = 0;
463  else
464  {
465  t = (A_dot_T - B_dot_T * A_dot_B) / denom;
466  clipToRange(t, (S)0.0, a);
467  }
468 
469  u = t * A_dot_B - B_dot_T;
470  if(u < 0)
471  {
472  u = 0;
473  t = A_dot_T;
474  clipToRange(t, (S)0.0, a);
475  }
476  else if(u > b)
477  {
478  u = b;
479  t = u * A_dot_B + A_dot_T;
480  clipToRange(t, (S)0.0, a);
481  }
482 }
483 
484 //==============================================================================
485 template <typename S>
486 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)
487 {
488  if(fabs(Anorm_dot_B) < 1e-7) return false;
489 
490  S t, u, v;
491 
492  u = -Anorm_dot_T / Anorm_dot_B;
493  clipToRange(u, (S)0.0, b);
494 
495  t = u * A_dot_B + A_dot_T;
496  clipToRange(t, (S)0.0, a);
497 
498  v = t * A_dot_B - B_dot_T;
499 
500  if(Anorm_dot_B > 0)
501  {
502  if(v > (u + 1e-7)) return true;
503  }
504  else
505  {
506  if(v < (u - 1e-7)) return true;
507  }
508  return false;
509 }
510 
511 //==============================================================================
512 template <typename S>
513 S rectDistance(const Matrix3<S>& Rab, Vector3<S> const& Tab, const S a[2], const S b[2], Vector3<S>* P, Vector3<S>* Q)
514 {
515  S A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
516 
517  A0_dot_B0 = Rab(0, 0);
518  A0_dot_B1 = Rab(0, 1);
519  A1_dot_B0 = Rab(1, 0);
520  A1_dot_B1 = Rab(1, 1);
521 
522  S aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
523  S bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
524 
525  aA0_dot_B0 = a[0] * A0_dot_B0;
526  aA0_dot_B1 = a[0] * A0_dot_B1;
527  aA1_dot_B0 = a[1] * A1_dot_B0;
528  aA1_dot_B1 = a[1] * A1_dot_B1;
529  bA0_dot_B0 = b[0] * A0_dot_B0;
530  bA1_dot_B0 = b[0] * A1_dot_B0;
531  bA0_dot_B1 = b[1] * A0_dot_B1;
532  bA1_dot_B1 = b[1] * A1_dot_B1;
533 
534  Vector3<S> Tba = Rab.transpose() * Tab;
535 
536  Vector3<S> D;
537  S t, u;
538 
539  // determine if any edge pair contains the closest points
540 
541  S ALL_x, ALU_x, AUL_x, AUU_x;
542  S BLL_x, BLU_x, BUL_x, BUU_x;
543  S LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
544 
545  ALL_x = -Tba[0];
546  ALU_x = ALL_x + aA1_dot_B0;
547  AUL_x = ALL_x + aA0_dot_B0;
548  AUU_x = ALU_x + aA0_dot_B0;
549 
550  if(ALL_x < ALU_x)
551  {
552  LA1_lx = ALL_x;
553  LA1_ux = ALU_x;
554  UA1_lx = AUL_x;
555  UA1_ux = AUU_x;
556  }
557  else
558  {
559  LA1_lx = ALU_x;
560  LA1_ux = ALL_x;
561  UA1_lx = AUU_x;
562  UA1_ux = AUL_x;
563  }
564 
565  BLL_x = Tab[0];
566  BLU_x = BLL_x + bA0_dot_B1;
567  BUL_x = BLL_x + bA0_dot_B0;
568  BUU_x = BLU_x + bA0_dot_B0;
569 
570  if(BLL_x < BLU_x)
571  {
572  LB1_lx = BLL_x;
573  LB1_ux = BLU_x;
574  UB1_lx = BUL_x;
575  UB1_ux = BUU_x;
576  }
577  else
578  {
579  LB1_lx = BLU_x;
580  LB1_ux = BLL_x;
581  UB1_lx = BUU_x;
582  UB1_ux = BUL_x;
583  }
584 
585  // UA1, UB1
586 
587  if((UA1_ux > b[0]) && (UB1_ux > a[0]))
588  {
589  if(((UA1_lx > b[0]) ||
590  inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0],
591  A1_dot_B1, aA0_dot_B1 - Tba[1],
592  -Tab[1] - bA1_dot_B0))
593  &&
594  ((UB1_lx > a[0]) ||
595  inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0],
596  A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1)))
597  {
598  segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0,
599  Tba[1] - aA0_dot_B1);
600 
601  D[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0] ;
602  D[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
603  D[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
604 
605  if(P && Q)
606  {
607  *P << a[0], t, 0;
608  *Q = D + (*P);
609  }
610 
611  return D.norm();
612  }
613  }
614 
615 
616  // UA1, LB1
617 
618  if((UA1_lx < 0) && (LB1_ux > a[0]))
619  {
620  if(((UA1_ux < 0) ||
621  inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
622  A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1]))
623  &&
624  ((LB1_lx > a[0]) ||
625  inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0],
626  A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1)))
627  {
628  segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1);
629 
630  D[0] = Tab[0] + Rab(0, 1) * u - a[0];
631  D[1] = Tab[1] + Rab(1, 1) * u - t;
632  D[2] = Tab[2] + Rab(2, 1) * u;
633 
634  if(P && Q)
635  {
636  *P << a[0], t, 0;
637  *Q = D + (*P);
638  }
639 
640  return D.norm();
641  }
642  }
643 
644  // LA1, UB1
645 
646  if((LA1_ux > b[0]) && (UB1_lx < 0))
647  {
648  if(((LA1_lx > b[0]) ||
649  inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0],
650  A1_dot_B1, -Tba[1], -Tab[1] - bA1_dot_B0))
651  &&
652  ((UB1_ux < 0) ||
653  inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0,
654  A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1])))
655  {
656  segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]);
657 
658  D[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u;
659  D[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
660  D[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
661 
662  if(P && Q)
663  {
664  *P << 0, t, 0;
665  *Q = D + (*P);
666  }
667 
668  return D.norm();
669  }
670  }
671 
672  // LA1, LB1
673 
674  if((LA1_lx < 0) && (LB1_lx < 0))
675  {
676  if (((LA1_ux < 0) ||
677  inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1,
678  -Tba[1], -Tab[1]))
679  &&
680  ((LB1_ux < 0) ||
681  inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1,
682  Tab[1], Tba[1])))
683  {
684  segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]);
685 
686  D[0] = Tab[0] + Rab(0, 1) * u;
687  D[1] = Tab[1] + Rab(1, 1) * u - t;
688  D[2] = Tab[2] + Rab(2, 1) * u;
689 
690  if(P && Q)
691  {
692  *P << 0, t, 0;
693  *Q = D + (*P);
694  }
695 
696  return D.norm();
697  }
698  }
699 
700  S ALL_y, ALU_y, AUL_y, AUU_y;
701 
702  ALL_y = -Tba[1];
703  ALU_y = ALL_y + aA1_dot_B1;
704  AUL_y = ALL_y + aA0_dot_B1;
705  AUU_y = ALU_y + aA0_dot_B1;
706 
707  S LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
708 
709  if(ALL_y < ALU_y)
710  {
711  LA1_ly = ALL_y;
712  LA1_uy = ALU_y;
713  UA1_ly = AUL_y;
714  UA1_uy = AUU_y;
715  }
716  else
717  {
718  LA1_ly = ALU_y;
719  LA1_uy = ALL_y;
720  UA1_ly = AUU_y;
721  UA1_uy = AUL_y;
722  }
723 
724  if(BLL_x < BUL_x)
725  {
726  LB0_lx = BLL_x;
727  LB0_ux = BUL_x;
728  UB0_lx = BLU_x;
729  UB0_ux = BUU_x;
730  }
731  else
732  {
733  LB0_lx = BUL_x;
734  LB0_ux = BLL_x;
735  UB0_lx = BUU_x;
736  UB0_ux = BLU_x;
737  }
738 
739  // UA1, UB0
740 
741  if((UA1_uy > b[1]) && (UB0_ux > a[0]))
742  {
743  if(((UA1_ly > b[1]) ||
744  inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1],
745  A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1))
746  &&
747  ((UB0_lx > a[0]) ||
748  inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1,
749  A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0)))
750  {
751  segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1,
752  Tba[0] - aA0_dot_B0);
753 
754  D[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0] ;
755  D[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
756  D[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
757 
758  if(P && Q)
759  {
760  *P << a[0], t, 0;
761  *Q = D + (*P);
762  }
763 
764  return D.norm();
765  }
766  }
767 
768  // UA1, LB0
769 
770  if((UA1_ly < 0) && (LB0_ux > a[0]))
771  {
772  if(((UA1_uy < 0) ||
773  inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0,
774  aA0_dot_B0 - Tba[0], -Tab[1]))
775  &&
776  ((LB0_lx > a[0]) ||
777  inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0],
778  A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0)))
779  {
780  segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0);
781 
782  D[0] = Tab[0] + Rab(0, 0) * u - a[0];
783  D[1] = Tab[1] + Rab(1, 0) * u - t;
784  D[2] = Tab[2] + Rab(2, 0) * u;
785 
786  if(P && Q)
787  {
788  *P << a[0], t, 0;
789  *Q = D + (*P);
790  }
791 
792  return D.norm();
793  }
794  }
795 
796  // LA1, UB0
797 
798  if((LA1_uy > b[1]) && (UB0_lx < 0))
799  {
800  if(((LA1_ly > b[1]) ||
801  inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1],
802  A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1))
803  &&
804 
805  ((UB0_ux < 0) ||
806  inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, A1_dot_B0,
807  Tab[1] + bA1_dot_B1, Tba[0])))
808  {
809  segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]);
810 
811  D[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u;
812  D[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
813  D[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
814 
815  if(P && Q)
816  {
817  *P << 0, t, 0;
818  *Q = D + (*P);
819  }
820 
821 
822  return D.norm();
823  }
824  }
825 
826  // LA1, LB0
827 
828  if((LA1_ly < 0) && (LB0_lx < 0))
829  {
830  if(((LA1_uy < 0) ||
831  inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0,
832  -Tba[0], -Tab[1]))
833  &&
834  ((LB0_ux < 0) ||
835  inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0,
836  Tab[1], Tba[0])))
837  {
838  segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]);
839 
840  D[0] = Tab[0] + Rab(0, 0) * u;
841  D[1] = Tab[1] + Rab(1, 0) * u - t;
842  D[2] = Tab[2] + Rab(2, 0) * u;
843 
844  if(P&& Q)
845  {
846  *P << 0, t, 0;
847  *Q = D + (*P);
848  }
849 
850  return D.norm();
851  }
852  }
853 
854  S BLL_y, BLU_y, BUL_y, BUU_y;
855 
856  BLL_y = Tab[1];
857  BLU_y = BLL_y + bA1_dot_B1;
858  BUL_y = BLL_y + bA1_dot_B0;
859  BUU_y = BLU_y + bA1_dot_B0;
860 
861  S LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
862 
863  if(ALL_x < AUL_x)
864  {
865  LA0_lx = ALL_x;
866  LA0_ux = AUL_x;
867  UA0_lx = ALU_x;
868  UA0_ux = AUU_x;
869  }
870  else
871  {
872  LA0_lx = AUL_x;
873  LA0_ux = ALL_x;
874  UA0_lx = AUU_x;
875  UA0_ux = ALU_x;
876  }
877 
878  if(BLL_y < BLU_y)
879  {
880  LB1_ly = BLL_y;
881  LB1_uy = BLU_y;
882  UB1_ly = BUL_y;
883  UB1_uy = BUU_y;
884  }
885  else
886  {
887  LB1_ly = BLU_y;
888  LB1_uy = BLL_y;
889  UB1_ly = BUU_y;
890  UB1_uy = BUL_y;
891  }
892 
893  // UA0, UB1
894 
895  if((UA0_ux > b[0]) && (UB1_uy > a[1]))
896  {
897  if(((UA0_lx > b[0]) ||
898  inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0],
899  A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0))
900  &&
901  ((UB1_ly > a[1]) ||
902  inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0,
903  A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1)))
904  {
905  segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0,
906  Tba[1] - aA1_dot_B1);
907 
908  D[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
909  D[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1];
910  D[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
911 
912  if(P && Q)
913  {
914  *P << t, a[1], 0;
915  *Q = D + (*P);
916  }
917 
918  return D.norm();
919  }
920  }
921 
922  // UA0, LB1
923 
924  if((UA0_lx < 0) && (LB1_uy > a[1]))
925  {
926  if(((UA0_ux < 0) ||
927  inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1,
928  aA1_dot_B1 - Tba[1], -Tab[0]))
929  &&
930  ((LB1_ly > a[1]) ||
931  inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0],
932  Tba[1] - aA1_dot_B1)))
933  {
934  segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1);
935 
936  D[0] = Tab[0] + Rab(0, 1) * u - t;
937  D[1] = Tab[1] + Rab(1, 1) * u - a[1];
938  D[2] = Tab[2] + Rab(2, 1) * u;
939 
940  if(P && Q)
941  {
942  *P << t, a[1], 0;
943  *Q = D + (*P);
944  }
945 
946  return D.norm();
947  }
948  }
949 
950  // LA0, UB1
951 
952  if((LA0_ux > b[0]) && (UB1_ly < 0))
953  {
954  if(((LA0_lx > b[0]) ||
955  inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1],
956  -bA0_dot_B0 - Tab[0]))
957  &&
958  ((UB1_uy < 0) ||
959  inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, A0_dot_B1,
960  Tab[0] + bA0_dot_B0, Tba[1])))
961  {
962  segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]);
963 
964  D[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
965  D[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u;
966  D[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
967 
968  if(P && Q)
969  {
970  *P << t, 0, 0;
971  *Q = D + (*P);
972  }
973 
974  return D.norm();
975  }
976  }
977 
978  // LA0, LB1
979 
980  if((LA0_lx < 0) && (LB1_ly < 0))
981  {
982  if(((LA0_ux < 0) ||
983  inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1],
984  -Tab[0]))
985  &&
986  ((LB1_uy < 0) ||
987  inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1,
988  Tab[0], Tba[1])))
989  {
990  segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]);
991 
992  D[0] = Tab[0] + Rab(0, 1) * u - t;
993  D[1] = Tab[1] + Rab(1, 1) * u;
994  D[2] = Tab[2] + Rab(2, 1) * u;
995 
996  if(P && Q)
997  {
998  *P << t, 0, 0;
999  *Q = D + (*P);
1000  }
1001 
1002  return D.norm();
1003  }
1004  }
1005 
1006  S LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
1007 
1008  if(ALL_y < AUL_y)
1009  {
1010  LA0_ly = ALL_y;
1011  LA0_uy = AUL_y;
1012  UA0_ly = ALU_y;
1013  UA0_uy = AUU_y;
1014  }
1015  else
1016  {
1017  LA0_ly = AUL_y;
1018  LA0_uy = ALL_y;
1019  UA0_ly = AUU_y;
1020  UA0_uy = ALU_y;
1021  }
1022 
1023  if(BLL_y < BUL_y)
1024  {
1025  LB0_ly = BLL_y;
1026  LB0_uy = BUL_y;
1027  UB0_ly = BLU_y;
1028  UB0_uy = BUU_y;
1029  }
1030  else
1031  {
1032  LB0_ly = BUL_y;
1033  LB0_uy = BLL_y;
1034  UB0_ly = BUU_y;
1035  UB0_uy = BLU_y;
1036  }
1037 
1038  // UA0, UB0
1039 
1040  if((UA0_uy > b[1]) && (UB0_uy > a[1]))
1041  {
1042  if(((UA0_ly > b[1]) ||
1043  inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1],
1044  A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1))
1045  &&
1046  ((UB0_ly > a[1]) ||
1047  inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0,
1048  Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0)))
1049  {
1050  segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1,
1051  Tba[0] - aA1_dot_B0);
1052 
1053  D[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
1054  D[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1];
1055  D[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
1056 
1057  if(P && Q)
1058  {
1059  *P << t, a[1], 0;
1060  *Q = D + (*P);
1061  }
1062 
1063  return D.norm();
1064  }
1065  }
1066 
1067  // UA0, LB0
1068 
1069  if((UA0_ly < 0) && (LB0_uy > a[1]))
1070  {
1071  if(((UA0_uy < 0) ||
1072  inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0,
1073  aA1_dot_B0 - Tba[0], -Tab[0]))
1074  &&
1075  ((LB0_ly > a[1]) ||
1076  inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1],
1077  A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0)))
1078  {
1079  segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0);
1080 
1081  D[0] = Tab[0] + Rab(0, 0) * u - t;
1082  D[1] = Tab[1] + Rab(1, 0) * u - a[1];
1083  D[2] = Tab[2] + Rab(2, 0) * u;
1084 
1085  if(P && Q)
1086  {
1087  *P << t, a[1], 0;
1088  *Q = D + (*P);
1089  }
1090 
1091  return D.norm();
1092  }
1093  }
1094 
1095  // LA0, UB0
1096 
1097  if((LA0_uy > b[1]) && (UB0_ly < 0))
1098  {
1099  if(((LA0_ly > b[1]) ||
1100  inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0],
1101  -Tab[0] - bA0_dot_B1))
1102  &&
1103 
1104  ((UB0_uy < 0) ||
1105  inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0,
1106  Tab[0] + bA0_dot_B1, Tba[0])))
1107  {
1108  segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]);
1109 
1110  D[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
1111  D[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u;
1112  D[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
1113 
1114  if(P && Q)
1115  {
1116  *P << t, 0, 0;
1117  *Q = D + (*P);
1118  }
1119 
1120  return D.norm();
1121  }
1122  }
1123 
1124  // LA0, LB0
1125 
1126  if((LA0_ly < 0) && (LB0_ly < 0))
1127  {
1128  if(((LA0_uy < 0) ||
1129  inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
1130  -Tba[0], -Tab[0]))
1131  &&
1132  ((LB0_uy < 0) ||
1133  inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0,
1134  Tab[0], Tba[0])))
1135  {
1136  segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]);
1137 
1138  D[0] = Tab[0] + Rab(0, 0) * u - t;
1139  D[1] = Tab[1] + Rab(1, 0) * u;
1140  D[2] = Tab[2] + Rab(2, 0) * u;
1141 
1142  if(P && Q)
1143  {
1144  *P << t, 0, 0;
1145  *Q = D + (*P);
1146  }
1147 
1148  return D.norm();
1149  }
1150  }
1151 
1152  // no edges passed, take max separation along face normals
1153 
1154  S sep1, sep2;
1155 
1156  if(Tab[2] > 0.0)
1157  {
1158  sep1 = Tab[2];
1159  if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0);
1160  if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1);
1161  }
1162  else
1163  {
1164  sep1 = -Tab[2];
1165  if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0);
1166  if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1);
1167  }
1168 
1169  if(Tba[2] < 0)
1170  {
1171  sep2 = -Tba[2];
1172  if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2);
1173  if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2);
1174  }
1175  else
1176  {
1177  sep2 = Tba[2];
1178  if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2);
1179  if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2);
1180  }
1181 
1182  if(sep1 >= sep2 && sep1 >= 0)
1183  {
1184  if(Tab[2] > 0)
1185  D << 0, 0, sep1;
1186  else
1187  D << 0, 0, -sep1;
1188 
1189  if(P && Q)
1190  {
1191  *Q = D;
1192  P->setZero();
1193  }
1194  }
1195 
1196  if(sep2 >= sep1 && sep2 >= 0)
1197  {
1198  Vector3<S> Q_(Tab[0], Tab[1], Tab[2]);
1199  Vector3<S> P_;
1200  if(Tba[2] < 0)
1201  {
1202  P_[0] = Rab(0, 2) * sep2 + Tab[0];
1203  P_[1] = Rab(1, 2) * sep2 + Tab[1];
1204  P_[2] = Rab(2, 2) * sep2 + Tab[2];
1205  }
1206  else
1207  {
1208  P_[0] = -Rab(0, 2) * sep2 + Tab[0];
1209  P_[1] = -Rab(1, 2) * sep2 + Tab[1];
1210  P_[2] = -Rab(2, 2) * sep2 + Tab[2];
1211  }
1212 
1213  D = Q_ - P_;
1214 
1215  if(P && Q)
1216  {
1217  *P = P_;
1218  *Q = Q_;
1219  }
1220  }
1221 
1222  S sep = (sep1 > sep2 ? sep1 : sep2);
1223  return (sep > 0 ? sep : 0);
1224 }
1225 
1226 //==============================================================================
1227 template <typename S>
1228 S rectDistance(
1229  const Transform3<S>& tfab,
1230  const S a[2],
1231  const S b[2],
1232  Vector3<S>* P,
1233  Vector3<S>* Q)
1234 {
1235  S A0_dot_B0 = tfab.linear()(0, 0);
1236  S A0_dot_B1 = tfab.linear()(0, 1);
1237  S A1_dot_B0 = tfab.linear()(1, 0);
1238  S A1_dot_B1 = tfab.linear()(1, 1);
1239 
1240  S aA0_dot_B0 = a[0] * A0_dot_B0;
1241  S aA0_dot_B1 = a[0] * A0_dot_B1;
1242  S aA1_dot_B0 = a[1] * A1_dot_B0;
1243  S aA1_dot_B1 = a[1] * A1_dot_B1;
1244  S bA0_dot_B0 = b[0] * A0_dot_B0;
1245  S bA1_dot_B0 = b[0] * A1_dot_B0;
1246  S bA0_dot_B1 = b[1] * A0_dot_B1;
1247  S bA1_dot_B1 = b[1] * A1_dot_B1;
1248 
1249  Vector3<S> Tba = tfab.linear().transpose() * tfab.translation();
1250 
1251  Vector3<S> D;
1252  S t, u;
1253 
1254  // determine if any edge pair contains the closest points
1255 
1256  S LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
1257 
1258  S ALL_x = -Tba[0];
1259  S ALU_x = ALL_x + aA1_dot_B0;
1260  S AUL_x = ALL_x + aA0_dot_B0;
1261  S AUU_x = ALU_x + aA0_dot_B0;
1262 
1263  if(ALL_x < ALU_x)
1264  {
1265  LA1_lx = ALL_x;
1266  LA1_ux = ALU_x;
1267  UA1_lx = AUL_x;
1268  UA1_ux = AUU_x;
1269  }
1270  else
1271  {
1272  LA1_lx = ALU_x;
1273  LA1_ux = ALL_x;
1274  UA1_lx = AUU_x;
1275  UA1_ux = AUL_x;
1276  }
1277 
1278  S BLL_x = tfab.translation()[0];
1279  S BLU_x = BLL_x + bA0_dot_B1;
1280  S BUL_x = BLL_x + bA0_dot_B0;
1281  S BUU_x = BLU_x + bA0_dot_B0;
1282 
1283  if(BLL_x < BLU_x)
1284  {
1285  LB1_lx = BLL_x;
1286  LB1_ux = BLU_x;
1287  UB1_lx = BUL_x;
1288  UB1_ux = BUU_x;
1289  }
1290  else
1291  {
1292  LB1_lx = BLU_x;
1293  LB1_ux = BLL_x;
1294  UB1_lx = BUU_x;
1295  UB1_ux = BUL_x;
1296  }
1297 
1298  // UA1, UB1
1299 
1300  if((UA1_ux > b[0]) && (UB1_ux > a[0]))
1301  {
1302  if(((UA1_lx > b[0]) ||
1303  inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0],
1304  A1_dot_B1, aA0_dot_B1 - Tba[1],
1305  -tfab.translation()[1] - bA1_dot_B0))
1306  &&
1307  ((UB1_lx > a[0]) ||
1308  inVoronoi(a[1], b[1], A0_dot_B1, tfab.translation()[0] + bA0_dot_B0 - a[0],
1309  A1_dot_B1, tfab.translation()[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1)))
1310  {
1311  segCoords(t, u, a[1], b[1], A1_dot_B1, tfab.translation()[1] + bA1_dot_B0,
1312  Tba[1] - aA0_dot_B1);
1313 
1314  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * b[0] + tfab.linear()(0, 1) * u - a[0] ;
1315  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * b[0] + tfab.linear()(1, 1) * u - t;
1316  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * b[0] + tfab.linear()(2, 1) * u;
1317 
1318  if(P && Q)
1319  {
1320  *P << a[0], t, 0;
1321  *Q = D + (*P);
1322  }
1323 
1324  return D.norm();
1325  }
1326  }
1327 
1328 
1329  // UA1, LB1
1330 
1331  if((UA1_lx < 0) && (LB1_ux > a[0]))
1332  {
1333  if(((UA1_ux < 0) ||
1334  inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
1335  A1_dot_B1, aA0_dot_B1 - Tba[1], -tfab.translation()[1]))
1336  &&
1337  ((LB1_lx > a[0]) ||
1338  inVoronoi(a[1], b[1], A0_dot_B1, tfab.translation()[0] - a[0],
1339  A1_dot_B1, tfab.translation()[1], Tba[1] - aA0_dot_B1)))
1340  {
1341  segCoords(t, u, a[1], b[1], A1_dot_B1, tfab.translation()[1], Tba[1] - aA0_dot_B1);
1342 
1343  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * u - a[0];
1344  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * u - t;
1345  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * u;
1346 
1347  if(P && Q)
1348  {
1349  *P << a[0], t, 0;
1350  *Q = D + (*P);
1351  }
1352 
1353  return D.norm();
1354  }
1355  }
1356 
1357  // LA1, UB1
1358 
1359  if((LA1_ux > b[0]) && (UB1_lx < 0))
1360  {
1361  if(((LA1_lx > b[0]) ||
1362  inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0],
1363  A1_dot_B1, -Tba[1], -tfab.translation()[1] - bA1_dot_B0))
1364  &&
1365  ((UB1_ux < 0) ||
1366  inVoronoi(a[1], b[1], -A0_dot_B1, -tfab.translation()[0] - bA0_dot_B0,
1367  A1_dot_B1, tfab.translation()[1] + bA1_dot_B0, Tba[1])))
1368  {
1369  segCoords(t, u, a[1], b[1], A1_dot_B1, tfab.translation()[1] + bA1_dot_B0, Tba[1]);
1370 
1371  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * b[0] + tfab.linear()(0, 1) * u;
1372  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * b[0] + tfab.linear()(1, 1) * u - t;
1373  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * b[0] + tfab.linear()(2, 1) * u;
1374 
1375  if(P && Q)
1376  {
1377  *P << 0, t, 0;
1378  *Q = D + (*P);
1379  }
1380 
1381  return D.norm();
1382  }
1383  }
1384 
1385  // LA1, LB1
1386 
1387  if((LA1_lx < 0) && (LB1_lx < 0))
1388  {
1389  if (((LA1_ux < 0) ||
1390  inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1,
1391  -Tba[1], -tfab.translation()[1]))
1392  &&
1393  ((LB1_ux < 0) ||
1394  inVoronoi(a[1], b[1], -A0_dot_B1, -tfab.translation()[0], A1_dot_B1,
1395  tfab.translation()[1], Tba[1])))
1396  {
1397  segCoords(t, u, a[1], b[1], A1_dot_B1, tfab.translation()[1], Tba[1]);
1398 
1399  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * u;
1400  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * u - t;
1401  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * u;
1402 
1403  if(P && Q)
1404  {
1405  *P << 0, t, 0;
1406  *Q = D + (*P);
1407  }
1408 
1409  return D.norm();
1410  }
1411  }
1412 
1413  S ALL_y, ALU_y, AUL_y, AUU_y;
1414 
1415  ALL_y = -Tba[1];
1416  ALU_y = ALL_y + aA1_dot_B1;
1417  AUL_y = ALL_y + aA0_dot_B1;
1418  AUU_y = ALU_y + aA0_dot_B1;
1419 
1420  S LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
1421 
1422  if(ALL_y < ALU_y)
1423  {
1424  LA1_ly = ALL_y;
1425  LA1_uy = ALU_y;
1426  UA1_ly = AUL_y;
1427  UA1_uy = AUU_y;
1428  }
1429  else
1430  {
1431  LA1_ly = ALU_y;
1432  LA1_uy = ALL_y;
1433  UA1_ly = AUU_y;
1434  UA1_uy = AUL_y;
1435  }
1436 
1437  if(BLL_x < BUL_x)
1438  {
1439  LB0_lx = BLL_x;
1440  LB0_ux = BUL_x;
1441  UB0_lx = BLU_x;
1442  UB0_ux = BUU_x;
1443  }
1444  else
1445  {
1446  LB0_lx = BUL_x;
1447  LB0_ux = BLL_x;
1448  UB0_lx = BUU_x;
1449  UB0_ux = BLU_x;
1450  }
1451 
1452  // UA1, UB0
1453 
1454  if((UA1_uy > b[1]) && (UB0_ux > a[0]))
1455  {
1456  if(((UA1_ly > b[1]) ||
1457  inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1],
1458  A1_dot_B0, aA0_dot_B0 - Tba[0], -tfab.translation()[1] - bA1_dot_B1))
1459  &&
1460  ((UB0_lx > a[0]) ||
1461  inVoronoi(a[1], b[0], A0_dot_B0, tfab.translation()[0] - a[0] + bA0_dot_B1,
1462  A1_dot_B0, tfab.translation()[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0)))
1463  {
1464  segCoords(t, u, a[1], b[0], A1_dot_B0, tfab.translation()[1] + bA1_dot_B1,
1465  Tba[0] - aA0_dot_B0);
1466 
1467  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * b[1] + tfab.linear()(0, 0) * u - a[0] ;
1468  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * b[1] + tfab.linear()(1, 0) * u - t;
1469  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * b[1] + tfab.linear()(2, 0) * u;
1470 
1471  if(P && Q)
1472  {
1473  *P << a[0], t, 0;
1474  *Q = D + (*P);
1475  }
1476 
1477  return D.norm();
1478  }
1479  }
1480 
1481  // UA1, LB0
1482 
1483  if((UA1_ly < 0) && (LB0_ux > a[0]))
1484  {
1485  if(((UA1_uy < 0) ||
1486  inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0,
1487  aA0_dot_B0 - Tba[0], -tfab.translation()[1]))
1488  &&
1489  ((LB0_lx > a[0]) ||
1490  inVoronoi(a[1], b[0], A0_dot_B0, tfab.translation()[0] - a[0],
1491  A1_dot_B0, tfab.translation()[1], Tba[0] - aA0_dot_B0)))
1492  {
1493  segCoords(t, u, a[1], b[0], A1_dot_B0, tfab.translation()[1], Tba[0] - aA0_dot_B0);
1494 
1495  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * u - a[0];
1496  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * u - t;
1497  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * u;
1498 
1499  if(P && Q)
1500  {
1501  *P << a[0], t, 0;
1502  *Q = D + (*P);
1503  }
1504 
1505  return D.norm();
1506  }
1507  }
1508 
1509  // LA1, UB0
1510 
1511  if((LA1_uy > b[1]) && (UB0_lx < 0))
1512  {
1513  if(((LA1_ly > b[1]) ||
1514  inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1],
1515  A1_dot_B0, -Tba[0], -tfab.translation()[1] - bA1_dot_B1))
1516  &&
1517 
1518  ((UB0_ux < 0) ||
1519  inVoronoi(a[1], b[0], -A0_dot_B0, -tfab.translation()[0] - bA0_dot_B1, A1_dot_B0,
1520  tfab.translation()[1] + bA1_dot_B1, Tba[0])))
1521  {
1522  segCoords(t, u, a[1], b[0], A1_dot_B0, tfab.translation()[1] + bA1_dot_B1, Tba[0]);
1523 
1524  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * b[1] + tfab.linear()(0, 0) * u;
1525  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * b[1] + tfab.linear()(1, 0) * u - t;
1526  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * b[1] + tfab.linear()(2, 0) * u;
1527 
1528  if(P && Q)
1529  {
1530  *P << 0, t, 0;
1531  *Q = D + (*P);
1532  }
1533 
1534 
1535  return D.norm();
1536  }
1537  }
1538 
1539  // LA1, LB0
1540 
1541  if((LA1_ly < 0) && (LB0_lx < 0))
1542  {
1543  if(((LA1_uy < 0) ||
1544  inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0,
1545  -Tba[0], -tfab.translation()[1]))
1546  &&
1547  ((LB0_ux < 0) ||
1548  inVoronoi(a[1], b[0], -A0_dot_B0, -tfab.translation()[0], A1_dot_B0,
1549  tfab.translation()[1], Tba[0])))
1550  {
1551  segCoords(t, u, a[1], b[0], A1_dot_B0, tfab.translation()[1], Tba[0]);
1552 
1553  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * u;
1554  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * u - t;
1555  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * u;
1556 
1557  if(P&& Q)
1558  {
1559  *P << 0, t, 0;
1560  *Q = D + (*P);
1561  }
1562 
1563  return D.norm();
1564  }
1565  }
1566 
1567  S BLL_y, BLU_y, BUL_y, BUU_y;
1568 
1569  BLL_y = tfab.translation()[1];
1570  BLU_y = BLL_y + bA1_dot_B1;
1571  BUL_y = BLL_y + bA1_dot_B0;
1572  BUU_y = BLU_y + bA1_dot_B0;
1573 
1574  S LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
1575 
1576  if(ALL_x < AUL_x)
1577  {
1578  LA0_lx = ALL_x;
1579  LA0_ux = AUL_x;
1580  UA0_lx = ALU_x;
1581  UA0_ux = AUU_x;
1582  }
1583  else
1584  {
1585  LA0_lx = AUL_x;
1586  LA0_ux = ALL_x;
1587  UA0_lx = AUU_x;
1588  UA0_ux = ALU_x;
1589  }
1590 
1591  if(BLL_y < BLU_y)
1592  {
1593  LB1_ly = BLL_y;
1594  LB1_uy = BLU_y;
1595  UB1_ly = BUL_y;
1596  UB1_uy = BUU_y;
1597  }
1598  else
1599  {
1600  LB1_ly = BLU_y;
1601  LB1_uy = BLL_y;
1602  UB1_ly = BUU_y;
1603  UB1_uy = BUL_y;
1604  }
1605 
1606  // UA0, UB1
1607 
1608  if((UA0_ux > b[0]) && (UB1_uy > a[1]))
1609  {
1610  if(((UA0_lx > b[0]) ||
1611  inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0],
1612  A0_dot_B1, aA1_dot_B1 - Tba[1], -tfab.translation()[0] - bA0_dot_B0))
1613  &&
1614  ((UB1_ly > a[1]) ||
1615  inVoronoi(a[0], b[1], A1_dot_B1, tfab.translation()[1] - a[1] + bA1_dot_B0,
1616  A0_dot_B1, tfab.translation()[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1)))
1617  {
1618  segCoords(t, u, a[0], b[1], A0_dot_B1, tfab.translation()[0] + bA0_dot_B0,
1619  Tba[1] - aA1_dot_B1);
1620 
1621  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * b[0] + tfab.linear()(0, 1) * u - t;
1622  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * b[0] + tfab.linear()(1, 1) * u - a[1];
1623  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * b[0] + tfab.linear()(2, 1) * u;
1624 
1625  if(P && Q)
1626  {
1627  *P << t, a[1], 0;
1628  *Q = D + (*P);
1629  }
1630 
1631  return D.norm();
1632  }
1633  }
1634 
1635  // UA0, LB1
1636 
1637  if((UA0_lx < 0) && (LB1_uy > a[1]))
1638  {
1639  if(((UA0_ux < 0) ||
1640  inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1,
1641  aA1_dot_B1 - Tba[1], -tfab.translation()[0]))
1642  &&
1643  ((LB1_ly > a[1]) ||
1644  inVoronoi(a[0], b[1], A1_dot_B1, tfab.translation()[1] - a[1], A0_dot_B1, tfab.translation()[0],
1645  Tba[1] - aA1_dot_B1)))
1646  {
1647  segCoords(t, u, a[0], b[1], A0_dot_B1, tfab.translation()[0], Tba[1] - aA1_dot_B1);
1648 
1649  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * u - t;
1650  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * u - a[1];
1651  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * u;
1652 
1653  if(P && Q)
1654  {
1655  *P << t, a[1], 0;
1656  *Q = D + (*P);
1657  }
1658 
1659  return D.norm();
1660  }
1661  }
1662 
1663  // LA0, UB1
1664 
1665  if((LA0_ux > b[0]) && (UB1_ly < 0))
1666  {
1667  if(((LA0_lx > b[0]) ||
1668  inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1],
1669  -bA0_dot_B0 - tfab.translation()[0]))
1670  &&
1671  ((UB1_uy < 0) ||
1672  inVoronoi(a[0], b[1], -A1_dot_B1, -tfab.translation()[1] - bA1_dot_B0, A0_dot_B1,
1673  tfab.translation()[0] + bA0_dot_B0, Tba[1])))
1674  {
1675  segCoords(t, u, a[0], b[1], A0_dot_B1, tfab.translation()[0] + bA0_dot_B0, Tba[1]);
1676 
1677  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * b[0] + tfab.linear()(0, 1) * u - t;
1678  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * b[0] + tfab.linear()(1, 1) * u;
1679  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * b[0] + tfab.linear()(2, 1) * u;
1680 
1681  if(P && Q)
1682  {
1683  *P << t, 0, 0;
1684  *Q = D + (*P);
1685  }
1686 
1687  return D.norm();
1688  }
1689  }
1690 
1691  // LA0, LB1
1692 
1693  if((LA0_lx < 0) && (LB1_ly < 0))
1694  {
1695  if(((LA0_ux < 0) ||
1696  inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1],
1697  -tfab.translation()[0]))
1698  &&
1699  ((LB1_uy < 0) ||
1700  inVoronoi(a[0], b[1], -A1_dot_B1, -tfab.translation()[1], A0_dot_B1,
1701  tfab.translation()[0], Tba[1])))
1702  {
1703  segCoords(t, u, a[0], b[1], A0_dot_B1, tfab.translation()[0], Tba[1]);
1704 
1705  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * u - t;
1706  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * u;
1707  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * u;
1708 
1709  if(P && Q)
1710  {
1711  *P << t, 0, 0;
1712  *Q = D + (*P);
1713  }
1714 
1715  return D.norm();
1716  }
1717  }
1718 
1719  S LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
1720 
1721  if(ALL_y < AUL_y)
1722  {
1723  LA0_ly = ALL_y;
1724  LA0_uy = AUL_y;
1725  UA0_ly = ALU_y;
1726  UA0_uy = AUU_y;
1727  }
1728  else
1729  {
1730  LA0_ly = AUL_y;
1731  LA0_uy = ALL_y;
1732  UA0_ly = AUU_y;
1733  UA0_uy = ALU_y;
1734  }
1735 
1736  if(BLL_y < BUL_y)
1737  {
1738  LB0_ly = BLL_y;
1739  LB0_uy = BUL_y;
1740  UB0_ly = BLU_y;
1741  UB0_uy = BUU_y;
1742  }
1743  else
1744  {
1745  LB0_ly = BUL_y;
1746  LB0_uy = BLL_y;
1747  UB0_ly = BUU_y;
1748  UB0_uy = BLU_y;
1749  }
1750 
1751  // UA0, UB0
1752 
1753  if((UA0_uy > b[1]) && (UB0_uy > a[1]))
1754  {
1755  if(((UA0_ly > b[1]) ||
1756  inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1],
1757  A0_dot_B0, aA1_dot_B0 - Tba[0], -tfab.translation()[0] - bA0_dot_B1))
1758  &&
1759  ((UB0_ly > a[1]) ||
1760  inVoronoi(a[0], b[0], A1_dot_B0, tfab.translation()[1] - a[1] + bA1_dot_B1, A0_dot_B0,
1761  tfab.translation()[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0)))
1762  {
1763  segCoords(t, u, a[0], b[0], A0_dot_B0, tfab.translation()[0] + bA0_dot_B1,
1764  Tba[0] - aA1_dot_B0);
1765 
1766  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * b[1] + tfab.linear()(0, 0) * u - t;
1767  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * b[1] + tfab.linear()(1, 0) * u - a[1];
1768  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * b[1] + tfab.linear()(2, 0) * u;
1769 
1770  if(P && Q)
1771  {
1772  *P << t, a[1], 0;
1773  *Q = D + (*P);
1774  }
1775 
1776  return D.norm();
1777  }
1778  }
1779 
1780  // UA0, LB0
1781 
1782  if((UA0_ly < 0) && (LB0_uy > a[1]))
1783  {
1784  if(((UA0_uy < 0) ||
1785  inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0,
1786  aA1_dot_B0 - Tba[0], -tfab.translation()[0]))
1787  &&
1788  ((LB0_ly > a[1]) ||
1789  inVoronoi(a[0], b[0], A1_dot_B0, tfab.translation()[1] - a[1],
1790  A0_dot_B0, tfab.translation()[0], Tba[0] - aA1_dot_B0)))
1791  {
1792  segCoords(t, u, a[0], b[0], A0_dot_B0, tfab.translation()[0], Tba[0] - aA1_dot_B0);
1793 
1794  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * u - t;
1795  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * u - a[1];
1796  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * u;
1797 
1798  if(P && Q)
1799  {
1800  *P << t, a[1], 0;
1801  *Q = D + (*P);
1802  }
1803 
1804  return D.norm();
1805  }
1806  }
1807 
1808  // LA0, UB0
1809 
1810  if((LA0_uy > b[1]) && (UB0_ly < 0))
1811  {
1812  if(((LA0_ly > b[1]) ||
1813  inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0],
1814  -tfab.translation()[0] - bA0_dot_B1))
1815  &&
1816 
1817  ((UB0_uy < 0) ||
1818  inVoronoi(a[0], b[0], -A1_dot_B0, -tfab.translation()[1] - bA1_dot_B1, A0_dot_B0,
1819  tfab.translation()[0] + bA0_dot_B1, Tba[0])))
1820  {
1821  segCoords(t, u, a[0], b[0], A0_dot_B0, tfab.translation()[0] + bA0_dot_B1, Tba[0]);
1822 
1823  D[0] = tfab.translation()[0] + tfab.linear()(0, 1) * b[1] + tfab.linear()(0, 0) * u - t;
1824  D[1] = tfab.translation()[1] + tfab.linear()(1, 1) * b[1] + tfab.linear()(1, 0) * u;
1825  D[2] = tfab.translation()[2] + tfab.linear()(2, 1) * b[1] + tfab.linear()(2, 0) * u;
1826 
1827  if(P && Q)
1828  {
1829  *P << t, 0, 0;
1830  *Q = D + (*P);
1831  }
1832 
1833  return D.norm();
1834  }
1835  }
1836 
1837  // LA0, LB0
1838 
1839  if((LA0_ly < 0) && (LB0_ly < 0))
1840  {
1841  if(((LA0_uy < 0) ||
1842  inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
1843  -Tba[0], -tfab.translation()[0]))
1844  &&
1845  ((LB0_uy < 0) ||
1846  inVoronoi(a[0], b[0], -A1_dot_B0, -tfab.translation()[1], A0_dot_B0,
1847  tfab.translation()[0], Tba[0])))
1848  {
1849  segCoords(t, u, a[0], b[0], A0_dot_B0, tfab.translation()[0], Tba[0]);
1850 
1851  D[0] = tfab.translation()[0] + tfab.linear()(0, 0) * u - t;
1852  D[1] = tfab.translation()[1] + tfab.linear()(1, 0) * u;
1853  D[2] = tfab.translation()[2] + tfab.linear()(2, 0) * u;
1854 
1855  if(P && Q)
1856  {
1857  *P << t, 0, 0;
1858  *Q = D + (*P);
1859  }
1860 
1861  return D.norm();
1862  }
1863  }
1864 
1865  // no edges passed, take max separation along face normals
1866 
1867  S sep1, sep2;
1868 
1869  if(tfab.translation()[2] > 0.0)
1870  {
1871  sep1 = tfab.translation()[2];
1872  if (tfab.linear()(2, 0) < 0.0) sep1 += b[0] * tfab.linear()(2, 0);
1873  if (tfab.linear()(2, 1) < 0.0) sep1 += b[1] * tfab.linear()(2, 1);
1874  }
1875  else
1876  {
1877  sep1 = -tfab.translation()[2];
1878  if (tfab.linear()(2, 0) > 0.0) sep1 -= b[0] * tfab.linear()(2, 0);
1879  if (tfab.linear()(2, 1) > 0.0) sep1 -= b[1] * tfab.linear()(2, 1);
1880  }
1881 
1882  if(Tba[2] < 0)
1883  {
1884  sep2 = -Tba[2];
1885  if (tfab.linear()(0, 2) < 0.0) sep2 += a[0] * tfab.linear()(0, 2);
1886  if (tfab.linear()(1, 2) < 0.0) sep2 += a[1] * tfab.linear()(1, 2);
1887  }
1888  else
1889  {
1890  sep2 = Tba[2];
1891  if (tfab.linear()(0, 2) > 0.0) sep2 -= a[0] * tfab.linear()(0, 2);
1892  if (tfab.linear()(1, 2) > 0.0) sep2 -= a[1] * tfab.linear()(1, 2);
1893  }
1894 
1895  if(sep1 >= sep2 && sep1 >= 0)
1896  {
1897  if(tfab.translation()[2] > 0)
1898  D << 0, 0, sep1;
1899  else
1900  D << 0, 0, -sep1;
1901 
1902  if(P && Q)
1903  {
1904  *Q = D;
1905  P->setZero();
1906  }
1907  }
1908 
1909  if(sep2 >= sep1 && sep2 >= 0)
1910  {
1911  Vector3<S> Q_(tfab.translation());
1912  Vector3<S> P_;
1913  if(Tba[2] < 0)
1914  {
1915  P_.noalias() = tfab.linear().col(2) * sep2;
1916  P_.noalias() += tfab.translation();
1917  }
1918  else
1919  {
1920  P_.noalias() = tfab.linear().col(2) * -sep2;
1921  P_.noalias() += tfab.translation();
1922  }
1923 
1924  D = Q_ - P_;
1925 
1926  if(P && Q)
1927  {
1928  *P = P_;
1929  *Q = Q_;
1930  }
1931  }
1932 
1933  S sep = (sep1 > sep2 ? sep1 : sep2);
1934  return (sep > 0 ? sep : 0);
1935 }
1936 
1937 //==============================================================================
1938 template <typename S, typename DerivedA, typename DerivedB>
1939 bool overlap(
1940  const Eigen::MatrixBase<DerivedA>& R0,
1941  const Eigen::MatrixBase<DerivedB>& T0,
1942  const RSS<S>& b1,
1943  const RSS<S>& b2)
1944 {
1945  Matrix3<S> R0b2 = R0 * b2.axis;
1946  Matrix3<S> R = b1.axis.transpose() * R0b2;
1947 
1948  Vector3<S> Ttemp = R0 * b2.To + T0 - b1.To;
1949  Vector3<S> T = Ttemp.transpose() * b1.axis;
1950 
1951  S dist = rectDistance(R, T, b1.l, b2.l);
1952  return (dist <= (b1.r + b2.r));
1953 }
1954 
1955 //==============================================================================
1956 template <typename S, typename DerivedA, typename DerivedB>
1957 S distance(
1958  const Eigen::MatrixBase<DerivedA>& R0,
1959  const Eigen::MatrixBase<DerivedB>& T0,
1960  const RSS<S>& b1,
1961  const RSS<S>& b2,
1962  Vector3<S>* P,
1963  Vector3<S>* Q)
1964 {
1965  Matrix3<S> R0b2 = R0 * b2.axis;
1966  Matrix3<S> R = b1.axis.transpose() * R0b2;
1967 
1968  Vector3<S> Ttemp = R0 * b2.To + T0 - b1.To;
1969  Vector3<S> T = Ttemp.transpose() * b1.axis;
1970 
1971  S dist = rectDistance(R, T, b1.l, b2.l, P, Q);
1972  dist -= (b1.r + b2.r);
1973  return (dist < (S)0.0) ? (S)0.0 : dist;
1974 }
1975 
1976 //==============================================================================
1977 template <typename S>
1978 RSS<S> translate(const RSS<S>& bv, const Vector3<S>& t)
1979 {
1980  RSS<S> res(bv);
1981  res.To += t;
1982  return res;
1983 }
1984 
1985 } // namespace fcl
1986 
1987 #endif
fcl::RSS< double >
template class FCL_EXPORT RSS< double >
fcl::RSS< Shape::S >::S
Shape::S S
Definition: RSS.h:62
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:92
fcl::rectDistance
template double rectDistance(const Matrix3< double > &Rab, const Vector3< double > &Tab, const double a[2], const double b[2], Vector3< double > *P, Vector3< double > *Q)
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::rectDistance
S rectDistance(const Transform3< S > &tfab, const S a[2], const S b[2], Vector3< S > *P, Vector3< S > *Q)
Distance between two oriented rectangles; P and Q (optional return values) are the closest points in ...
Definition: RSS-inl.h:1228
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::RSS::To
Vector3< S > To
Origin of frame T in frame F.
Definition: RSS.h:76
fcl::RSS< S >
fcl::inVoronoi
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)
Returns whether the nearest point on rectangle edge Pb + B*u, 0 <= u <= b, to the rectangle edge,...
Definition: RSS-inl.h:486
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:71
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:86
fcl::RSS::RSS
RSS()
Constructor.
Definition: RSS-inl.h:101
fcl::clipToRange
void clipToRange(S &val, S a, S b)
Clip value between a and b.
Definition: RSS-inl.h:450
fcl::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const RSS< S > &b1, const RSS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
distance between two RSS bounding volumes P and Q (optional return values) are the closest points in ...
Definition: RSS-inl.h:1957
fcl::RSS::l
S l[2]
Side lengths of rectangle in frame T.
Definition: RSS.h:79
r
S r
Definition: test_sphere_box.cpp:171
fcl::eigen_old
template void eigen_old(const Matrix3d &m, Vector3d &dout, Matrix3d &vout)
fcl::inVoronoi
template bool inVoronoi(double a, double b, double Anorm_dot_B, double Anorm_dot_T, double A_dot_B, double A_dot_T, double B_dot_T)
fcl::RSS::r
S r
Radius of sphere summed with rectangle to form RSS.
Definition: RSS.h:82
min
static T min(T x, T y)
Definition: svm.cpp:49
fcl::constants
Definition: constants.h:129
fcl::segCoords
template void segCoords(double &t, double &u, double a, double b, double A_dot_B, double A_dot_T, double B_dot_T)
fcl::overlap
bool overlap(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const RSS< S > &b1, const RSS< S > &b2)
Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
Definition: RSS-inl.h:1939
RSS.h
fcl::segCoords
void segCoords(S &t, S &u, S a, S b, S A_dot_B, S A_dot_T, S B_dot_T)
Finds the parameters t & u corresponding to the two closest points on a pair of line segments....
Definition: RSS-inl.h:458
fcl::translate
RSS< S > translate(const RSS< S > &bv, const Vector3< S > &t)
Translate the RSS bv.
Definition: RSS-inl.h:1978
fcl::clipToRange
template void clipToRange(double &val, double a, double b)
fcl::RSS::axis
Matrix3< S > axis
Frame T's orientation in frame F.
Definition: RSS.h:70
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::operator+
template TMatrix3< double > operator+(const Matrix3< double > &m1, const TMatrix3< double > &m2)

fcl
Author(s):
autogenerated on Fri Mar 14 2025 02:38:18