box_box-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_NARROWPHASE_DETAIL_BOXBOX_INL_H
39 #define FCL_NARROWPHASE_DETAIL_BOXBOX_INL_H
40 
42 
43 #include <algorithm>
44 
45 namespace fcl
46 {
47 
48 namespace detail
49 {
50 
51 //==============================================================================
52 extern template
53 void lineClosestApproach(const Vector3<double>& pa, const Vector3<double>& ua,
54  const Vector3<double>& pb, const Vector3<double>& ub,
55  double* alpha, double* beta);
56 
57 //==============================================================================
58 extern template
59 int intersectRectQuad2(double h[2], double p[8], double ret[16]);
60 
61 //==============================================================================
62 extern template
63 void cullPoints2(int n, double p[], int m, int i0, int iret[]);
64 
65 //==============================================================================
66 extern template
67 int boxBox2(
68  const Vector3<double>& side1,
69  const Transform3<double>& tf1,
70  const Vector3<double>& side2,
71  const Transform3<double>& tf2,
72  Vector3<double>& normal,
73  double* depth,
74  int* return_code,
75  int maxc,
76  std::vector<ContactPoint<double>>& contacts);
77 
78 //==============================================================================
79 extern template
80 bool boxBoxIntersect(const Box<double>& s1, const Transform3<double>& tf1,
81  const Box<double>& s2, const Transform3<double>& tf2,
82  std::vector<ContactPoint<double>>* contacts_);
83 
84 //==============================================================================
85 template <typename S>
86 void lineClosestApproach(const Vector3<S>& pa, const Vector3<S>& ua,
87  const Vector3<S>& pb, const Vector3<S>& ub,
88  S* alpha, S* beta)
89 {
90  Vector3<S> p = pb - pa;
91  S uaub = ua.dot(ub);
92  S q1 = ua.dot(p);
93  S q2 = -ub.dot(p);
94  S d = 1 - uaub * uaub;
95  if(d <= (S)(0.0001f))
96  {
97  *alpha = 0;
98  *beta = 0;
99  }
100  else
101  {
102  d = 1 / d;
103  *alpha = (q1 + uaub * q2) * d;
104  *beta = (uaub * q1 + q2) * d;
105  }
106 }
107 
108 //==============================================================================
109 template <typename S>
110 int intersectRectQuad2(S h[2], S p[8], S ret[16])
111 {
112  // q (and r) contain nq (and nr) coordinate points for the current (and
113  // chopped) polygons
114  int nq = 4, nr = 0;
115  S buffer[16];
116  S* q = p;
117  S* r = ret;
118  for(int dir = 0; dir <= 1; ++dir)
119  {
120  // direction notation: xy[0] = x axis, xy[1] = y axis
121  for(int sign = -1; sign <= 1; sign += 2)
122  {
123  // chop q along the line xy[dir] = sign*h[dir]
124  S* pq = q;
125  S* pr = r;
126  nr = 0;
127  for(int i = nq; i > 0; --i)
128  {
129  // go through all points in q and all lines between adjacent points
130  if(sign * pq[dir] < h[dir])
131  {
132  // this point is inside the chopping line
133  pr[0] = pq[0];
134  pr[1] = pq[1];
135  pr += 2;
136  nr++;
137  if(nr & 8)
138  {
139  q = r;
140  goto done;
141  }
142  }
143  S* nextq = (i > 1) ? pq+2 : q;
144  if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir]))
145  {
146  // this line crosses the chopping line
147  pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
148  (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
149  pr[dir] = sign*h[dir];
150  pr += 2;
151  nr++;
152  if(nr & 8)
153  {
154  q = r;
155  goto done;
156  }
157  }
158  pq += 2;
159  }
160  q = r;
161  r = (q == ret) ? buffer : ret;
162  nq = nr;
163  }
164  }
165 
166  done:
167  if(q != ret) memcpy(ret, q, nr*2*sizeof(S));
168  return nr;
169 }
170 
171 //==============================================================================
172 template <typename S>
173 void cullPoints2(int n, S p[], int m, int i0, int iret[])
174 {
175  // compute the centroid of the polygon in cx,cy
176  S a, cx, cy, q;
177  switch(n)
178  {
179  case 1:
180  cx = p[0];
181  cy = p[1];
182  break;
183  case 2:
184  cx = 0.5 * (p[0] + p[2]);
185  cy = 0.5 * (p[1] + p[3]);
186  break;
187  default:
188  a = 0;
189  cx = 0;
190  cy = 0;
191  for(int i = 0; i < n-1; ++i)
192  {
193  q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
194  a += q;
195  cx += q*(p[i*2]+p[i*2+2]);
196  cy += q*(p[i*2+1]+p[i*2+3]);
197  }
198  q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
199  if(std::abs(a+q) > std::numeric_limits<S>::epsilon())
200  a = 1/(3*(a+q));
201  else
202  a= 1e18f;
203 
204  cx = a*(cx + q*(p[n*2-2]+p[0]));
205  cy = a*(cy + q*(p[n*2-1]+p[1]));
206  }
207 
208 
209  // compute the angle of each point w.r.t. the centroid
210  S A[8];
211  for(int i = 0; i < n; ++i)
212  A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx);
213 
214  // search for points that have angles closest to A[i0] + i*(2*pi/m).
215  int avail[8];
216  for(int i = 0; i < n; ++i) avail[i] = 1;
217  avail[i0] = 0;
218  iret[0] = i0;
219  iret++;
220  const S pi = constants<S>::pi();
221  for(int j = 1; j < m; ++j)
222  {
223  a = j*(2*pi/m) + A[i0];
224  if (a > pi) a -= 2*pi;
225  S maxdiff= 1e9, diff;
226 
227  *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
228  for(int i = 0; i < n; ++i)
229  {
230  if(avail[i])
231  {
232  diff = std::abs(A[i]-a);
233  if(diff > pi) diff = 2*pi - diff;
234  if(diff < maxdiff)
235  {
236  maxdiff = diff;
237  *iret = i;
238  }
239  }
240  }
241  avail[*iret] = 0;
242  iret++;
243  }
244 }
245 
246 //==============================================================================
247 template <typename S, typename DerivedA, typename DerivedB>
249  const Vector3<S>& side1,
250  const Eigen::MatrixBase<DerivedA>& R1,
251  const Eigen::MatrixBase<DerivedB>& T1,
252  const Vector3<S>& side2,
253  const Eigen::MatrixBase<DerivedA>& R2,
254  const Eigen::MatrixBase<DerivedB>& T2,
255  Vector3<S>& normal,
256  S* depth,
257  int* return_code,
258  int maxc,
259  std::vector<ContactPoint<S>>& contacts)
260 {
261  // NOTE: This fudge factor is used to prefer face-feature contact over
262  // edge-edge. It is unclear why edge-edge contact receives this 5% penalty.
263  // Someone should document this.
264  const S fudge_factor = S(1.05);
265  Vector3<S> normalC;
266  S s, s2, l;
267  int invert_normal, code;
268 
269  Vector3<S> p = T2 - T1; // get vector from centers of box 1 to box 2, relative to box 1
270  Vector3<S> pp = R1.transpose() * p; // get pp = p relative to body 1
271 
272  // get side lengths / 2
273  Vector3<S> A = side1 * 0.5;
274  Vector3<S> B = side2 * 0.5;
275 
276  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
277  Matrix3<S> R = R1.transpose() * R2;
278  Matrix3<S> Q = R.cwiseAbs();
279 
280  // for all 15 possible separating axes:
281  // * see if the axis separates the boxes. if so, return 0.
282  // * find the depth of the penetration along the separating axis (s2)
283  // * if this is the largest depth so far, record it.
284  // the normal vector will be set to the separating axis with the smallest
285  // depth. note: normalR is set to point to a column of R1 or R2 if that is
286  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
287  // set to a vector relative to body 1. invert_normal is 1 if the sign of
288  // the normal should be flipped.
289 
290  int best_col_id = -1;
291  const Eigen::MatrixBase<DerivedA>* normalR = 0;
292  S tmp = 0;
293 
295  invert_normal = 0;
296  code = 0;
297 
298  // separating axis = u1, u2, u3
299  tmp = pp[0];
300  s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]);
301  if(s2 > 0) { *return_code = 0; return 0; }
302  if(s2 > s)
303  {
304  s = s2;
305  best_col_id = 0;
306  normalR = &R1;
307  invert_normal = (tmp < 0);
308  code = 1;
309  }
310 
311  tmp = pp[1];
312  s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]);
313  if(s2 > 0) { *return_code = 0; return 0; }
314  if(s2 > s)
315  {
316  s = s2;
317  best_col_id = 1;
318  normalR = &R1;
319  invert_normal = (tmp < 0);
320  code = 2;
321  }
322 
323  tmp = pp[2];
324  s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]);
325  if(s2 > 0) { *return_code = 0; return 0; }
326  if(s2 > s)
327  {
328  s = s2;
329  best_col_id = 2;
330  normalR = &R1;
331  invert_normal = (tmp < 0);
332  code = 3;
333  }
334 
335  // separating axis = v1, v2, v3
336  tmp = R2.col(0).dot(p);
337  s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]);
338  if(s2 > 0) { *return_code = 0; return 0; }
339  if(s2 > s)
340  {
341  s = s2;
342  best_col_id = 0;
343  normalR = &R2;
344  invert_normal = (tmp < 0);
345  code = 4;
346  }
347 
348  tmp = R2.col(1).dot(p);
349  s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]);
350  if(s2 > 0) { *return_code = 0; return 0; }
351  if(s2 > s)
352  {
353  s = s2;
354  best_col_id = 1;
355  normalR = &R2;
356  invert_normal = (tmp < 0);
357  code = 5;
358  }
359 
360  tmp = R2.col(2).dot(p);
361  s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]);
362  if(s2 > 0) { *return_code = 0; return 0; }
363  if(s2 > s)
364  {
365  s = s2;
366  best_col_id = 2;
367  normalR = &R2;
368  invert_normal = (tmp < 0);
369  code = 6;
370  }
371 
372 
373  // This is used to detect zero-length axes which arise from taking the cross
374  // product of parallel edges.
376 
377  // We are performing the mathematical test: 0 > 0 (which should always be
378  // false). However, zero can sometimes be 1e-16 or 1.5e-16. Thus,
379  // mathematically we would interpret a scenario as 0 > 0 but end up with
380  // 1.5e-16 > 1e-16. The former evaluates to false, the latter evaluates to
381  // true; a clear falsehood.
382  // (See http://gamma.cs.unc.edu/users/gottschalk/main.pdf, page 83).
383  //
384  // Gottschalk simply offers the value 1e-6 without any justification.
385  // For double-precision values, that is quite a large epsilon; we can do
386  // better. The idea is that this fictional zero can be scaled by the boxes'
387  // dimensions and summed up four times. To account for that zero, we need to
388  // make sure our epsilon is large enough to account for the *maximum* scale
389  // factor and the sum. We pad Q by that amount so that the error will *not*
390  // be the largest term.
391  {
392  using std::max;
393  // For small boxes (dimensions all less than 1), limit the scale factor to
394  // be no smaller than 10 * eps. This assumes all dimensions are strictly
395  // non-negative.
396  S scale_factor = max(max(A.maxCoeff(), B.maxCoeff()), S(1.0)) * S(10) * eps;
397  Q.array() += scale_factor;
398  }
399 
400  Vector3<S> n;
401 
402  // separating axis = u1 x (v1,v2,v3)
403  tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0);
404  s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1));
405  if(s2 > 0) { *return_code = 0; return 0; }
406  n = Vector3<S>(0, -R(2, 0), R(1, 0));
407  l = n.norm();
408  if(l > eps)
409  {
410  s2 /= l;
411  if(s2 * fudge_factor > s)
412  {
413  s = s2;
414  best_col_id = -1;
415  normalC = n / l;
416  invert_normal = (tmp < 0);
417  code = 7;
418  }
419  }
420 
421  tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1);
422  s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0));
423  if(s2 > 0) { *return_code = 0; return 0; }
424  n = Vector3<S>(0, -R(2, 1), R(1, 1));
425  l = n.norm();
426  if(l > eps)
427  {
428  s2 /= l;
429  if(s2 * fudge_factor > s)
430  {
431  s = s2;
432  best_col_id = -1;
433  normalC = n / l;
434  invert_normal = (tmp < 0);
435  code = 8;
436  }
437  }
438 
439  tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2);
440  s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0));
441  if(s2 > 0) { *return_code = 0; return 0; }
442  n = Vector3<S>(0, -R(2, 2), R(1, 2));
443  l = n.norm();
444  if(l > eps)
445  {
446  s2 /= l;
447  if(s2 * fudge_factor > s)
448  {
449  s = s2;
450  best_col_id = -1;
451  normalC = n / l;
452  invert_normal = (tmp < 0);
453  code = 9;
454  }
455  }
456 
457  // separating axis = u2 x (v1,v2,v3)
458  tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0);
459  s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1));
460  if(s2 > 0) { *return_code = 0; return 0; }
461  n = Vector3<S>(R(2, 0), 0, -R(0, 0));
462  l = n.norm();
463  if(l > eps)
464  {
465  s2 /= l;
466  if(s2 * fudge_factor > s)
467  {
468  s = s2;
469  best_col_id = -1;
470  normalC = n / l;
471  invert_normal = (tmp < 0);
472  code = 10;
473  }
474  }
475 
476  tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1);
477  s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0));
478  if(s2 > 0) { *return_code = 0; return 0; }
479  n = Vector3<S>(R(2, 1), 0, -R(0, 1));
480  l = n.norm();
481  if(l > eps)
482  {
483  s2 /= l;
484  if(s2 * fudge_factor > s)
485  {
486  s = s2;
487  best_col_id = -1;
488  normalC = n / l;
489  invert_normal = (tmp < 0);
490  code = 11;
491  }
492  }
493 
494  tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2);
495  s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0));
496  if(s2 > 0) { *return_code = 0; return 0; }
497  n = Vector3<S>(R(2, 2), 0, -R(0, 2));
498  l = n.norm();
499  if(l > eps)
500  {
501  s2 /= l;
502  if(s2 * fudge_factor > s)
503  {
504  s = s2;
505  best_col_id = -1;
506  normalC = n / l;
507  invert_normal = (tmp < 0);
508  code = 12;
509  }
510  }
511 
512  // separating axis = u3 x (v1,v2,v3)
513  tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0);
514  s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1));
515  if(s2 > 0) { *return_code = 0; return 0; }
516  n = Vector3<S>(-R(1, 0), R(0, 0), 0);
517  l = n.norm();
518  if(l > eps)
519  {
520  s2 /= l;
521  if(s2 * fudge_factor > s)
522  {
523  s = s2;
524  best_col_id = -1;
525  normalC = n / l;
526  invert_normal = (tmp < 0);
527  code = 13;
528  }
529  }
530 
531  tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1);
532  s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0));
533  if(s2 > 0) { *return_code = 0; return 0; }
534  n = Vector3<S>(-R(1, 1), R(0, 1), 0);
535  l = n.norm();
536  if(l > eps)
537  {
538  s2 /= l;
539  if(s2 * fudge_factor > s)
540  {
541  s = s2;
542  best_col_id = -1;
543  normalC = n / l;
544  invert_normal = (tmp < 0);
545  code = 14;
546  }
547  }
548 
549  tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2);
550  s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0));
551  if(s2 > 0) { *return_code = 0; return 0; }
552  n = Vector3<S>(-R(1, 2), R(0, 2), 0);
553  l = n.norm();
554  if(l > eps)
555  {
556  s2 /= l;
557  if(s2 * fudge_factor > s)
558  {
559  s = s2;
560  best_col_id = -1;
561  normalC = n / l;
562  invert_normal = (tmp < 0);
563  code = 15;
564  }
565  }
566 
567  if (!code) { *return_code = code; return 0; }
568 
569  // if we get to this point, the boxes interpenetrate. compute the normal
570  // in global coordinates.
571  if(best_col_id != -1)
572  normal = normalR->col(best_col_id);
573  else
574  normal = R1 * normalC;
575 
576  if(invert_normal)
577  normal = -normal;
578 
579  *depth = -s; // s is negative when the boxes are in collision
580 
581  // compute contact point(s)
582 
583  if(code > 6)
584  {
585  // an edge from box 1 touches an edge from box 2.
586  // find a point pa on the intersecting edge of box 1
587  Vector3<S> pa(T1);
588  S sign;
589 
590  for(int j = 0; j < 3; ++j)
591  {
592  sign = (R1.col(j).dot(normal) > 0) ? 1 : -1;
593  pa += R1.col(j) * (A[j] * sign);
594  }
595 
596  // find a point pb on the intersecting edge of box 2
597  Vector3<S> pb(T2);
598 
599  for(int j = 0; j < 3; ++j)
600  {
601  sign = (R2.col(j).dot(normal) > 0) ? -1 : 1;
602  pb += R2.col(j) * (B[j] * sign);
603  }
604 
605  S alpha, beta;
606  Vector3<S> ua(R1.col((code-7)/3));
607  Vector3<S> ub(R2.col((code-7)%3));
608 
609  lineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
610  pa += ua * alpha;
611  pb += ub * beta;
612 
613  Vector3<S> pointInWorld((pa + pb) * 0.5);
614  contacts.emplace_back(normal, pointInWorld, *depth);
615  *return_code = code;
616 
617  return 1;
618  }
619 
620  // okay, we have a face-something intersection (because the separating
621  // axis is perpendicular to a face). define face 'a' to be the reference
622  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
623  // the incident face (the closest face of the other box).
624 
625  const Eigen::MatrixBase<DerivedA> *Ra, *Rb;
626  const Eigen::MatrixBase<DerivedB> *pa, *pb;
627  const Vector3<S> *Sa, *Sb;
628 
629  if(code <= 3)
630  {
631  Ra = &R1;
632  Rb = &R2;
633  pa = &T1;
634  pb = &T2;
635  Sa = &A;
636  Sb = &B;
637  }
638  else
639  {
640  Ra = &R2;
641  Rb = &R1;
642  pa = &T2;
643  pb = &T1;
644  Sa = &B;
645  Sb = &A;
646  }
647 
648  // nr = normal vector of reference face dotted with axes of incident box.
649  // anr = absolute values of nr.
650  Vector3<S> normal2, nr, anr;
651  if(code <= 3)
652  normal2 = normal;
653  else
654  normal2 = -normal;
655 
656  nr = Rb->transpose() * normal2;
657  anr = nr.cwiseAbs();
658 
659  // find the largest compontent of anr: this corresponds to the normal
660  // for the indident face. the other axis numbers of the indicent face
661  // are stored in a1,a2.
662  int lanr, a1, a2;
663  if(anr[1] > anr[0])
664  {
665  if(anr[1] > anr[2])
666  {
667  a1 = 0;
668  lanr = 1;
669  a2 = 2;
670  }
671  else
672  {
673  a1 = 0;
674  a2 = 1;
675  lanr = 2;
676  }
677  }
678  else
679  {
680  if(anr[0] > anr[2])
681  {
682  lanr = 0;
683  a1 = 1;
684  a2 = 2;
685  }
686  else
687  {
688  a1 = 0;
689  a2 = 1;
690  lanr = 2;
691  }
692  }
693 
694  // compute center point of incident face, in reference-face coordinates
695  Vector3<S> center;
696  if(nr[lanr] < 0)
697  center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]);
698  else
699  center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]);
700 
701  // find the normal and non-normal axis numbers of the reference box
702  int codeN, code1, code2;
703  if(code <= 3)
704  codeN = code-1;
705  else codeN = code-4;
706 
707  if(codeN == 0)
708  {
709  code1 = 1;
710  code2 = 2;
711  }
712  else if(codeN == 1)
713  {
714  code1 = 0;
715  code2 = 2;
716  }
717  else
718  {
719  code1 = 0;
720  code2 = 1;
721  }
722 
723  // find the four corners of the incident face, in reference-face coordinates
724  S quad[8]; // 2D coordinate of incident face (x,y pairs)
725  S c1, c2, m11, m12, m21, m22;
726  c1 = Ra->col(code1).dot(center);
727  c2 = Ra->col(code2).dot(center);
728  // optimize this? - we have already computed this data above, but it is not
729  // stored in an easy-to-index format. for now it's quicker just to recompute
730  // the four dot products.
731  Vector3<S> tempRac = Ra->col(code1);
732  m11 = Rb->col(a1).dot(tempRac);
733  m12 = Rb->col(a2).dot(tempRac);
734  tempRac = Ra->col(code2);
735  m21 = Rb->col(a1).dot(tempRac);
736  m22 = Rb->col(a2).dot(tempRac);
737 
738  S k1 = m11 * (*Sb)[a1];
739  S k2 = m21 * (*Sb)[a1];
740  S k3 = m12 * (*Sb)[a2];
741  S k4 = m22 * (*Sb)[a2];
742  quad[0] = c1 - k1 - k3;
743  quad[1] = c2 - k2 - k4;
744  quad[2] = c1 - k1 + k3;
745  quad[3] = c2 - k2 + k4;
746  quad[4] = c1 + k1 + k3;
747  quad[5] = c2 + k2 + k4;
748  quad[6] = c1 + k1 - k3;
749  quad[7] = c2 + k2 - k4;
750 
751  // find the size of the reference face
752  S rect[2];
753  rect[0] = (*Sa)[code1];
754  rect[1] = (*Sa)[code2];
755 
756  // intersect the incident and reference faces
757  S ret[16];
758  int n_intersect = intersectRectQuad2(rect, quad, ret);
759  if(n_intersect < 1) { *return_code = code; return 0; } // this should never happen
760 
761  // convert the intersection points into reference-face coordinates,
762  // and compute the contact position and depth for each point. only keep
763  // those points that have a positive (penetrating) depth. delete points in
764  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
765  Vector3<S> points[8]; // penetrating contact points
766  S dep[8]; // depths for those points
767  S det1 = 1.f/(m11*m22 - m12*m21);
768  m11 *= det1;
769  m12 *= det1;
770  m21 *= det1;
771  m22 *= det1;
772  int cnum = 0; // number of penetrating contact points found
773  for(int j = 0; j < n_intersect; ++j)
774  {
775  S k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
776  S k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
777  points[cnum] = center + Rb->col(a1) * k1 + Rb->col(a2) * k2;
778  dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]);
779  if(dep[cnum] >= 0)
780  {
781  ret[cnum*2] = ret[j*2];
782  ret[cnum*2+1] = ret[j*2+1];
783  cnum++;
784  }
785  }
786  if(cnum < 1) { *return_code = code; return 0; } // this should never happen
787 
788  // we can't generate more contacts than we actually have
789  if(maxc > cnum) maxc = cnum;
790  if(maxc < 1) maxc = 1;
791 
792  // The determination of these contact computations are tested in:
793  // test_fcl_box_box.cpp.
794  // The case where cnum <= maxc is tested by the test:
795  // test_collision_box_box_all_contacts
796  // The case where cnum > maxc is tested by the test:
797  // test_collision_box_box_cull_contacts
798  //
799  // Each of those tests is exercised twice: once when code < 4 and once when
800  // 4 <= code < 7.
801  int iret[] = {0, 1, 2, 3, 4, 5, 6, 7};
802  if (cnum > maxc) {
803  int i1 = 0;
804  S maxdepth = dep[0];
805  for(int i = 1; i < cnum; ++i)
806  {
807  if(dep[i] > maxdepth)
808  {
809  maxdepth = dep[i];
810  i1 = i;
811  }
812  }
813 
814  cullPoints2(cnum, ret, maxc, i1, iret);
815  cnum = maxc;
816  }
817 
818  if (code < 4) {
819  for(int j = 0; j < cnum; ++j)
820  {
821  int i = iret[j];
822  Vector3<S> pointInWorld = points[i] + (*pa) + normal * (dep[i] / 2);
823  contacts.emplace_back(normal, pointInWorld, dep[i]);
824  }
825  } else {
826  for(int j = 0; j < cnum; ++j)
827  {
828  int i = iret[j];
829  Vector3<S> pointInWorld = points[i] + (*pa) - normal * (dep[i] / 2);
830  contacts.emplace_back(normal, pointInWorld, dep[i]);
831  }
832  }
833 
834  *return_code = code;
835  return cnum;
836 }
837 
838 //==============================================================================
839 template <typename S>
841  const Vector3<S>& side1,
842  const Transform3<S>& tf1,
843  const Vector3<S>& side2,
844  const Transform3<S>& tf2,
845  Vector3<S>& normal,
846  S* depth,
847  int* return_code,
848  int maxc,
849  std::vector<ContactPoint<S>>& contacts)
850 {
851  return boxBox2(side1, tf1.linear(), tf1.translation(), side2, tf2.linear(),
852  tf2.translation(), normal, depth, return_code, maxc, contacts);
853 }
854 
855 //==============================================================================
856 template <typename S>
857 bool boxBoxIntersect(const Box<S>& s1, const Transform3<S>& tf1,
858  const Box<S>& s2, const Transform3<S>& tf2,
859  std::vector<ContactPoint<S>>* contacts_)
860 {
861  std::vector<ContactPoint<S>> contacts;
862  int return_code;
863  Vector3<S> normal;
864  S depth;
865  /* int cnum = */ boxBox2(s1.side, tf1,
866  s2.side, tf2,
867  normal, &depth, &return_code,
868  4, contacts);
869 
870  if(contacts_)
871  *contacts_ = contacts;
872 
873  return return_code != 0;
874 }
875 
876 } // namespace detail
877 } // namespace fcl
878 
879 #endif
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::ContactPoint
Minimal contact information returned by collision.
Definition: contact_point.h:48
epsilon
S epsilon()
Definition: test_fcl_simple.cpp:56
fcl::detail::boxBoxIntersect
template bool boxBoxIntersect(const Box< double > &s1, const Transform3< double > &tf1, const Box< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts_)
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::detail::intersectRectQuad2
template int intersectRectQuad2(double h[2], double p[8], double ret[16])
fcl::detail::boxBox2
template int boxBox2(const Vector3< double > &side1, const Transform3< double > &tf1, const Vector3< double > &side2, const Transform3< double > &tf2, Vector3< double > &normal, double *depth, int *return_code, int maxc, std::vector< ContactPoint< double >> &contacts)
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::detail::cullPoints2
template void cullPoints2(int n, double p[], int m, int i0, int iret[])
r
S r
Definition: test_sphere_box.cpp:171
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
fcl::detail::lineClosestApproach
template void lineClosestApproach(const Vector3< double > &pa, const Vector3< double > &ua, const Vector3< double > &pb, const Vector3< double > &ub, double *alpha, double *beta)
fcl::constants::pi
static constexpr S pi()
The mathematical constant pi.
Definition: constants.h:134
box_box.h
fcl::Box< double >
template class FCL_EXPORT Box< double >
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::Box::side
Vector3< S > side
box side length
Definition: box.h:67


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:48