intersect-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_INTERSECT_INL_H
39 #define FCL_NARROWPHASE_DETAIL_INTERSECT_INL_H
40 
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 extern template
51 class FCL_EXPORT Intersect<double>;
52 
53 //==============================================================================
54 template <typename S>
56 {
57  return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold());
58 }
59 
60 //==============================================================================
62 template <typename S>
64  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vd,
65  S& l, S& r, bool bVF, S coeffs[], Vector3<S>* data)
66 {
67  S v2[2]= {l*l,r*r};
68  S v[2]= {l,r};
69  S r_backup;
70 
71  unsigned char min3, min2, min1, max3, max2, max1;
72 
73  min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1;
74  min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1;
75  min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1;
76 
77  // bound the cubic
78 
79  S minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0];
80  S major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0];
81 
82  if(major<0) return false;
83  if(minor>0) return false;
84 
85  // starting here, the bounds have opposite values
86  S m = 0.5 * (r + l);
87 
88  // bound the derivative
89  S dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1];
90  S dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1];
91 
92  if((dminor > 0)||(dmajor < 0)) // we can use Newton
93  {
94  S m2 = m*m;
95  S fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0];
96  S nl = m;
97  S nu = m;
98  if(fm>0)
99  {
100  nl-=(fm/dminor);
101  nu-=(fm/dmajor);
102  }
103  else
104  {
105  nu-=(fm/dminor);
106  nl-=(fm/dmajor);
107  }
108 
109  //intersect with [l,r]
110 
111  if(nl>r) return false;
112  if(nu<l) return false;
113  if(nl>l)
114  {
115  if(nu<r) { l=nl; r=nu; m=0.5*(l+r); }
116  else { l=nl; m=0.5*(l+r); }
117  }
118  else
119  {
120  if(nu<r) { r=nu; m=0.5*(l+r); }
121  }
122  }
123 
124  // sufficient temporal resolution, check root validity
125  if((r-l)< getCcdResolution())
126  {
127  if(bVF)
128  return checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r);
129  else
130  return checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, data);
131  }
132 
133  r_backup = r, r = m;
134  if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, bVF, coeffs, data))
135  return true;
136 
137  l = m, r = r_backup;
138  return solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, bVF, coeffs, data);
139 }
140 
141 //==============================================================================
142 template <typename S>
143 bool Intersect<S>::insideTriangle(const Vector3<S>& a, const Vector3<S>& b, const Vector3<S>& c, const Vector3<S>&p)
144 {
145  Vector3<S> ab = b - a;
146  Vector3<S> ac = c - a;
147  Vector3<S> n = ab.cross(ac);
148 
149  Vector3<S> pa = a - p;
150  Vector3<S> pb = b - p;
151  Vector3<S> pc = c - p;
152 
153  if((pb.cross(pc)).dot(n) < -getEpsilon()) return false;
154  if((pc.cross(pa)).dot(n) < -getEpsilon()) return false;
155  if((pa.cross(pb)).dot(n) < -getEpsilon()) return false;
156 
157  return true;
158 }
159 
160 //==============================================================================
161 template <typename S>
163 {
164  return (p - a).dot(p - b) <= 0;
165 }
166 
167 //==============================================================================
173 template <typename S>
174 bool Intersect<S>::linelineIntersect(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3, const Vector3<S>& p4,
175  Vector3<S>* pa, Vector3<S>* pb, S* mua, S* mub)
176 {
177  Vector3<S> p31 = p1 - p3;
178  Vector3<S> p34 = p4 - p3;
179  if(fabs(p34[0]) < getEpsilon() && fabs(p34[1]) < getEpsilon() && fabs(p34[2]) < getEpsilon())
180  return false;
181 
182  Vector3<S> p12 = p2 - p1;
183  if(fabs(p12[0]) < getEpsilon() && fabs(p12[1]) < getEpsilon() && fabs(p12[2]) < getEpsilon())
184  return false;
185 
186  S d3134 = p31.dot(p34);
187  S d3412 = p34.dot(p12);
188  S d3112 = p31.dot(p12);
189  S d3434 = p34.dot(p34);
190  S d1212 = p12.dot(p12);
191 
192  S denom = d1212 * d3434 - d3412 * d3412;
193  if(fabs(denom) < getEpsilon())
194  return false;
195  S numer = d3134 * d3412 - d3112 * d3434;
196 
197  *mua = numer / denom;
198  if(*mua < 0 || *mua > 1)
199  return false;
200 
201  *mub = (d3134 + d3412 * (*mua)) / d3434;
202  if(*mub < 0 || *mub > 1)
203  return false;
204 
205  *pa = p1 + p12 * (*mua);
206  *pb = p3 + p34 * (*mub);
207  return true;
208 }
209 
210 //==============================================================================
211 template <typename S>
212 bool Intersect<S>::checkRootValidity_VF(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& p0,
213  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vp,
214  S t)
215 {
216  return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t);
217 }
218 
219 //==============================================================================
220 template <typename S>
221 bool Intersect<S>::checkRootValidity_EE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
222  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vd,
223  S t, Vector3<S>* q_i)
224 {
225  Vector3<S> a = a0 + va * t;
226  Vector3<S> b = b0 + vb * t;
227  Vector3<S> c = c0 + vc * t;
228  Vector3<S> d = d0 + vd * t;
229  Vector3<S> p1, p2;
230  S t_ab, t_cd;
231  if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd))
232  {
233  if(q_i) *q_i = p1;
234  return true;
235  }
236 
237  return false;
238 }
239 
240 //==============================================================================
241 template <typename S>
243  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vp,
244  S t)
245 {
246  return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t);
247 }
248 
249 //==============================================================================
250 template <typename S>
251 bool Intersect<S>::solveSquare(S a, S b, S c,
252  const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
253  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vd,
254  bool bVF,
255  S* ret)
256 {
257  S discriminant = b * b - 4 * a * c;
258  if(discriminant < 0)
259  return false;
260 
261  S sqrt_dis = sqrt(discriminant);
262  S r1 = (-b + sqrt_dis) / (2 * a);
263  bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false;
264 
265  S r2 = (-b - sqrt_dis) / (2 * a);
266  bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false;
267 
268  if(v1 && v2)
269  {
270  *ret = (r1 > r2) ? r2 : r1;
271  return true;
272  }
273  if(v1)
274  {
275  *ret = r1;
276  return true;
277  }
278  if(v2)
279  {
280  *ret = r2;
281  return true;
282  }
283 
284  return false;
285 }
286 
287 //==============================================================================
288 template <typename S>
289 bool Intersect<S>::solveSquare(S a, S b, S c,
290  const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& p0,
291  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vp)
292 {
293  if(isZero(a))
294  {
295  S t = -c/b;
296  return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false;
297  }
298 
299  S discriminant = b*b-4*a*c;
300  if(discriminant < 0)
301  return false;
302 
303  S sqrt_dis = sqrt(discriminant);
304 
305  S r1 = (-b+sqrt_dis) / (2 * a);
306  bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false;
307  if(v1) return true;
308 
309  S r2 = (-b-sqrt_dis) / (2 * a);
310  bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false;
311  return v2;
312 }
313 
314 //==============================================================================
317 template <typename S>
318 void Intersect<S>::computeCubicCoeff_VF(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& p0,
319  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vp,
320  S* a, S* b, S* c, S* d)
321 {
322  Vector3<S> vavb = vb - va;
323  Vector3<S> vavc = vc - va;
324  Vector3<S> vavp = vp - va;
325  Vector3<S> a0b0 = b0 - a0;
326  Vector3<S> a0c0 = c0 - a0;
327  Vector3<S> a0p0 = p0 - a0;
328 
329  Vector3<S> vavb_cross_vavc = vavb.cross(vavc);
330  Vector3<S> vavb_cross_a0c0 = vavb.cross(a0c0);
331  Vector3<S> a0b0_cross_vavc = a0b0.cross(vavc);
332  Vector3<S> a0b0_cross_a0c0 = a0b0.cross(a0c0);
333 
334  *a = vavp.dot(vavb_cross_vavc);
335  *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc);
336  *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc);
337  *d = a0p0.dot(a0b0_cross_a0c0);
338 }
339 
340 //==============================================================================
341 template <typename S>
342 void Intersect<S>::computeCubicCoeff_EE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
343  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vd,
344  S* a, S* b, S* c, S* d)
345 {
346  Vector3<S> vavb = vb - va;
347  Vector3<S> vcvd = vd - vc;
348  Vector3<S> vavc = vc - va;
349  Vector3<S> c0d0 = d0 - c0;
350  Vector3<S> a0b0 = b0 - a0;
351  Vector3<S> a0c0 = c0 - a0;
352  Vector3<S> vavb_cross_vcvd = vavb.cross(vcvd);
353  Vector3<S> vavb_cross_c0d0 = vavb.cross(c0d0);
354  Vector3<S> a0b0_cross_vcvd = a0b0.cross(vcvd);
355  Vector3<S> a0b0_cross_c0d0 = a0b0.cross(c0d0);
356 
357  *a = vavc.dot(vavb_cross_vcvd);
358  *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd);
359  *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd);
360  *d = a0c0.dot(a0b0_cross_c0d0);
361 }
362 
363 //==============================================================================
364 template <typename S>
366  const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vp,
367  const Vector3<S>& L,
368  S* a, S* b, S* c)
369 {
370  Vector3<S> vbva = va - vb;
371  Vector3<S> vbvp = vp - vb;
372  Vector3<S> b0a0 = a0 - b0;
373  Vector3<S> b0p0 = p0 - b0;
374 
375  Vector3<S> L_cross_vbvp = L.cross(vbvp);
376  Vector3<S> L_cross_b0p0 = L.cross(b0p0);
377 
378  *a = L_cross_vbvp.dot(vbva);
379  *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva);
380  *c = L_cross_b0p0.dot(b0a0);
381 }
382 
383 //==============================================================================
384 template <typename S>
385 bool Intersect<S>::intersect_VF(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& p0,
386  const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& p1,
387  S* collision_time, Vector3<S>* p_i, bool useNewton)
388 {
389  *collision_time = 2.0;
390 
391  Vector3<S> vp, va, vb, vc;
392  vp = p1 - p0;
393  va = a1 - a0;
394  vb = b1 - b0;
395  vc = c1 - c0;
396 
397  S a, b, c, d;
398  computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d);
399 
400  if(isZero(a) && isZero(b) && isZero(c) && isZero(d))
401  {
402  return false;
403  }
404 
405 
410 
411  S coeffs[4];
412  coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
413 
414  if(useNewton)
415  {
416  S l = 0;
417  S r = 1;
418 
419  if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs))
420  {
421  *collision_time = 0.5 * (l + r);
422  }
423  }
424  else
425  {
426  S roots[3];
427  int num = PolySolver<S>::solveCubic(coeffs, roots);
428  for(int i = 0; i < num; ++i)
429  {
430  S r = roots[i];
431  if(r < 0 || r > 1) continue;
432  if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r))
433  {
434  *collision_time = r;
435  break;
436  }
437  }
438  }
439 
440  if(*collision_time > 1)
441  {
442  return false;
443  }
444 
445  *p_i = vp * (*collision_time) + p0;
446  return true;
447 }
448 
449 //==============================================================================
450 template <typename S>
451 bool Intersect<S>::intersect_EE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
452  const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& d1,
453  S* collision_time, Vector3<S>* p_i, bool useNewton)
454 {
455  *collision_time = 2.0;
456 
457  Vector3<S> va, vb, vc, vd;
458  va = a1 - a0;
459  vb = b1 - b0;
460  vc = c1 - c0;
461  vd = d1 - d0;
462 
463  S a, b, c, d;
464  computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d);
465 
466  if(isZero(a) && isZero(b) && isZero(c) && isZero(d))
467  {
468  return false;
469  }
470 
475 
476 
477  S coeffs[4];
478  coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
479 
480  if(useNewton)
481  {
482  S l = 0;
483  S r = 1;
484 
485  if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i))
486  {
487  *collision_time = (l + r) * 0.5;
488  }
489  }
490  else
491  {
492  S roots[3];
493  int num = PolySolver<S>::solveCubic(coeffs, roots);
494  for(int i = 0; i < num; ++i)
495  {
496  S r = roots[i];
497  if(r < 0 || r > 1) continue;
498 
499  if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i))
500  {
501  *collision_time = r;
502  break;
503  }
504  }
505  }
506 
507  if(*collision_time > 1)
508  {
509  return false;
510  }
511 
512  return true;
513 }
514 
515 //==============================================================================
516 template <typename S>
517 bool Intersect<S>::intersect_VE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& p0,
518  const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& p1,
519  const Vector3<S>& L)
520 {
521  Vector3<S> va, vb, vp;
522  va = a1 - a0;
523  vb = b1 - b0;
524  vp = p1 - p0;
525 
526  S a, b, c;
527  computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c);
528 
529  if(isZero(a) && isZero(b) && isZero(c))
530  return true;
531 
532  return solveSquare(a, b, c, a0, b0, p0, va, vb, vp);
533 
534 }
535 
536 //==============================================================================
538 template <typename S>
539 bool Intersect<S>::intersectPreFiltering(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
540  const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& d1)
541 {
542  Vector3<S> n0 = (b0 - a0).cross(c0 - a0);
543  Vector3<S> n1 = (b1 - a1).cross(c1 - a1);
544  Vector3<S> a0a1 = a1 - a0;
545  Vector3<S> b0b1 = b1 - b0;
546  Vector3<S> c0c1 = c1 - c0;
547  Vector3<S> delta = (b0b1 - a0a1).cross(c0c1 - a0a1);
548  Vector3<S> nx = (n0 + n1 - delta) * 0.5;
549 
550  Vector3<S> a0d0 = d0 - a0;
551  Vector3<S> a1d1 = d1 - a1;
552 
553  S A = n0.dot(a0d0);
554  S B = n1.dot(a1d1);
555  S C = nx.dot(a0d0);
556  S D = nx.dot(a1d1);
557  S E = n1.dot(a0d0);
558  S F = n0.dot(a1d1);
559 
560  if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0)
561  return false;
562  if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0)
563  return false;
564 
565  return true;
566 }
567 
568 //==============================================================================
569 template <typename S>
570 bool Intersect<S>::intersect_VF_filtered(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& p0,
571  const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& p1,
572  S* collision_time, Vector3<S>* p_i, bool useNewton)
573 {
574  if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1))
575  {
576  return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton);
577  }
578  else
579  return false;
580 }
581 
582 //==============================================================================
583 template <typename S>
584 bool Intersect<S>::intersect_EE_filtered(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
585  const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& d1,
586  S* collision_time, Vector3<S>* p_i, bool useNewton)
587 {
588  if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1))
589  {
590  return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton);
591  }
592  else
593  return false;
594 }
595 
596 //==============================================================================
597 template <typename S>
599  const Vector3<S>& P1,
600  const Vector3<S>& P2,
601  const Vector3<S>& P3,
602  const Vector3<S>& Q1,
603  const Vector3<S>& Q2,
604  const Vector3<S>& Q3,
605  const Matrix3<S>& R,
606  const Vector3<S>& T,
607  Vector3<S>* contact_points,
608  unsigned int* num_contact_points,
609  S* penetration_depth,
610  Vector3<S>* normal)
611 {
612  Vector3<S> Q1_ = R * Q1 + T;
613  Vector3<S> Q2_ = R * Q2 + T;
614  Vector3<S> Q3_ = R * Q3 + T;
615 
616  return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal);
617 }
618 
619 //==============================================================================
620 template <typename S>
622  const Vector3<S>& P1,
623  const Vector3<S>& P2,
624  const Vector3<S>& P3,
625  const Vector3<S>& Q1,
626  const Vector3<S>& Q2,
627  const Vector3<S>& Q3,
628  const Transform3<S>& tf,
629  Vector3<S>* contact_points,
630  unsigned int* num_contact_points,
631  S* penetration_depth,
632  Vector3<S>* normal)
633 {
634  Vector3<S> Q1_ = tf * Q1;
635  Vector3<S> Q2_ = tf * Q2;
636  Vector3<S> Q3_ = tf * Q3;
637 
638  return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal);
639 }
640 
641 //==============================================================================
642 template <typename S>
644  const Vector3<S>& P1, const Vector3<S>& P2, const Vector3<S>& P3,
645  const Vector3<S>& Q1, const Vector3<S>& Q2, const Vector3<S>& Q3,
646  Vector3<S>* contact_points,
647  unsigned int* num_contact_points,
648  S* penetration_depth,
649  Vector3<S>* normal)
650 {
651  Vector3<S> n1;
652  S t1;
653  bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1);
654  if(!b1) return false;
655 
656  Vector3<S> n2;
657  S t2;
658  bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2);
659  if(!b2) return false;
660 
661  if(sameSideOfPlane(P1, P2, P3, n2, t2))
662  return false;
663 
664  if(sameSideOfPlane(Q1, Q2, Q3, n1, t1))
665  return false;
666 
667  Vector3<S> clipped_points1[getMaxTriangleClips()];
668  unsigned int num_clipped_points1 = 0;
669  Vector3<S> clipped_points2[getMaxTriangleClips()];
670  unsigned int num_clipped_points2 = 0;
671 
672  Vector3<S> deepest_points1[getMaxTriangleClips()];
673  unsigned int num_deepest_points1 = 0;
674  Vector3<S> deepest_points2[getMaxTriangleClips()];
675  unsigned int num_deepest_points2 = 0;
676  S penetration_depth1 = -1, penetration_depth2 = -1;
677 
678  clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2);
679 
680  if(num_clipped_points2 == 0)
681  return false;
682 
683  computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2);
684  if(num_deepest_points2 == 0)
685  return false;
686 
687  clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1);
688  if(num_clipped_points1 == 0)
689  return false;
690 
691  computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1);
692  if(num_deepest_points1 == 0)
693  return false;
694 
695 
697  if(contact_points && num_contact_points && penetration_depth && normal)
698  {
699  if(penetration_depth1 > penetration_depth2)
700  {
701  *num_contact_points = num_deepest_points2;
702  for(unsigned int i = 0; i < num_deepest_points2; ++i)
703  {
704  contact_points[i] = deepest_points2[i];
705  }
706 
707  *normal = n1;
708  *penetration_depth = penetration_depth2;
709  }
710  else
711  {
712  *num_contact_points = num_deepest_points1;
713  for(unsigned int i = 0; i < num_deepest_points1; ++i)
714  {
715  contact_points[i] = deepest_points1[i];
716  }
717 
718  *normal = -n2;
719  *penetration_depth = penetration_depth1;
720  }
721  }
722 
723  return true;
724 }
725 
726 //==============================================================================
727 template <typename S>
729  const Vector3<S>& P1, const Vector3<S>& P2, const Vector3<S>& P3,
730  const Vector3<S>& Q1, const Vector3<S>& Q2, const Vector3<S>& Q3,
731  Vector3<S>* contact_points,
732  unsigned int* num_contact_points,
733  S* penetration_depth,
734  Vector3<S>* normal)
735 {
736  Vector3<S> p1 = P1 - P1;
737  Vector3<S> p2 = P2 - P1;
738  Vector3<S> p3 = P3 - P1;
739  Vector3<S> q1 = Q1 - P1;
740  Vector3<S> q2 = Q2 - P1;
741  Vector3<S> q3 = Q3 - P1;
742 
743  Vector3<S> e1 = p2 - p1;
744  Vector3<S> e2 = p3 - p2;
745  Vector3<S> n1 = e1.cross(e2);
746  if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false;
747 
748  Vector3<S> f1 = q2 - q1;
749  Vector3<S> f2 = q3 - q2;
750  Vector3<S> m1 = f1.cross(f2);
751  if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false;
752 
753  Vector3<S> ef11 = e1.cross(f1);
754  if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false;
755 
756  Vector3<S> ef12 = e1.cross(f2);
757  if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false;
758 
759  Vector3<S> f3 = q1 - q3;
760  Vector3<S> ef13 = e1.cross(f3);
761  if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false;
762 
763  Vector3<S> ef21 = e2.cross(f1);
764  if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false;
765 
766  Vector3<S> ef22 = e2.cross(f2);
767  if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false;
768 
769  Vector3<S> ef23 = e2.cross(f3);
770  if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false;
771 
772  Vector3<S> e3 = p1 - p3;
773  Vector3<S> ef31 = e3.cross(f1);
774  if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false;
775 
776  Vector3<S> ef32 = e3.cross(f2);
777  if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false;
778 
779  Vector3<S> ef33 = e3.cross(f3);
780  if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false;
781 
782  Vector3<S> g1 = e1.cross(n1);
783  if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false;
784 
785  Vector3<S> g2 = e2.cross(n1);
786  if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false;
787 
788  Vector3<S> g3 = e3.cross(n1);
789  if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false;
790 
791  Vector3<S> h1 = f1.cross(m1);
792  if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false;
793 
794  Vector3<S> h2 = f2.cross(m1);
795  if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false;
796 
797  Vector3<S> h3 = f3.cross(m1);
798  if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false;
799 
800  if(contact_points && num_contact_points && penetration_depth && normal)
801  {
802  Vector3<S> n1, n2;
803  S t1, t2;
804  buildTrianglePlane(P1, P2, P3, &n1, &t1);
805  buildTrianglePlane(Q1, Q2, Q3, &n2, &t2);
806 
807  Vector3<S> deepest_points1[3];
808  unsigned int num_deepest_points1 = 0;
809  Vector3<S> deepest_points2[3];
810  unsigned int num_deepest_points2 = 0;
811  S penetration_depth1, penetration_depth2;
812 
813  Vector3<S> P[3] = {P1, P2, P3};
814  Vector3<S> Q[3] = {Q1, Q2, Q3};
815 
816  computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2);
817  computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1);
818 
819 
820  if(penetration_depth1 > penetration_depth2)
821  {
822  *num_contact_points = std::min(num_deepest_points2, (unsigned int)2);
823  for(unsigned int i = 0; i < *num_contact_points; ++i)
824  {
825  contact_points[i] = deepest_points2[i];
826  }
827 
828  *normal = n1;
829  *penetration_depth = penetration_depth2;
830  }
831  else
832  {
833  *num_contact_points = std::min(num_deepest_points1, (unsigned int)2);
834  for(unsigned int i = 0; i < *num_contact_points; ++i)
835  {
836  contact_points[i] = deepest_points1[i];
837  }
838 
839  *normal = -n2;
840  *penetration_depth = penetration_depth1;
841  }
842  }
843 
844  return true;
845 }
846 
847 //==============================================================================
848 template <typename S>
849 void Intersect<S>::computeDeepestPoints(Vector3<S>* clipped_points, unsigned int num_clipped_points, const Vector3<S>& n, S t, S* penetration_depth, Vector3<S>* deepest_points, unsigned int* num_deepest_points)
850 {
851  *num_deepest_points = 0;
852  S max_depth = -std::numeric_limits<S>::max();
853  unsigned int num_deepest_points_ = 0;
854  unsigned int num_neg = 0;
855  unsigned int num_pos = 0;
856  unsigned int num_zero = 0;
857 
858  for(unsigned int i = 0; i < num_clipped_points; ++i)
859  {
860  S dist = -distanceToPlane(n, t, clipped_points[i]);
861  if(dist > getEpsilon()) num_pos++;
862  else if(dist < -getEpsilon()) num_neg++;
863  else num_zero++;
864  if(dist > max_depth)
865  {
866  max_depth = dist;
867  num_deepest_points_ = 1;
868  deepest_points[num_deepest_points_ - 1] = clipped_points[i];
869  }
870  else if(dist + 1e-6 >= max_depth)
871  {
872  num_deepest_points_++;
873  deepest_points[num_deepest_points_ - 1] = clipped_points[i];
874  }
875  }
876 
877  if(max_depth < -getEpsilon())
878  num_deepest_points_ = 0;
879 
880  if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0)))
881  num_deepest_points_ = 0;
882 
883  *penetration_depth = max_depth;
884  *num_deepest_points = num_deepest_points_;
885 }
886 
887 //==============================================================================
888 template <typename S>
890  const Vector3<S>& t1, const Vector3<S>& t2, const Vector3<S>& t3,
891  const Vector3<S>& tn, S to,
892  Vector3<S> clipped_points[], unsigned int* num_clipped_points,
893  bool clip_triangle)
894 {
895  *num_clipped_points = 0;
896  Vector3<S> temp_clip[getMaxTriangleClips()];
897  Vector3<S> temp_clip2[getMaxTriangleClips()];
898  unsigned int num_temp_clip = 0;
899  unsigned int num_temp_clip2 = 0;
900  Vector3<S> v[3] = {v1, v2, v3};
901 
902  Vector3<S> plane_n;
903  S plane_dist;
904 
905  if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist))
906  {
907  clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip);
908  if(num_temp_clip > 0)
909  {
910  if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist))
911  {
912  clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2);
913  if(num_temp_clip2 > 0)
914  {
915  if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist))
916  {
917  if(clip_triangle)
918  {
919  num_temp_clip = 0;
920  clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip);
921  if(num_temp_clip > 0)
922  {
923  clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points);
924  }
925  }
926  else
927  {
928  clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points);
929  }
930  }
931  }
932  }
933  }
934  }
935 }
936 
937 //==============================================================================
938 template <typename S>
939 void Intersect<S>::clipPolygonByPlane(Vector3<S>* polygon_points, unsigned int num_polygon_points, const Vector3<S>& n, S t, Vector3<S> clipped_points[], unsigned int* num_clipped_points)
940 {
941  *num_clipped_points = 0;
942 
943  unsigned int num_clipped_points_ = 0;
944  unsigned int vi;
945  unsigned int prev_classify = 2;
946  unsigned int classify;
947  for(unsigned int i = 0; i <= num_polygon_points; ++i)
948  {
949  vi = (i % num_polygon_points);
950  S d = distanceToPlane(n, t, polygon_points[i]);
951  classify = ((d > getEpsilon()) ? 1 : 0);
952  if(classify == 0)
953  {
954  if(prev_classify == 1)
955  {
956  if(num_clipped_points_ < getMaxTriangleClips())
957  {
958  Vector3<S> tmp;
959  clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp);
960  if(num_clipped_points_ > 0)
961  {
962  if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon())
963  {
964  clipped_points[num_clipped_points_] = tmp;
965  num_clipped_points_++;
966  }
967  }
968  else
969  {
970  clipped_points[num_clipped_points_] = tmp;
971  num_clipped_points_++;
972  }
973  }
974  }
975 
976  if(num_clipped_points_ < getMaxTriangleClips() && i < num_polygon_points)
977  {
978  clipped_points[num_clipped_points_] = polygon_points[vi];
979  num_clipped_points_++;
980  }
981  }
982  else
983  {
984  if(prev_classify == 0)
985  {
986  if(num_clipped_points_ < getMaxTriangleClips())
987  {
988  Vector3<S> tmp;
989  clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp);
990  if(num_clipped_points_ > 0)
991  {
992  if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon())
993  {
994  clipped_points[num_clipped_points_] = tmp;
995  num_clipped_points_++;
996  }
997  }
998  else
999  {
1000  clipped_points[num_clipped_points_] = tmp;
1001  num_clipped_points_++;
1002  }
1003  }
1004  }
1005  }
1006 
1007  prev_classify = classify;
1008  }
1009 
1010  if(num_clipped_points_ > 2)
1011  {
1012  if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).squaredNorm() < getEpsilon())
1013  {
1014  num_clipped_points_--;
1015  }
1016  }
1017 
1018  *num_clipped_points = num_clipped_points_;
1019 }
1020 
1021 //==============================================================================
1022 template <typename S>
1023 void Intersect<S>::clipSegmentByPlane(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& n, S t, Vector3<S>* clipped_point)
1024 {
1025  S dist1 = distanceToPlane(n, t, v1);
1026  Vector3<S> tmp = v2 - v1;
1027  S dist2 = tmp.dot(n);
1028  *clipped_point = tmp * (-dist1 / dist2) + v1;
1029 }
1030 
1031 //==============================================================================
1032 template <typename S>
1034 {
1035  return n.dot(v) - t;
1036 }
1037 
1038 //==============================================================================
1039 template <typename S>
1040 bool Intersect<S>::buildTrianglePlane(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& v3, Vector3<S>* n, S* t)
1041 {
1042  Vector3<S> n_ = (v2 - v1).cross(v3 - v1);
1043  bool can_normalize = false;
1044  normalize(n_, &can_normalize);
1045  if(can_normalize)
1046  {
1047  *n = n_;
1048  *t = n_.dot(v1);
1049  return true;
1050  }
1051 
1052  return false;
1053 }
1054 
1055 //==============================================================================
1056 template <typename S>
1057 bool Intersect<S>::buildEdgePlane(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& tn, Vector3<S>* n, S* t)
1058 {
1059  Vector3<S> n_ = (v2 - v1).cross(tn);
1060  bool can_normalize = false;
1061  normalize(n_, &can_normalize);
1062  if(can_normalize)
1063  {
1064  *n = n_;
1065  *t = n_.dot(v1);
1066  return true;
1067  }
1068 
1069  return false;
1070 }
1071 
1072 //==============================================================================
1073 template <typename S>
1074 bool Intersect<S>::sameSideOfPlane(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& v3, const Vector3<S>& n, S t)
1075 {
1076  S dist1 = distanceToPlane(n, t, v1);
1077  S dist2 = dist1 * distanceToPlane(n, t, v2);
1078  S dist3 = dist1 * distanceToPlane(n, t, v3);
1079  if((dist2 > 0) && (dist3 > 0))
1080  return true;
1081  return false;
1082 }
1083 
1084 //==============================================================================
1085 template <typename S>
1087  const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3,
1088  const Vector3<S>& q1, const Vector3<S>& q2, const Vector3<S>& q3)
1089 {
1090  S P1 = ax.dot(p1);
1091  S P2 = ax.dot(p2);
1092  S P3 = ax.dot(p3);
1093  S Q1 = ax.dot(q1);
1094  S Q2 = ax.dot(q2);
1095  S Q3 = ax.dot(q3);
1096 
1097  S mn1 = std::min(P1, std::min(P2, P3));
1098  S mx2 = std::max(Q1, std::max(Q2, Q3));
1099  if(mn1 > mx2) return 0;
1100 
1101  S mx1 = std::max(P1, std::max(P2, P3));
1102  S mn2 = std::min(Q1, std::min(Q2, Q3));
1103 
1104  if(mn2 > mx1) return 0;
1105  return 1;
1106 }
1107 
1108 //==============================================================================
1109 template <typename S>
1111 {
1112  return 0.5 * std::erfc(-x / sqrt(2.0));
1113 }
1114 
1115 //==============================================================================
1116 template <typename S>
1118 {
1119  return 1e-5;
1120 }
1121 
1122 //==============================================================================
1123 template <typename S>
1125 {
1126  return 1e-7;
1127 }
1128 
1129 //==============================================================================
1130 template <typename S>
1132 {
1133  return 1e-7;
1134 }
1135 
1136 //==============================================================================
1137 template <typename S>
1138 constexpr unsigned int Intersect<S>::getMaxTriangleClips()
1139 {
1140  return 8;
1141 }
1142 
1143 } // namespace detail
1144 } // namespace fcl
1145 
1146 #endif
fcl::detail::Intersect::intersectPreFiltering
static bool intersectPreFiltering(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &d0, const Vector3< S > &a1, const Vector3< S > &b1, const Vector3< S > &c1, const Vector3< S > &d1)
filter for intersection, works for both VF and EE
Definition: intersect-inl.h:539
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::detail::Intersect::computeCubicCoeff_VE
static void computeCubicCoeff_VE(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &p0, const Vector3< S > &va, const Vector3< S > &vb, const Vector3< S > &vp, const Vector3< S > &L, S *a, S *b, S *c)
Compute the cubic coefficients for VE intersection.
Definition: intersect-inl.h:365
fcl::detail::Intersect::solveCubicWithIntervalNewton
static bool solveCubicWithIntervalNewton(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &d0, const Vector3< S > &va, const Vector3< S > &vb, const Vector3< S > &vc, const Vector3< S > &vd, S &l, S &r, bool bVF, S coeffs[], Vector3< S > *data=nullptr)
Solve the cubic function using Newton method, also satisfies the interval restriction.
Definition: intersect-inl.h:63
fcl::detail::Intersect::computeCubicCoeff_EE
static void computeCubicCoeff_EE(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &d0, const Vector3< S > &va, const Vector3< S > &vb, const Vector3< S > &vc, const Vector3< S > &vd, S *a, S *b, S *c, S *d)
Compute the cubic coefficients for EE intersection.
Definition: intersect-inl.h:342
fcl::detail::Intersect::isZero
static bool isZero(S v)
Check whether one value is zero.
Definition: intersect-inl.h:55
fcl::detail::Intersect::getMaxTriangleClips
static constexpr unsigned int getMaxTriangleClips()
Definition: intersect-inl.h:1138
fcl::detail::Intersect::getNearZeroThreshold
static constexpr S getNearZeroThreshold()
Definition: intersect-inl.h:1124
fcl::detail::Intersect::linelineIntersect
static bool linelineIntersect(const Vector3< S > &p1, const Vector3< S > &p2, const Vector3< S > &p3, const Vector3< S > &p4, Vector3< S > *pa, Vector3< S > *pb, S *mua, S *mub)
Calculate the line segment papb that is the shortest route between two lines p1p2 and p3p4....
Definition: intersect-inl.h:174
fcl::detail::Intersect::getEpsilon
static constexpr S getEpsilon()
Definition: intersect-inl.h:1117
fcl::detail::Intersect::buildTrianglePlane
static bool buildTrianglePlane(const Vector3< S > &v1, const Vector3< S > &v2, const Vector3< S > &v3, Vector3< S > *n, S *t)
build a plane passed through triangle v1 v2 v3
Definition: intersect-inl.h:1040
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::detail::Intersect::checkRootValidity_VF
static bool checkRootValidity_VF(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &p0, const Vector3< S > &va, const Vector3< S > &vb, const Vector3< S > &vc, const Vector3< S > &vp, S t)
Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t.
Definition: intersect-inl.h:212
fcl::detail::Intersect::getCcdResolution
static constexpr S getCcdResolution()
Definition: intersect-inl.h:1131
fcl::normalize
template void normalize(Vector3d &v, bool *signal)
fcl::detail::Intersect::clipSegmentByPlane
static void clipSegmentByPlane(const Vector3< S > &v1, const Vector3< S > &v2, const Vector3< S > &n, S t, Vector3< S > *clipped_point)
clip a line segment by plane
Definition: intersect-inl.h:1023
fcl::detail::Intersect< double >
template class FCL_EXPORT Intersect< double >
fcl::detail::Intersect::insideTriangle
static bool insideTriangle(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &c, const Vector3< S > &p)
Check whether one point p is within triangle [a, b, c].
Definition: intersect-inl.h:143
fcl::detail::Intersect::clipPolygonByPlane
static void clipPolygonByPlane(Vector3< S > *polygon_points, unsigned int num_polygon_points, const Vector3< S > &n, S t, Vector3< S > clipped_points[], unsigned int *num_clipped_points)
clip polygon by plane
Definition: intersect-inl.h:939
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::Intersect::intersect_Triangle
static bool intersect_Triangle(const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Vector3< S > &Q1, const Vector3< S > &Q2, const Vector3< S > &Q3, Vector3< S > *contact_points=nullptr, unsigned int *num_contact_points=nullptr, S *penetration_depth=nullptr, Vector3< S > *normal=nullptr)
CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3].
Definition: intersect-inl.h:728
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::detail::Intersect::clipTriangleByTriangleAndEdgePlanes
static void clipTriangleByTriangleAndEdgePlanes(const Vector3< S > &v1, const Vector3< S > &v2, const Vector3< S > &v3, const Vector3< S > &t1, const Vector3< S > &t2, const Vector3< S > &t3, const Vector3< S > &tn, S to, Vector3< S > clipped_points[], unsigned int *num_clipped_points, bool clip_triangle=false)
clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cut...
Definition: intersect-inl.h:889
fcl::detail::Intersect::checkRootValidity_VE
static bool checkRootValidity_VE(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &p0, const Vector3< S > &va, const Vector3< S > &vb, const Vector3< S > &vp, S t)
Check whether a root for VE intersection is valid.
Definition: intersect-inl.h:242
r
S r
Definition: test_sphere_box.cpp:171
fcl::detail::Intersect::gaussianCDF
static S gaussianCDF(S x)
compute the cdf(x)
Definition: intersect-inl.h:1110
fcl::detail::Intersect::buildEdgePlane
static bool buildEdgePlane(const Vector3< S > &v1, const Vector3< S > &v2, const Vector3< S > &tn, Vector3< S > *n, S *t)
build a plane pass through edge v1 and v2, normal is tn
Definition: intersect-inl.h:1057
fcl::detail::Intersect::intersect_VF_filtered
static bool intersect_VF_filtered(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &p0, const Vector3< S > &a1, const Vector3< S > &b1, const Vector3< S > &c1, const Vector3< S > &p1, S *collision_time, Vector3< S > *p_i, bool useNewton=true)
CCD intersect between one vertex and one face, using additional filter.
Definition: intersect-inl.h:570
fcl::detail::Intersect::insideLineSegment
static bool insideLineSegment(const Vector3< S > &a, const Vector3< S > &b, const Vector3< S > &p)
Check whether one point p is within a line segment [a, b].
Definition: intersect-inl.h:162
fcl::detail::Intersect::intersect_EE
static bool intersect_EE(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &d0, const Vector3< S > &a1, const Vector3< S > &b1, const Vector3< S > &c1, const Vector3< S > &d1, S *collision_time, Vector3< S > *p_i, bool useNewton=true)
CCD intersect between two edges [a0, b0] and [a1, b1] are points for one edge in time t0 and t1 [c0,...
Definition: intersect-inl.h:451
fcl::detail::Intersect::computeDeepestPoints
static void computeDeepestPoints(Vector3< S > *clipped_points, unsigned int num_clipped_points, const Vector3< S > &n, S t, S *penetration_depth, Vector3< S > *deepest_points, unsigned int *num_deepest_points)
compute the points which has deepest penetration depth
Definition: intersect-inl.h:849
min
static T min(T x, T y)
Definition: svm.cpp:49
intersect.h
fcl::detail::Intersect::sameSideOfPlane
static bool sameSideOfPlane(const Vector3< S > &v1, const Vector3< S > &v2, const Vector3< S > &v3, const Vector3< S > &n, S t)
check wether points v1, v2, v2 are on the same side of plane n * x - t = 0
Definition: intersect-inl.h:1074
fcl::detail::Intersect::project6
static int project6(const Vector3< S > &ax, const Vector3< S > &p1, const Vector3< S > &p2, const Vector3< S > &p3, const Vector3< S > &q1, const Vector3< S > &q2, const Vector3< S > &q3)
Project function used in intersect_Triangle()
Definition: intersect-inl.h:1086
fcl::detail::Intersect::intersect_VF
static bool intersect_VF(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &p0, const Vector3< S > &a1, const Vector3< S > &b1, const Vector3< S > &c1, const Vector3< S > &p1, S *collision_time, Vector3< S > *p_i, bool useNewton=true)
CCD intersect between one vertex and one face [a0, b0, c0] and [a1, b1, c1] are points for the triang...
Definition: intersect-inl.h:385
fcl::detail::Intersect::intersect_VE
static bool intersect_VE(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &p0, const Vector3< S > &a1, const Vector3< S > &b1, const Vector3< S > &p1, const Vector3< S > &L)
CCD intersect between one vertex and and one edge.
Definition: intersect-inl.h:517
fcl::detail::Intersect::computeCubicCoeff_VF
static void computeCubicCoeff_VF(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &p0, const Vector3< S > &va, const Vector3< S > &vb, const Vector3< S > &vc, const Vector3< S > &vp, S *a, S *b, S *c, S *d)
Compute the cubic coefficients for VF intersection See Paper "Interactive Continuous Collision Detect...
Definition: intersect-inl.h:318
fcl::detail::Intersect::solveSquare
static bool solveSquare(S a, S b, S c, const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &d0, const Vector3< S > &va, const Vector3< S > &vb, const Vector3< S > &vc, const Vector3< S > &vd, bool bVF, S *ret)
Solve a square function for EE intersection (with interval restriction)
Definition: intersect-inl.h:251
fcl::detail::Intersect::intersect_EE_filtered
static bool intersect_EE_filtered(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &d0, const Vector3< S > &a1, const Vector3< S > &b1, const Vector3< S > &c1, const Vector3< S > &d1, S *collision_time, Vector3< S > *p_i, bool useNewton=true)
CCD intersect between two edges, using additional filter.
Definition: intersect-inl.h:584
fcl::detail::Intersect::distanceToPlane
static S distanceToPlane(const Vector3< S > &n, S t, const Vector3< S > &v)
distance of point v to a plane n * x - t = 0
Definition: intersect-inl.h:1033
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::Intersect::checkRootValidity_EE
static bool checkRootValidity_EE(const Vector3< S > &a0, const Vector3< S > &b0, const Vector3< S > &c0, const Vector3< S > &d0, const Vector3< S > &va, const Vector3< S > &vb, const Vector3< S > &vc, const Vector3< S > &vd, S t, Vector3< S > *q_i=nullptr)
Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given...
Definition: intersect-inl.h:221
fcl::detail::Intersect::intersect_Triangle_ODE_style
static bool intersect_Triangle_ODE_style(const Vector3< S > &P1, const Vector3< S > &P2, const Vector3< S > &P3, const Vector3< S > &Q1, const Vector3< S > &Q2, const Vector3< S > &Q3, Vector3< S > *contact_points=nullptr, unsigned int *num_contact_points=nullptr, S *penetration_depth=nullptr, Vector3< S > *normal=nullptr)
CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3].
Definition: intersect-inl.h:643
fcl::detail::PolySolver::solveCubic
static int solveCubic(S c[4], S s[3])
Solve a cubic function with coefficients c, return roots s and number of roots.
Definition: polysolver-inl.h:103


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