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::RSS
RSS()
Constructor.
Definition: RSS-inl.h:101
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. The first segment is defined as Pa + A*t, 0 <= t <= a, where "Pa" is one endpoint of the segment, "A" is a unit vector pointing to the other endpoint, and t is a scalar that produces all the points between the two endpoints. Since "A" is a unit vector, "a" is the segment&#39;s length. The second segment is defined as Pb + B*u, 0 <= u <= b. Many of the terms needed by the algorithm are already computed for other purposes,so we just pass these terms into the function instead of complete specifications of each segment. "T" in the dot products is the vector betweeen Pa and Pb. Reference: "On fast computation of distance between line segments." Vladimir J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985.
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
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
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::RSS::axis
Matrix3< S > axis
Frame T&#39;s orientation in frame F.
Definition: RSS.h:70
fcl::RSS::To
Vector3< S > To
Origin of frame T in frame F.
Definition: RSS.h:76
fcl::operator+
template TMatrix3< double > operator+(const Matrix3< double > &m1, const TMatrix3< double > &m2)
fcl::RSS< double >
template class FCL_EXPORT RSS< double >
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
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::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::clipToRange
void clipToRange(S &val, S a, S b)
Clip value between a and b.
Definition: RSS-inl.h:450
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
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
r
S r
Definition: test_sphere_box.cpp:171
fcl::RSS::l
S l[2]
Side lengths of rectangle in frame T.
Definition: RSS.h:79
fcl::eigen_old
template void eigen_old(const Matrix3d &m, Vector3d &dout, Matrix3d &vout)
max
static T max(T x, T y)
Definition: svm.cpp:52
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
min
static T min(T x, T y)
Definition: svm.cpp:49
fcl::RSS::r
S r
Radius of sphere summed with rectangle to form RSS.
Definition: RSS.h:82
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< Shape::S >::S
Shape::S S
Definition: RSS.h:62
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
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_catkin
Author(s):
autogenerated on Thu Mar 23 2023 03:00:18