narrowphase/detail/primitive_shape_algorithm/halfspace-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_HALFSPACE_INL_H
39 #define FCL_NARROWPHASE_DETAIL_HALFSPACE_INL_H
40 
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 extern template
52  const Sphere<double>& s1, const Transform3<double>& tf1,
53  const Halfspace<double>& s2, const Transform3<double>& tf2,
54  std::vector<ContactPoint<double>>* contacts);
55 
56 //==============================================================================
57 extern template
59  const Ellipsoid<double>& s1, const Transform3<double>& tf1,
60  const Halfspace<double>& s2, const Transform3<double>& tf2,
61  std::vector<ContactPoint<double>>* contacts);
62 
63 //==============================================================================
64 extern template
66  const Box<double>& s1, const Transform3<double>& tf1,
67  const Halfspace<double>& s2, const Transform3<double>& tf2);
68 
69 //==============================================================================
70 extern template
72  const Box<double>& s1, const Transform3<double>& tf1,
73  const Halfspace<double>& s2, const Transform3<double>& tf2,
74  std::vector<ContactPoint<double>>* contacts);
75 
76 //==============================================================================
77 extern template
79  const Capsule<double>& s1, const Transform3<double>& tf1,
80  const Halfspace<double>& s2, const Transform3<double>& tf2,
81  std::vector<ContactPoint<double>>* contacts);
82 
83 //==============================================================================
84 extern template
86  const Cylinder<double>& s1, const Transform3<double>& tf1,
87  const Halfspace<double>& s2, const Transform3<double>& tf2,
88  std::vector<ContactPoint<double>>* contacts);
89 
90 //==============================================================================
91 extern template
93  const Cone<double>& s1, const Transform3<double>& tf1,
94  const Halfspace<double>& s2, const Transform3<double>& tf2,
95  std::vector<ContactPoint<double>>* contacts);
96 
97 //==============================================================================
98 extern template
100  const Convex<double>& s1, const Transform3<double>& tf1,
101  const Halfspace<double>& s2, const Transform3<double>& tf2,
102  Vector3<double>* contact_points, double* penetration_depth, Vector3<double>* normal);
103 
104 //==============================================================================
105 extern template bool convexHalfspaceIntersect(
106  const Convex<double>& convex_C, const Transform3<double>& X_FC,
107  const Halfspace<double>& half_space_H, const Transform3<double>& X_FH,
108  std::vector<ContactPoint<double>>* contacts);
109 
110 //==============================================================================
111 extern template
113  const Halfspace<double>& s1, const Transform3<double>& tf1,
114  const Vector3<double>& P1, const Vector3<double>& P2, const Vector3<double>& P3, const Transform3<double>& tf2,
115  Vector3<double>* contact_points, double* penetration_depth, Vector3<double>* normal);
116 
117 //==============================================================================
118 extern template
120  const Plane<double>& s1, const Transform3<double>& tf1,
121  const Halfspace<double>& s2, const Transform3<double>& tf2,
122  Plane<double>& pl,
123  Vector3<double>& p, Vector3<double>& d,
124  double& penetration_depth,
125  int& ret);
126 
127 //==============================================================================
128 extern template
130  const Halfspace<double>& s1, const Transform3<double>& tf1,
131  const Plane<double>& s2, const Transform3<double>& tf2,
132  Plane<double>& pl, Vector3<double>& p, Vector3<double>& d,
133  double& penetration_depth,
134  int& ret);
135 
136 //==============================================================================
137 extern template
138 bool halfspaceIntersect(
139  const Halfspace<double>& s1, const Transform3<double>& tf1,
140  const Halfspace<double>& s2, const Transform3<double>& tf2,
141  Vector3<double>& p, Vector3<double>& d,
143  double& penetration_depth,
144  int& ret);
145 
146 //==============================================================================
147 template <typename S>
149 {
150  return 0;
151 }
152 
153 //==============================================================================
154 template <typename S>
156  const Halfspace<S>& s2, const Transform3<S>& tf2,
157  std::vector<ContactPoint<S>>* contacts)
158 {
159  const Halfspace<S> new_s2 = transform(s2, tf2);
160  const Vector3<S>& center = tf1.translation();
161  const S depth = s1.radius - new_s2.signedDistance(center);
162 
163  if (depth >= 0)
164  {
165  if (contacts)
166  {
167  const Vector3<S> normal = -new_s2.n; // pointing from s1 to s2
168  const Vector3<S> point = center - new_s2.n * s1.radius + new_s2.n * (depth * 0.5);
169  const S penetration_depth = depth;
170 
171  contacts->emplace_back(normal, point, penetration_depth);
172  }
173 
174  return true;
175  }
176  else
177  {
178  return false;
179  }
180 }
181 
182 //==============================================================================
183 template <typename S>
185  const Halfspace<S>& s2, const Transform3<S>& tf2,
186  std::vector<ContactPoint<S>>* contacts)
187 {
188  // We first compute a single contact in the ellipsoid coordinates, tf1, then
189  // will transform it to the world frame. So we use a new halfspace that is
190  // expressed in the ellipsoid coordinates.
191  const Halfspace<S>& new_s2 = transform(s2, tf1.inverse(Eigen::Isometry) * tf2);
192 
193  // Compute distance between the ellipsoid's center and a contact plane, whose
194  // normal is equal to the halfspace's normal.
195  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));
196  const Vector3<S> radii2(std::pow(s1.radii[0], 2), std::pow(s1.radii[1], 2), std::pow(s1.radii[2], 2));
197  const S center_to_contact_plane = std::sqrt(normal2.dot(radii2));
198 
199  // Depth is the distance between the contact plane and the halfspace.
200  const S depth = center_to_contact_plane + new_s2.d;
201 
202  if (depth >= 0)
203  {
204  if (contacts)
205  {
206  // Transform the results to the world coordinates.
207  const Vector3<S> normal = tf1.linear() * -new_s2.n; // pointing from s1 to s2
208  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]);
209  const Vector3<S> point_in_halfspace_coords = support_vector * (0.5 * depth / new_s2.n.dot(support_vector) - 1.0);
210  const Vector3<S> point = tf1 * point_in_halfspace_coords; // roughly speaking, a middle point of the intersecting volume
211  const S penetration_depth = depth;
212 
213  contacts->emplace_back(normal, point, penetration_depth);
214  }
215 
216  return true;
217  }
218  else
219  {
220  return false;
221  }
222 }
223 
224 //==============================================================================
225 template <typename S>
226 bool boxHalfspaceIntersect(const Box<S>& s1, const Transform3<S>& tf1,
227  const Halfspace<S>& s2, const Transform3<S>& tf2)
228 {
229  Halfspace<S> new_s2 = transform(s2, tf2);
230 
231  const Matrix3<S>& R = tf1.linear();
232  const Vector3<S>& T = tf1.translation();
233 
234  Vector3<S> Q = R.transpose() * new_s2.n;
235  Vector3<S> A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
236  Vector3<S> B = A.cwiseAbs();
237 
238  S depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T);
239  return (depth >= 0);
240 }
241 
242 //==============================================================================
243 template <typename S>
244 bool boxHalfspaceIntersect(const Box<S>& s1, const Transform3<S>& tf1,
245  const Halfspace<S>& s2, const Transform3<S>& tf2,
246  std::vector<ContactPoint<S>>* contacts)
247 {
248  if(!contacts)
249  {
250  return boxHalfspaceIntersect(s1, tf1, s2, tf2);
251  }
252  else
253  {
254  const Halfspace<S> new_s2 = transform(s2, tf2);
255 
256  const Matrix3<S>& R = tf1.linear();
257  const Vector3<S>& T = tf1.translation();
258 
259  Vector3<S> Q = R.transpose() * new_s2.n;
260  Vector3<S> A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
261  Vector3<S> B = A.cwiseAbs();
262 
263  S depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T);
264  if(depth < 0) return false;
265 
266  Vector3<S> axis[3];
267  axis[0] = R.col(0);
268  axis[1] = R.col(1);
269  axis[2] = R.col(2);
270 
272  Vector3<S> p(T);
273  int sign = 0;
274 
275  if(std::abs(Q[0] - 1) < halfspaceIntersectTolerance<S>() || std::abs(Q[0] + 1) < halfspaceIntersectTolerance<S>())
276  {
277  sign = (A[0] > 0) ? -1 : 1;
278  p += axis[0] * (0.5 * s1.side[0] * sign);
279  }
280  else if(std::abs(Q[1] - 1) < halfspaceIntersectTolerance<S>() || std::abs(Q[1] + 1) < halfspaceIntersectTolerance<S>())
281  {
282  sign = (A[1] > 0) ? -1 : 1;
283  p += axis[1] * (0.5 * s1.side[1] * sign);
284  }
285  else if(std::abs(Q[2] - 1) < halfspaceIntersectTolerance<S>() || std::abs(Q[2] + 1) < halfspaceIntersectTolerance<S>())
286  {
287  sign = (A[2] > 0) ? -1 : 1;
288  p += axis[2] * (0.5 * s1.side[2] * sign);
289  }
290  else
291  {
292  for(std::size_t i = 0; i < 3; ++i)
293  {
294  sign = (A[i] > 0) ? -1 : 1;
295  p += axis[i] * (0.5 * s1.side[i] * sign);
296  }
297  }
298 
300  if (contacts)
301  {
302  const Vector3<S> normal = -new_s2.n;
303  const Vector3<S> point = p + new_s2.n * (depth * 0.5);
304  const S penetration_depth = depth;
305 
306  contacts->emplace_back(normal, point, penetration_depth);
307  }
308 
309  return true;
310  }
311 }
312 
313 //==============================================================================
314 template <typename S>
316  const Halfspace<S>& s2, const Transform3<S>& tf2,
317  std::vector<ContactPoint<S>>* contacts)
318 {
319  Halfspace<S> new_s2 = transform(s2, tf2);
320 
321  const Matrix3<S>& R = tf1.linear();
322  const Vector3<S>& T = tf1.translation();
323 
324  Vector3<S> dir_z = R.col(2);
325 
326  S cosa = dir_z.dot(new_s2.n);
327  if(std::abs(cosa) < halfspaceIntersectTolerance<S>())
328  {
329  S signed_dist = new_s2.signedDistance(T);
330  S depth = s1.radius - signed_dist;
331  if(depth < 0) return false;
332 
333  if (contacts)
334  {
335  const Vector3<S> normal = -new_s2.n;
336  const Vector3<S> point = T + new_s2.n * (0.5 * depth - s1.radius);
337  const S penetration_depth = depth;
338 
339  contacts->emplace_back(normal, point, penetration_depth);
340  }
341 
342  return true;
343  }
344  else
345  {
346  int sign = (cosa > 0) ? -1 : 1;
347  Vector3<S> p = T + dir_z * (s1.lz * 0.5 * sign);
348 
349  S signed_dist = new_s2.signedDistance(p);
350  S depth = s1.radius - signed_dist;
351  if(depth < 0) return false;
352 
353  if (contacts)
354  {
355  const Vector3<S> normal = -new_s2.n;
356  const Vector3<S> point = p - new_s2.n * s1.radius + new_s2.n * (0.5 * depth); // deepest point
357  const S penetration_depth = depth;
358 
359  contacts->emplace_back(normal, point, penetration_depth);
360  }
361 
362  return true;
363  }
364 }
365 
366 //==============================================================================
367 template <typename S>
369  const Halfspace<S>& s2, const Transform3<S>& tf2,
370  std::vector<ContactPoint<S>>* contacts)
371 {
372  Halfspace<S> new_s2 = transform(s2, tf2);
373 
374  const Matrix3<S>& R = tf1.linear();
375  const Vector3<S>& T = tf1.translation();
376 
377  Vector3<S> dir_z = R.col(2);
378  S cosa = dir_z.dot(new_s2.n);
379 
380  if(std::abs(cosa) < halfspaceIntersectTolerance<S>())
381  {
382  S signed_dist = new_s2.signedDistance(T);
383  S depth = s1.radius - signed_dist;
384  if(depth < 0) return false;
385 
386  if (contacts)
387  {
388  const Vector3<S> normal = -new_s2.n;
389  const Vector3<S> point = T + new_s2.n * (0.5 * depth - s1.radius);
390  const S penetration_depth = depth;
391 
392  contacts->emplace_back(normal, point, penetration_depth);
393  }
394 
395  return true;
396  }
397  else
398  {
399  Vector3<S> C = dir_z * cosa - new_s2.n;
400  if(std::abs(cosa + 1) < halfspaceIntersectTolerance<S>() || std::abs(cosa - 1) < halfspaceIntersectTolerance<S>())
401  C = Vector3<S>(0, 0, 0);
402  else
403  {
404  S s = C.norm();
405  s = s1.radius / s;
406  C *= s;
407  }
408 
409  int sign = (cosa > 0) ? -1 : 1;
410  // deepest point
411  Vector3<S> p = T + dir_z * (s1.lz * 0.5 * sign) + C;
412  S depth = -new_s2.signedDistance(p);
413  if(depth < 0) return false;
414  else
415  {
416  if (contacts)
417  {
418  const Vector3<S> normal = -new_s2.n;
419  const Vector3<S> point = p + new_s2.n * (0.5 * depth);
420  const S penetration_depth = depth;
421 
422  contacts->emplace_back(normal, point, penetration_depth);
423  }
424 
425  return true;
426  }
427  }
428 }
429 
430 //==============================================================================
431 template <typename S>
432 bool coneHalfspaceIntersect(const Cone<S>& s1, const Transform3<S>& tf1,
433  const Halfspace<S>& s2, const Transform3<S>& tf2,
434  std::vector<ContactPoint<S>>* contacts)
435 {
436  Halfspace<S> new_s2 = transform(s2, tf2);
437 
438  const Matrix3<S>& R = tf1.linear();
439  const Vector3<S>& T = tf1.translation();
440 
441  Vector3<S> dir_z = R.col(2);
442  S cosa = dir_z.dot(new_s2.n);
443 
444  if(cosa < halfspaceIntersectTolerance<S>())
445  {
446  S signed_dist = new_s2.signedDistance(T);
447  S depth = s1.radius - signed_dist;
448  if(depth < 0) return false;
449  else
450  {
451  if (contacts)
452  {
453  const Vector3<S> normal = -new_s2.n;
454  const Vector3<S> point = T - dir_z * (s1.lz * 0.5) + new_s2.n * (0.5 * depth - s1.radius);
455  const S penetration_depth = depth;
456 
457  contacts->emplace_back(normal, point, penetration_depth);
458  }
459 
460  return true;
461  }
462  }
463  else
464  {
465  Vector3<S> C = dir_z * cosa - new_s2.n;
466  if(std::abs(cosa + 1) < halfspaceIntersectTolerance<S>() || std::abs(cosa - 1) < halfspaceIntersectTolerance<S>())
467  C = Vector3<S>(0, 0, 0);
468  else
469  {
470  S s = C.norm();
471  s = s1.radius / s;
472  C *= s;
473  }
474 
475  Vector3<S> p1 = T + dir_z * (0.5 * s1.lz);
476  Vector3<S> p2 = T - dir_z * (0.5 * s1.lz) + C;
477 
478  S d1 = new_s2.signedDistance(p1);
479  S d2 = new_s2.signedDistance(p2);
480 
481  if(d1 > 0 && d2 > 0) return false;
482  else
483  {
484  if (contacts)
485  {
486  const S penetration_depth = -std::min(d1, d2);
487  const Vector3<S> normal = -new_s2.n;
488  const Vector3<S> point = ((d1 < d2) ? p1 : p2) + new_s2.n * (0.5 * penetration_depth);
489 
490  contacts->emplace_back(normal, point, penetration_depth);
491  }
492 
493  return true;
494  }
495  }
496 }
497 
498 //==============================================================================
499 // TODO(SeanCurtis-TRI): This is generally unused in FCL. Consider killing it.
500 template <typename S>
502  const Halfspace<S>& s2, const Transform3<S>& tf2,
503  Vector3<S>* contact_points, S* penetration_depth, Vector3<S>* normal)
504 {
505  Halfspace<S> new_s2 = transform(s2, tf2);
506 
507  Vector3<S> v;
508  S depth = std::numeric_limits<S>::max();
509 
510  // Note: There are two issues with this for loop:
511  // 1. We are transforming *every* vertex in the convex. That's a waste.
512  // 2. If we don't report contact results, and we detect collision with the
513  // first vertex, we still process all vertices. Also a waste.
514  for (const auto& vertex : s1.getVertices())
515  {
516  Vector3<S> p = tf1 * vertex;
517 
518  S d = new_s2.signedDistance(p);
519  if(d < depth)
520  {
521  depth = d;
522  v = p;
523  }
524  }
525 
526  if(depth <= 0)
527  {
528  // Note: this value for contact_point only works because depth is really
529  // signed distance, so negating the normal cancels the negation of the
530  // "penetration depth".
531  if(contact_points) *contact_points = v - new_s2.n * (0.5 * depth);
532  // TODO(SeanCurtis-TRI): This appears to have the wrong sign for depth.
533  // We've actually computed *signed distance* which is -depth.
534  if(penetration_depth) *penetration_depth = depth;
535  // Note: This points *into* the half space. It is not clear this matches
536  // any documented convention.
537  if(normal) *normal = -new_s2.n;
538  return true;
539  }
540  else
541  return false;
542 }
543 
544 //==============================================================================
545 template <typename S>
546 bool convexHalfspaceIntersect(const Convex<S>& convex_C,
547  const Transform3<S>& X_FC,
548  const Halfspace<S>& half_space_H,
549  const Transform3<S>& X_FH,
550  std::vector<ContactPoint<S>>* contacts) {
551  Halfspace<S> half_space_C = transform(half_space_H, X_FC.inverse() * X_FH);
552 
553  Vector3<S> p_CV_deepest;
554  S min_signed_distance = std::numeric_limits<S>::max();
555 
556  // TODO: Once we have an efficient "support vector" implementation for Convex
557  // (necessary to make GJK run faster with convex), this could benefit by
558  // simply asking for the support vector in the negative normal direction.
559  // That would also make computing normal_C cheaper; it could just be the
560  // product: X_FC.linear().transpose() * X_FH.linear() * half_space_H.n.
561  for (const auto& p_CV : convex_C.getVertices()) {
562  const S signed_distance = half_space_C.signedDistance(p_CV);
563  if (signed_distance < min_signed_distance) {
564  min_signed_distance = signed_distance;
565  p_CV_deepest = p_CV;
566  if (signed_distance <= 0 && contacts == nullptr) return true;
567  }
568  }
569 
570  const bool intersecting = min_signed_distance <= 0;
571 
572  if (intersecting && contacts) {
573  const Vector3<S> normal_F = X_FH.linear() * half_space_H.n;
574  const Vector3<S> p_FV = X_FC * p_CV_deepest;
575  // NOTE: penetration depth is defined as the negative of signed distance.
576  // So, the depth reported here will always be non-negative.
577  const S depth = -min_signed_distance;
578  contacts->emplace_back(-normal_F, p_FV + normal_F * (0.5 * depth), depth);
579  }
580 
581  return intersecting;
582 }
583 
584 //==============================================================================
585 template <typename S>
587  const Vector3<S>& P1, const Vector3<S>& P2, const Vector3<S>& P3, const Transform3<S>& tf2,
588  Vector3<S>* contact_points, S* penetration_depth, Vector3<S>* normal)
589 {
590  Halfspace<S> new_s1 = transform(s1, tf1);
591 
592  Vector3<S> v = tf2 * P1;
593  S depth = new_s1.signedDistance(v);
594 
595  Vector3<S> p = tf2 * P2;
596  S d = new_s1.signedDistance(p);
597  if(d < depth)
598  {
599  depth = d;
600  v = p;
601  }
602 
603  p = tf2 * P3;
604  d = new_s1.signedDistance(p);
605  if(d < depth)
606  {
607  depth = d;
608  v = p;
609  }
610 
611  if(depth <= 0)
612  {
613  if(penetration_depth) *penetration_depth = -depth;
614  if(normal) *normal = new_s1.n;
615  if(contact_points) *contact_points = v - new_s1.n * (0.5 * depth);
616  return true;
617  }
618  else
619  return false;
620 }
621 
622 //==============================================================================
623 template <typename S>
624 bool planeHalfspaceIntersect(const Plane<S>& s1, const Transform3<S>& tf1,
625  const Halfspace<S>& s2, const Transform3<S>& tf2,
626  Plane<S>& pl,
627  Vector3<S>& p, Vector3<S>& d,
628  S& penetration_depth,
629  int& ret)
630 {
631  Plane<S> new_s1 = transform(s1, tf1);
632  Halfspace<S> new_s2 = transform(s2, tf2);
633 
634  ret = 0;
635 
636  Vector3<S> dir = (new_s1.n).cross(new_s2.n);
637  S dir_norm = dir.squaredNorm();
638  if(dir_norm < std::numeric_limits<S>::epsilon()) // parallel
639  {
640  if((new_s1.n).dot(new_s2.n) > 0)
641  {
642  if(new_s1.d < new_s2.d)
643  {
644  penetration_depth = new_s2.d - new_s1.d;
645  ret = 1;
646  pl = new_s1;
647  return true;
648  }
649  else
650  return false;
651  }
652  else
653  {
654  if(new_s1.d + new_s2.d > 0)
655  return false;
656  else
657  {
658  penetration_depth = -(new_s1.d + new_s2.d);
659  ret = 2;
660  pl = new_s1;
661  return true;
662  }
663  }
664  }
665 
666  Vector3<S> n = new_s2.n * new_s1.d - new_s1.n * new_s2.d;
667  Vector3<S> origin = n.cross(dir);
668  origin *= (1.0 / dir_norm);
669 
670  p = origin;
671  d = dir;
672  ret = 3;
673  penetration_depth = std::numeric_limits<S>::max();
674 
675  return true;
676 }
677 
678 //==============================================================================
679 template <typename S>
681  const Plane<S>& s2, const Transform3<S>& tf2,
682  Plane<S>& pl, Vector3<S>& p, Vector3<S>& d,
683  S& penetration_depth,
684  int& ret)
685 {
686  return planeHalfspaceIntersect(s2, tf2, s1, tf1, pl, p, d, penetration_depth, ret);
687 }
688 
689 //==============================================================================
690 template <typename S>
691 bool halfspaceIntersect(const Halfspace<S>& s1, const Transform3<S>& tf1,
692  const Halfspace<S>& s2, const Transform3<S>& tf2,
693  Vector3<S>& p, Vector3<S>& d,
694  Halfspace<S>& s,
695  S& penetration_depth,
696  int& ret)
697 {
698  Halfspace<S> new_s1 = transform(s1, tf1);
699  Halfspace<S> new_s2 = transform(s2, tf2);
700 
701  ret = 0;
702 
703  Vector3<S> dir = (new_s1.n).cross(new_s2.n);
704  S dir_norm = dir.squaredNorm();
705  if(dir_norm < std::numeric_limits<S>::epsilon()) // parallel
706  {
707  if((new_s1.n).dot(new_s2.n) > 0)
708  {
709  if(new_s1.d < new_s2.d) // s1 is inside s2
710  {
711  ret = 1;
712  penetration_depth = std::numeric_limits<S>::max();
713  s = new_s1;
714  }
715  else // s2 is inside s1
716  {
717  ret = 2;
718  penetration_depth = std::numeric_limits<S>::max();
719  s = new_s2;
720  }
721  return true;
722  }
723  else
724  {
725  if(new_s1.d + new_s2.d > 0) // not collision
726  return false;
727  else // in each other
728  {
729  ret = 3;
730  penetration_depth = -(new_s1.d + new_s2.d);
731  return true;
732  }
733  }
734  }
735 
736  Vector3<S> n = new_s2.n * new_s1.d - new_s1.n * new_s2.d;
737  Vector3<S> origin = n.cross(dir);
738  origin *= (1.0 / dir_norm);
739 
740  p = origin;
741  d = dir;
742  ret = 4;
743  penetration_depth = std::numeric_limits<S>::max();
744 
745  return true;
746 }
747 
748 } // namespace detail
749 } // namespace fcl
750 
751 #endif
fcl::Sphere< double >
template class FCL_EXPORT Sphere< double >
fcl::detail::halfspaceTriangleIntersect
template bool halfspaceTriangleIntersect(const Halfspace< double > &s1, const Transform3< double > &tf1, const Vector3< double > &P1, const Vector3< double > &P2, const Vector3< double > &P3, const Transform3< double > &tf2, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal)
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::Capsule::radius
S radius
Radius of capsule.
Definition: capsule.h:60
fcl::ContactPoint
Minimal contact information returned by collision.
Definition: contact_point.h:48
fcl::detail::cylinderHalfspaceIntersect
template bool cylinderHalfspaceIntersect(const Cylinder< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::detail::sphereHalfspaceIntersect
template bool sphereHalfspaceIntersect(const Sphere< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::Convex< double >
template class FCL_EXPORT Convex< double >
epsilon
S epsilon()
Definition: test_fcl_simple.cpp:56
fcl::Convex< S >
fcl::detail::planeHalfspaceIntersect
template bool planeHalfspaceIntersect(const Plane< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, Plane< double > &pl, Vector3< double > &p, Vector3< double > &d, double &penetration_depth, int &ret)
fcl::detail::capsuleHalfspaceIntersect
template bool capsuleHalfspaceIntersect(const Capsule< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::Ellipsoid::radii
Vector3< S > radii
Radii of the ellipsoid.
Definition: ellipsoid.h:63
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
fcl::Capsule::lz
S lz
Length along z axis.
Definition: capsule.h:63
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::convexHalfspaceIntersect
template bool convexHalfspaceIntersect(const Convex< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, Vector3< double > *contact_points, double *penetration_depth, Vector3< double > *normal)
fcl::Plane::n
Vector3< S > n
Plane normal.
Definition: geometry/shape/plane.h:75
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
halfspace.h
fcl::Plane< double >
template class FCL_EXPORT Plane< double >
fcl::detail::coneHalfspaceIntersect
template bool coneHalfspaceIntersect(const Cone< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::Cone::lz
S lz
Length along z axis.
Definition: cone.h:62
fcl::Convex::getVertices
const std::vector< Vector3< S > > & getVertices() const
Gets the vertex positions in the geometry's frame G.
Definition: convex.h:125
fcl::Halfspace< double >
template class FCL_EXPORT Halfspace< double >
fcl::detail::halfspaceIntersect
template bool halfspaceIntersect(const Halfspace< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, Vector3< double > &p, Vector3< double > &d, Halfspace< double > &s, double &penetration_depth, int &ret)
fcl::Halfspace
Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d....
Definition: geometry/shape/halfspace.h:59
fcl::Sphere::radius
S radius
Radius of the sphere.
Definition: sphere.h:59
fcl::detail::halfspacePlaneIntersect
template bool halfspacePlaneIntersect(const Halfspace< double > &s1, const Transform3< double > &tf1, const Plane< double > &s2, const Transform3< double > &tf2, Plane< double > &pl, Vector3< double > &p, Vector3< double > &d, double &penetration_depth, int &ret)
fcl::Halfspace::signedDistance
S signedDistance(const Vector3< S > &p) const
Definition: geometry/shape/halfspace-inl.h:79
fcl::Cone::radius
S radius
Radius of the cone.
Definition: cone.h:59
fcl::time::point
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: time.h:52
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:50
fcl::detail::boxHalfspaceIntersect
template bool boxHalfspaceIntersect(const Box< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2)
fcl::Plane::d
S d
Plane offset.
Definition: geometry/shape/plane.h:78
fcl::Halfspace::n
Vector3< S > n
Planed normal.
Definition: geometry/shape/halfspace.h:84
fcl::Halfspace::d
S d
Planed offset.
Definition: geometry/shape/halfspace.h:87
fcl::Ellipsoid< double >
template class FCL_EXPORT Ellipsoid< double >
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:50
fcl::Cylinder::lz
S lz
Length along z axis.
Definition: cylinder.h:63
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:50
fcl::Cylinder< double >
template class FCL_EXPORT Cylinder< double >
fcl::Capsule< double >
template class FCL_EXPORT Capsule< double >
min
static T min(T x, T y)
Definition: svm.cpp:49
fcl::Plane
Infinite plane.
Definition: geometry/shape/plane.h:50
fcl::Cone
Center at zero cone.
Definition: cone.h:50
fcl::Box< double >
template class FCL_EXPORT Box< double >
fcl::detail::halfspaceIntersectTolerance
S halfspaceIntersectTolerance()
Definition: narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h:148
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::Cone< double >
template class FCL_EXPORT Cone< double >
fcl::detail::ellipsoidHalfspaceIntersect
template bool ellipsoidHalfspaceIntersect(const Ellipsoid< double > &s1, const Transform3< double > &tf1, const Halfspace< double > &s2, const Transform3< double > &tf2, std::vector< ContactPoint< double >> *contacts)
fcl::Box::side
Vector3< S > side
box side length
Definition: box.h:66
fcl::Capsule< S >
fcl::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:50
fcl::Cylinder::radius
S radius
Radius of the cylinder.
Definition: cylinder.h:60


fcl
Author(s):
autogenerated on Fri Apr 2 2021 02:38:00