FCL  0.6.0
Flexible Collision Library
plane-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef FCL_NARROWPHASE_DETAIL_PLANE_INL_H
39 #define FCL_NARROWPHASE_DETAIL_PLANE_INL_H
40 
41 #include "fcl/narrowphase/detail/primitive_shape_algorithm/plane.h"
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 extern template
51 bool spherePlaneIntersect(const Sphere<double>& s1, const Transform3<double>& tf1,
52  const Plane<double>& s2, const Transform3<double>& tf2,
53  std::vector<ContactPoint<double>>* contacts);
54 
55 //==============================================================================
56 extern template
57 bool ellipsoidPlaneIntersect(const Ellipsoid<double>& s1, const Transform3<double>& tf1,
58  const Plane<double>& s2, const Transform3<double>& tf2,
59  std::vector<ContactPoint<double>>* contacts);
60 
61 //==============================================================================
62 extern template
63 bool boxPlaneIntersect(const Box<double>& s1, const Transform3<double>& tf1,
64  const Plane<double>& s2, const Transform3<double>& tf2,
65  std::vector<ContactPoint<double>>* contacts);
66 
67 //==============================================================================
68 extern template
69 bool capsulePlaneIntersect(const Capsule<double>& s1, const Transform3<double>& tf1,
70  const Plane<double>& s2, const Transform3<double>& tf2);
71 
72 //==============================================================================
73 extern template
74 bool capsulePlaneIntersect(const Capsule<double>& s1, const Transform3<double>& tf1,
75  const Plane<double>& s2, const Transform3<double>& tf2,
76  std::vector<ContactPoint<double>>* contacts);
77 
78 //==============================================================================
79 extern template
80 bool cylinderPlaneIntersect(const Cylinder<double>& s1, const Transform3<double>& tf1,
81  const Plane<double>& s2, const Transform3<double>& tf2);
82 
83 //==============================================================================
84 extern template
85 bool cylinderPlaneIntersect(const Cylinder<double>& s1, const Transform3<double>& tf1,
86  const Plane<double>& s2, const Transform3<double>& tf2,
87  std::vector<ContactPoint<double>>* contacts);
88 
89 //==============================================================================
90 extern template
91 bool conePlaneIntersect(const Cone<double>& s1, const Transform3<double>& tf1,
92  const Plane<double>& s2, const Transform3<double>& tf2,
93  std::vector<ContactPoint<double>>* contacts);
94 
95 //==============================================================================
96 extern template
97 bool convexPlaneIntersect(const Convex<double>& s1, const Transform3<double>& tf1,
98  const Plane<double>& s2, const Transform3<double>& tf2,
99  Vector3<double>* contact_points, double* penetration_depth, Vector3<double>* normal);
100 
101 //==============================================================================
102 extern template
103 bool planeTriangleIntersect(const Plane<double>& s1, const Transform3<double>& tf1,
104  const Vector3<double>& P1, const Vector3<double>& P2, const Vector3<double>& P3, const Transform3<double>& tf2,
105  Vector3<double>* contact_points, double* penetration_depth, Vector3<double>* normal);
106 
107 //==============================================================================
108 extern template
109 bool planeIntersect(const Plane<double>& s1, const Transform3<double>& tf1,
110  const Plane<double>& s2, const Transform3<double>& tf2,
111  std::vector<ContactPoint<double>>* contacts);
112 
113 //==============================================================================
114 template <typename S>
115 S planeIntersectTolerance()
116 {
117  return 0;
118 }
119 
120 //==============================================================================
121 template <typename S>
122 bool spherePlaneIntersect(const Sphere<S>& s1, const Transform3<S>& tf1,
123  const Plane<S>& s2, const Transform3<S>& tf2,
124  std::vector<ContactPoint<S>>* contacts)
125 {
126  const Plane<S> new_s2 = transform(s2, tf2);
127 
128  const Vector3<S>& center = tf1.translation();
129  const S signed_dist = new_s2.signedDistance(center);
130  const S depth = - std::abs(signed_dist) + s1.radius;
131 
132  if(depth >= 0)
133  {
134  if (contacts)
135  {
136  const Vector3<S> normal = (signed_dist > 0) ? (-new_s2.n).eval() : new_s2.n;
137  const Vector3<S> point = center - new_s2.n * signed_dist;
138  const S penetration_depth = depth;
139 
140  contacts->emplace_back(normal, point, penetration_depth);
141  }
142 
143  return true;
144  }
145  else
146  {
147  return false;
148  }
149 }
150 
151 //==============================================================================
152 template <typename S>
153 bool ellipsoidPlaneIntersect(const Ellipsoid<S>& s1, const Transform3<S>& tf1,
154  const Plane<S>& s2, const Transform3<S>& tf2,
155  std::vector<ContactPoint<S>>* contacts)
156 {
157  // We first compute a single contact in the ellipsoid coordinates, tf1, then
158  // will transform it to the world frame. So we use a new plane that is
159  // expressed in the ellipsoid coordinates.
160  const Plane<S>& new_s2 = transform(s2, tf1.inverse(Eigen::Isometry) * tf2);
161 
162  // Compute distance between the ellipsoid's center and a contact plane, whose
163  // normal is equal to the plane's normal.
164  const Vector3<S> normal2(std::pow(new_s2.n[0], 2), std::pow(new_s2.n[1], 2), std::pow(new_s2.n[2], 2));
165  const Vector3<S> radii2(std::pow(s1.radii[0], 2), std::pow(s1.radii[1], 2), std::pow(s1.radii[2], 2));
166  const S center_to_contact_plane = std::sqrt(normal2.dot(radii2));
167 
168  const S signed_dist = -new_s2.d;
169 
170  // Depth is the distance between the contact plane and the given plane.
171  const S depth = center_to_contact_plane - std::abs(signed_dist);
172 
173  if (depth >= 0)
174  {
175  if (contacts)
176  {
177  // Transform the results to the world coordinates.
178  const Vector3<S> normal = (signed_dist > 0) ? (tf1.linear() * -new_s2.n).eval() : (tf1.linear() * new_s2.n).eval(); // pointing from the ellipsoid's center to the plane
179  const Vector3<S> support_vector = (1.0/center_to_contact_plane) * Vector3<S>(radii2[0]*new_s2.n[0], radii2[1]*new_s2.n[1], radii2[2]*new_s2.n[2]);
180  const Vector3<S> point_in_plane_coords = support_vector * (depth / new_s2.n.dot(support_vector) - 1.0);
181  const Vector3<S> point = (signed_dist > 0) ? tf1 * point_in_plane_coords : tf1 * -point_in_plane_coords; // a middle point of the intersecting volume
182  const S penetration_depth = depth;
183 
184  contacts->emplace_back(normal, point, penetration_depth);
185  }
186 
187  return true;
188  }
189  else
190  {
191  return false;
192  }
193 }
194 
195 //==============================================================================
196 template <typename S>
197 bool boxPlaneIntersect(const Box<S>& s1, const Transform3<S>& tf1,
198  const Plane<S>& s2, const Transform3<S>& tf2,
199  std::vector<ContactPoint<S>>* contacts)
200 {
201  Plane<S> new_s2 = transform(s2, tf2);
202 
203  const Matrix3<S>& R = tf1.linear();
204  const Vector3<S>& T = tf1.translation();
205 
206  Vector3<S> Q = R.transpose() * new_s2.n;
207  Vector3<S> A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
208  Vector3<S> B = A.cwiseAbs();
209 
210  S signed_dist = new_s2.signedDistance(T);
211  S depth = 0.5 * (B[0] + B[1] + B[2]) - std::abs(signed_dist);
212  if(depth < 0) return false;
213 
214  Vector3<S> axis[3];
215  axis[0] = R.col(0);
216  axis[1] = R.col(1);
217  axis[2] = R.col(2);
218 
219  // find the deepest point
220  Vector3<S> p = T;
221 
222  // when center is on the positive side of the plane, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the minimum
223  // otherwise, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the maximum
224  int sign = (signed_dist > 0) ? 1 : -1;
225 
226  if(std::abs(Q[0] - 1) < planeIntersectTolerance<S>() || std::abs(Q[0] + 1) < planeIntersectTolerance<S>())
227  {
228  int sign2 = (A[0] > 0) ? -1 : 1;
229  sign2 *= sign;
230  p += axis[0] * (0.5 * s1.side[0] * sign2);
231  }
232  else if(std::abs(Q[1] - 1) < planeIntersectTolerance<S>() || std::abs(Q[1] + 1) < planeIntersectTolerance<S>())
233  {
234  int sign2 = (A[1] > 0) ? -1 : 1;
235  sign2 *= sign;
236  p += axis[1] * (0.5 * s1.side[1] * sign2);
237  }
238  else if(std::abs(Q[2] - 1) < planeIntersectTolerance<S>() || std::abs(Q[2] + 1) < planeIntersectTolerance<S>())
239  {
240  int sign2 = (A[2] > 0) ? -1 : 1;
241  sign2 *= sign;
242  p += axis[2] * (0.5 * s1.side[2] * sign2);
243  }
244  else
245  {
246  for(std::size_t i = 0; i < 3; ++i)
247  {
248  int sign2 = (A[i] > 0) ? -1 : 1;
249  sign2 *= sign;
250  p += axis[i] * (0.5 * s1.side[i] * sign2);
251  }
252  }
253 
254  // compute the contact point by project the deepest point onto the plane
255  if (contacts)
256  {
257  const Vector3<S> normal = (signed_dist > 0) ? (-new_s2.n).eval() : new_s2.n;
258  const Vector3<S> point = p - new_s2.n * new_s2.signedDistance(p);
259  const S penetration_depth = depth;
260 
261  contacts->emplace_back(normal, point, penetration_depth);
262  }
263 
264  return true;
265 }
266 
267 //==============================================================================
268 template <typename S>
269 bool capsulePlaneIntersect(const Capsule<S>& s1, const Transform3<S>& tf1,
270  const Plane<S>& s2, const Transform3<S>& tf2)
271 {
272  Plane<S> new_s2 = transform(s2, tf2);
273 
274  const Matrix3<S>& R = tf1.linear();
275  const Vector3<S>& T = tf1.translation();
276 
277  Vector3<S> dir_z = R.col(2);
278  Vector3<S> p1 = T + dir_z * (0.5 * s1.lz);
279  Vector3<S> p2 = T - dir_z * (0.5 * s1.lz);
280 
281  S d1 = new_s2.signedDistance(p1);
282  S d2 = new_s2.signedDistance(p2);
283 
284  // two end points on different side of the plane
285  if(d1 * d2 <= 0)
286  return true;
287 
288  // two end points on the same side of the plane, but the end point spheres might intersect the plane
289  return (std::abs(d1) <= s1.radius) || (std::abs(d2) <= s1.radius);
290 }
291 
292 //==============================================================================
293 template <typename S>
294 bool capsulePlaneIntersect(const Capsule<S>& s1, const Transform3<S>& tf1,
295  const Plane<S>& s2, const Transform3<S>& tf2,
296  std::vector<ContactPoint<S>>* contacts)
297 {
298  if(!contacts)
299  {
300  return capsulePlaneIntersect(s1, tf1, s2, tf2);
301  }
302  else
303  {
304  Plane<S> new_s2 = transform(s2, tf2);
305 
306  const Matrix3<S>& R = tf1.linear();
307  const Vector3<S>& T = tf1.translation();
308 
309  Vector3<S> dir_z = R.col(2);
310 
311 
312  Vector3<S> p1 = T + dir_z * (0.5 * s1.lz);
313  Vector3<S> p2 = T - dir_z * (0.5 * s1.lz);
314 
315  S d1 = new_s2.signedDistance(p1);
316  S d2 = new_s2.signedDistance(p2);
317 
318  S abs_d1 = std::abs(d1);
319  S abs_d2 = std::abs(d2);
320 
321  // two end points on different side of the plane
322  // the contact point is the intersect of axis with the plane
323  // the normal is the direction to avoid intersection
324  // the depth is the minimum distance to resolve the collision
325  if(d1 * d2 < -planeIntersectTolerance<S>())
326  {
327  if(abs_d1 < abs_d2)
328  {
329  if (contacts)
330  {
331  const Vector3<S> normal = (d1 < 0) ? (-new_s2.n).eval() : new_s2.n;
332  const Vector3<S> point = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2));
333  const S penetration_depth = abs_d1 + s1.radius;
334 
335  contacts->emplace_back(normal, point, penetration_depth);
336  }
337  }
338  else
339  {
340  if (contacts)
341  {
342  const Vector3<S> normal = (d2 < 0) ? (-new_s2.n).eval() : new_s2.n;
343  const Vector3<S> point = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2));
344  const S penetration_depth = abs_d2 + s1.radius;
345 
346  contacts->emplace_back(normal, point, penetration_depth);
347  }
348  }
349  return true;
350  }
351 
352  if(abs_d1 > s1.radius && abs_d2 > s1.radius)
353  {
354  return false;
355  }
356  else
357  {
358  if (contacts)
359  {
360  const Vector3<S> normal = (d1 < 0) ? new_s2.n : (-new_s2.n).eval();
361  const S penetration_depth = s1.radius - std::min(abs_d1, abs_d2);
362  Vector3<S> point;
363  if(abs_d1 <= s1.radius && abs_d2 <= s1.radius)
364  {
365  const Vector3<S> c1 = p1 - new_s2.n * d2;
366  const Vector3<S> c2 = p2 - new_s2.n * d1;
367  point = (c1 + c2) * 0.5;
368  }
369  else if(abs_d1 <= s1.radius)
370  {
371  const Vector3<S> c = p1 - new_s2.n * d1;
372  point = c;
373  }
374  else // (abs_d2 <= s1.radius)
375  {
376  assert(abs_d2 <= s1.radius);
377 
378  const Vector3<S> c = p2 - new_s2.n * d2;
379  point = c;
380  }
381 
382  contacts->emplace_back(normal, point, penetration_depth);
383  }
384 
385  return true;
386  }
387  }
388 }
389 
390 //==============================================================================
391 template <typename S>
392 bool cylinderPlaneIntersect(const Cylinder<S>& s1, const Transform3<S>& tf1,
393  const Plane<S>& s2, const Transform3<S>& tf2)
394 {
395  Plane<S> new_s2 = transform(s2, tf2);
396 
397  const Matrix3<S>& R = tf1.linear();
398  const Vector3<S>& T = tf1.translation();
399 
400  Vector3<S> Q = R.transpose() * new_s2.n;
401 
402  S term = std::abs(Q[2]) * s1.lz + s1.radius * std::sqrt(Q[0] * Q[0] + Q[1] * Q[1]);
403  S dist = new_s2.distance(T);
404  S depth = term - dist;
405 
406  if(depth < 0)
407  return false;
408  else
409  return true;
410 }
411 
412 //==============================================================================
413 template <typename S>
414 bool cylinderPlaneIntersect(const Cylinder<S>& s1, const Transform3<S>& tf1,
415  const Plane<S>& s2, const Transform3<S>& tf2,
416  std::vector<ContactPoint<S>>* contacts)
417 {
418  if(!contacts)
419  {
420  return cylinderPlaneIntersect(s1, tf1, s2, tf2);
421  }
422  else
423  {
424  Plane<S> new_s2 = transform(s2, tf2);
425 
426  const Matrix3<S>& R = tf1.linear();
427  const Vector3<S>& T = tf1.translation();
428 
429  Vector3<S> dir_z = R.col(2);
430  S cosa = dir_z.dot(new_s2.n);
431 
432  if(std::abs(cosa) < planeIntersectTolerance<S>())
433  {
434  S d = new_s2.signedDistance(T);
435  S depth = s1.radius - std::abs(d);
436  if(depth < 0) return false;
437  else
438  {
439  if (contacts)
440  {
441  const Vector3<S> normal = (d < 0) ? new_s2.n : (-new_s2.n).eval();
442  const Vector3<S> point = T - new_s2.n * d;
443  const S penetration_depth = depth;
444 
445  contacts->emplace_back(normal, point, penetration_depth);
446  }
447  return true;
448  }
449  }
450  else
451  {
452  Vector3<S> C = dir_z * cosa - new_s2.n;
453  if(std::abs(cosa + 1) < planeIntersectTolerance<S>() || std::abs(cosa - 1) < planeIntersectTolerance<S>())
454  C = Vector3<S>(0, 0, 0);
455  else
456  {
457  S s = C.norm();
458  s = s1.radius / s;
459  C *= s;
460  }
461 
462  Vector3<S> p1 = T + dir_z * (0.5 * s1.lz);
463  Vector3<S> p2 = T - dir_z * (0.5 * s1.lz);
464 
465  Vector3<S> c1, c2;
466  if(cosa > 0)
467  {
468  c1 = p1 - C;
469  c2 = p2 + C;
470  }
471  else
472  {
473  c1 = p1 + C;
474  c2 = p2 - C;
475  }
476 
477  S d1 = new_s2.signedDistance(c1);
478  S d2 = new_s2.signedDistance(c2);
479 
480  if(d1 * d2 <= 0)
481  {
482  S abs_d1 = std::abs(d1);
483  S abs_d2 = std::abs(d2);
484 
485  if(abs_d1 > abs_d2)
486  {
487  if (contacts)
488  {
489  const Vector3<S> normal = (d2 < 0) ? (-new_s2.n).eval() : new_s2.n;
490  const Vector3<S> point = c2 - new_s2.n * d2;
491  const S penetration_depth = abs_d2;
492 
493  contacts->emplace_back(normal, point, penetration_depth);
494  }
495  }
496  else
497  {
498  if (contacts)
499  {
500  const Vector3<S> normal = (d1 < 0) ? (-new_s2.n).eval() : new_s2.n;
501  const Vector3<S> point = c1 - new_s2.n * d1;
502  const S penetration_depth = abs_d1;
503 
504  contacts->emplace_back(normal, point, penetration_depth);
505  }
506  }
507  return true;
508  }
509  else
510  {
511  return false;
512  }
513  }
514  }
515 }
516 
517 //==============================================================================
518 template <typename S>
519 bool conePlaneIntersect(const Cone<S>& s1, const Transform3<S>& tf1,
520  const Plane<S>& s2, const Transform3<S>& tf2,
521  std::vector<ContactPoint<S>>* contacts)
522 {
523  Plane<S> new_s2 = transform(s2, tf2);
524 
525  const Matrix3<S>& R = tf1.linear();
526  const Vector3<S>& T = tf1.translation();
527 
528  Vector3<S> dir_z = R.col(2);
529  S cosa = dir_z.dot(new_s2.n);
530 
531  if(std::abs(cosa) < planeIntersectTolerance<S>())
532  {
533  S d = new_s2.signedDistance(T);
534  S depth = s1.radius - std::abs(d);
535  if(depth < 0) return false;
536  else
537  {
538  if (contacts)
539  {
540  const Vector3<S> normal = (d < 0) ? new_s2.n : (-new_s2.n).eval();
541  const Vector3<S> point = T - dir_z * (0.5 * s1.lz) + dir_z * (0.5 * depth / s1.radius * s1.lz) - new_s2.n * d;
542  const S penetration_depth = depth;
543 
544  contacts->emplace_back(normal, point, penetration_depth);
545  }
546 
547  return true;
548  }
549  }
550  else
551  {
552  Vector3<S> C = dir_z * cosa - new_s2.n;
553  if(std::abs(cosa + 1) < planeIntersectTolerance<S>() || std::abs(cosa - 1) < planeIntersectTolerance<S>())
554  C = Vector3<S>(0, 0, 0);
555  else
556  {
557  S s = C.norm();
558  s = s1.radius / s;
559  C *= s;
560  }
561 
562  Vector3<S> c[3];
563  c[0] = T + dir_z * (0.5 * s1.lz);
564  c[1] = T - dir_z * (0.5 * s1.lz) + C;
565  c[2] = T - dir_z * (0.5 * s1.lz) - C;
566 
567  S d[3];
568  d[0] = new_s2.signedDistance(c[0]);
569  d[1] = new_s2.signedDistance(c[1]);
570  d[2] = new_s2.signedDistance(c[2]);
571 
572  if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) || (d[0] <= 0 && d[1] <= 0 && d[2] <= 0))
573  return false;
574  else
575  {
576  bool positive[3];
577  for(std::size_t i = 0; i < 3; ++i)
578  positive[i] = (d[i] >= 0);
579 
580  int n_positive = 0;
581  S d_positive = 0, d_negative = 0;
582  for(std::size_t i = 0; i < 3; ++i)
583  {
584  if(positive[i])
585  {
586  n_positive++;
587  if(d_positive <= d[i]) d_positive = d[i];
588  }
589  else
590  {
591  if(d_negative <= -d[i]) d_negative = -d[i];
592  }
593  }
594 
595  if (contacts)
596  {
597  const Vector3<S> normal = (d_positive > d_negative) ? (-new_s2.n).eval() : new_s2.n;
598  const S penetration_depth = std::min(d_positive, d_negative);
599 
600  Vector3<S> point;
601  Vector3<S> p[2] { Vector3<S>::Zero(), Vector3<S>::Zero() };
602  Vector3<S> q = Vector3<S>::Zero();
603 
604  S p_d[2];
605  S q_d(0);
606 
607  if(n_positive == 2)
608  {
609  for(std::size_t i = 0, j = 0; i < 3; ++i)
610  {
611  if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
612  else { q = c[i]; q_d = d[i]; }
613  }
614 
615  const Vector3<S> t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]);
616  const Vector3<S> t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]);
617  point = (t1 + t2) * 0.5;
618  }
619  else
620  {
621  for(std::size_t i = 0, j = 0; i < 3; ++i)
622  {
623  if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
624  else { q = c[i]; q_d = d[i]; }
625  }
626 
627  const Vector3<S> t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]);
628  const Vector3<S> t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]);
629  point = (t1 + t2) * 0.5;
630  }
631 
632  contacts->emplace_back(normal, point, penetration_depth);
633  }
634 
635  return true;
636  }
637  }
638 }
639 
640 //==============================================================================
641 template <typename S>
642 bool convexPlaneIntersect(const Convex<S>& s1, const Transform3<S>& tf1,
643  const Plane<S>& s2, const Transform3<S>& tf2,
644  Vector3<S>* contact_points, S* penetration_depth, Vector3<S>* normal)
645 {
646  Plane<S> new_s2 = transform(s2, tf2);
647 
648  Vector3<S> v_min, v_max;
649  S d_min = std::numeric_limits<S>::max(), d_max = -std::numeric_limits<S>::max();
650 
651  for(int i = 0; i < s1.num_points; ++i)
652  {
653  Vector3<S> p = tf1 * s1.points[i];
654 
655  S d = new_s2.signedDistance(p);
656 
657  if(d < d_min) { d_min = d; v_min = p; }
658  if(d > d_max) { d_max = d; v_max = p; }
659  }
660 
661  if(d_min * d_max > 0) return false;
662  else
663  {
664  if(d_min + d_max > 0)
665  {
666  if(penetration_depth) *penetration_depth = -d_min;
667  if(normal) *normal = -new_s2.n;
668  if(contact_points) *contact_points = v_min - new_s2.n * d_min;
669  }
670  else
671  {
672  if(penetration_depth) *penetration_depth = d_max;
673  if(normal) *normal = new_s2.n;
674  if(contact_points) *contact_points = v_max - new_s2.n * d_max;
675  }
676  return true;
677  }
678 }
679 
680 //==============================================================================
681 template <typename S>
682 bool planeTriangleIntersect(const Plane<S>& s1, const Transform3<S>& tf1,
683  const Vector3<S>& P1, const Vector3<S>& P2, const Vector3<S>& P3, const Transform3<S>& tf2,
684  Vector3<S>* contact_points, S* penetration_depth, Vector3<S>* normal)
685 {
686  Plane<S> new_s1 = transform(s1, tf1);
687 
688  Vector3<S> c[3];
689  c[0] = tf2 * P1;
690  c[1] = tf2 * P2;
691  c[2] = tf2 * P3;
692 
693  S d[3];
694  d[0] = new_s1.signedDistance(c[0]);
695  d[1] = new_s1.signedDistance(c[1]);
696  d[2] = new_s1.signedDistance(c[2]);
697 
698  if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) || (d[0] <= 0 && d[1] <= 0 && d[2] <= 0))
699  return false;
700  else
701  {
702  bool positive[3];
703  for(std::size_t i = 0; i < 3; ++i)
704  positive[i] = (d[i] > 0);
705 
706  int n_positive = 0;
707  S d_positive = 0, d_negative = 0;
708  for(std::size_t i = 0; i < 3; ++i)
709  {
710  if(positive[i])
711  {
712  n_positive++;
713  if(d_positive <= d[i]) d_positive = d[i];
714  }
715  else
716  {
717  if(d_negative <= -d[i]) d_negative = -d[i];
718  }
719  }
720 
721  if(penetration_depth) *penetration_depth = std::min(d_positive, d_negative);
722  if(normal) *normal = (d_positive > d_negative) ? new_s1.n : (-new_s1.n).eval();
723  if(contact_points)
724  {
725  Vector3<S> p[2] = {Vector3<S>::Zero(), Vector3<S>::Zero()};
726  Vector3<S> q = Vector3<S>::Zero();
727 
728  S p_d[2];
729  S q_d(0);
730 
731  if(n_positive == 2)
732  {
733  for(std::size_t i = 0, j = 0; i < 3; ++i)
734  {
735  if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
736  else { q = c[i]; q_d = d[i]; }
737  }
738 
739  Vector3<S> t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]);
740  Vector3<S> t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]);
741  *contact_points = (t1 + t2) * 0.5;
742  }
743  else
744  {
745  for(std::size_t i = 0, j = 0; i < 3; ++i)
746  {
747  if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
748  else { q = c[i]; q_d = d[i]; }
749  }
750 
751  Vector3<S> t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]);
752  Vector3<S> t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]);
753  *contact_points = (t1 + t2) * 0.5;
754  }
755  }
756  return true;
757  }
758 }
759 
760 //==============================================================================
761 template <typename S>
762 bool planeIntersect(const Plane<S>& s1, const Transform3<S>& tf1,
763  const Plane<S>& s2, const Transform3<S>& tf2,
764  std::vector<ContactPoint<S>>* /*contacts*/)
765 {
766  Plane<S> new_s1 = transform(s1, tf1);
767  Plane<S> new_s2 = transform(s2, tf2);
768 
769  S a = (new_s1.n).dot(new_s2.n);
770  if(a == 1 && new_s1.d != new_s2.d)
771  return false;
772  if(a == -1 && new_s1.d != -new_s2.d)
773  return false;
774 
775  return true;
776 }
777 
778 } // namespace detail
779 } // namespace fcl
780 
781 #endif
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::time::point
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: time.h:51