narrowphase.cpp
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-2015, 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 #include <hpp/fcl/narrowphase/narrowphase.h>
39 
40 #include <vector>
41 
42 #include <hpp/fcl/shape/geometric_shapes_utility.h>
43 #include <hpp/fcl/internal/intersect.h>
44 #include "details.h"
45 
46 namespace hpp {
47 namespace fcl {
48 // Shape intersect algorithms based on:
49 // - built-in function: 0
50 // - GJK: 1
51 //
52 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
53 // | | box | sphere | capsule | cone | cylinder | plane | half-space
54 // | triangle |
55 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
56 // | box | 0 | 0 | 1 | 1 | 1 | 0 | 0 | 1
57 // |
58 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
59 // | sphere |/////| 0 | 0 | 1 | 1 | 0 | 0 | 0
60 // |
61 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
62 // | capsule |/////|////////| 1 | 1 | 1 | 0 | 0 | 1
63 // |
64 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
65 // | cone |/////|////////|/////////| 1 | 1 | 0 | 0 | 1
66 // |
67 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
68 // | cylinder |/////|////////|/////////|//////| 1 | 0 | 0 | 1
69 // |
70 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
71 // | plane |/////|////////|/////////|//////|//////////| 0 | 0 | 0
72 // |
73 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
74 // | half-space |/////|////////|/////////|//////|//////////|///////| 0 | 0
75 // |
76 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
77 // | triangle |/////|////////|/////////|//////|//////////|///////|////////////|
78 // 1 |
79 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
80 
81 #define SHAPE_INTERSECT_INVERTED(Shape1, Shape2) \
82  template <> \
83  bool GJKSolver::shapeIntersect<Shape1, Shape2>( \
84  const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \
85  const Transform3f& tf2, FCL_REAL& distance_lower_bound, \
86  bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const { \
87  bool res = shapeIntersect(s2, tf2, s1, tf1, distance_lower_bound, \
88  enable_penetration, contact_points, normal); \
89  (*normal) *= -1.0; \
90  return res; \
91  }
92 
93 template <>
94 bool GJKSolver::shapeIntersect<Sphere, Capsule>(
95  const Sphere& s1, const Transform3f& tf1, const Capsule& s2,
96  const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool,
97  Vec3f* contact_points, Vec3f* normal) const {
98  return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, distance_lower_bound,
99  contact_points, normal);
100 }
101 
102 SHAPE_INTERSECT_INVERTED(Capsule, Sphere)
103 
104 template <>
105 bool GJKSolver::shapeIntersect<Sphere, Sphere>(
106  const Sphere& s1, const Transform3f& tf1, const Sphere& s2,
107  const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool,
108  Vec3f* contact_points, Vec3f* normal) const {
109  return details::sphereSphereIntersect(s1, tf1, s2, tf2, distance_lower_bound,
110  contact_points, normal);
111 }
112 
113 template <>
114 bool GJKSolver::shapeIntersect<Box, Sphere>(
115  const Box& s1, const Transform3f& tf1, const Sphere& s2,
116  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
117  Vec3f* normal) const {
118  Vec3f ps, pb, n;
119  bool res = details::boxSphereDistance(s1, tf1, s2, tf2, distance, ps, pb, n);
120  if (normal) *normal = n;
121  if (contact_points) *contact_points = pb;
122  return res;
123 }
124 
125 SHAPE_INTERSECT_INVERTED(Sphere, Box)
126 
127 /*
128 template<>
129 bool GJKSolver::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
130  const Box& s2, const Transform3f& tf2,
131  FCL_REAL& distance_lower_bound,
132  bool,
133  Vec3f* contact_points, Vec3f* normal)
134 const
135 {
136  return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points,
137 &distance_lower_bound, normal);
138 }
139 */
140 
141 template <>
142 bool GJKSolver::shapeIntersect<Sphere, Halfspace>(
143  const Sphere& s1, const Transform3f& tf1, const Halfspace& s2,
144  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
145  Vec3f* normal) const {
146  Vec3f p1, p2, n;
147  bool res =
148  details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
149  if (contact_points) *contact_points = p1;
150  if (normal) *normal = n;
151  return res;
152 }
153 
154 SHAPE_INTERSECT_INVERTED(Halfspace, Sphere)
155 
156 template <>
157 bool GJKSolver::shapeIntersect<Box, Halfspace>(
158  const Box& s1, const Transform3f& tf1, const Halfspace& s2,
159  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
160  Vec3f* normal) const {
161  Vec3f p1, p2, n;
162  bool res =
163  details::boxHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
164  if (contact_points) *contact_points = p1;
165  if (normal) *normal = n;
166  return res;
167 }
168 
169 SHAPE_INTERSECT_INVERTED(Halfspace, Box)
170 
171 template <>
172 bool GJKSolver::shapeIntersect<Capsule, Halfspace>(
173  const Capsule& s1, const Transform3f& tf1, const Halfspace& s2,
174  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
175  Vec3f* normal) const {
176  Vec3f p1, p2, n;
177  bool res =
178  details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
179  if (contact_points) *contact_points = p1;
180  if (normal) *normal = n;
181  return res;
182 }
183 
184 SHAPE_INTERSECT_INVERTED(Halfspace, Capsule)
185 
186 template <>
187 bool GJKSolver::shapeIntersect<Cylinder, Halfspace>(
188  const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2,
189  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
190  Vec3f* normal) const {
191  Vec3f p1, p2, n;
192  bool res = details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1,
193  p2, n);
194  if (contact_points) *contact_points = p1;
195  if (normal) *normal = n;
196  return res;
197 }
198 
199 SHAPE_INTERSECT_INVERTED(Halfspace, Cylinder)
200 
201 template <>
202 bool GJKSolver::shapeIntersect<Cone, Halfspace>(
203  const Cone& s1, const Transform3f& tf1, const Halfspace& s2,
204  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
205  Vec3f* normal) const {
206  Vec3f p1, p2, n;
207  bool res =
208  details::coneHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
209  if (contact_points) *contact_points = p1;
210  if (normal) *normal = n;
211  return res;
212 }
213 
214 SHAPE_INTERSECT_INVERTED(Halfspace, Cone)
215 
216 template <>
217 bool GJKSolver::shapeIntersect<Halfspace, Halfspace>(
218  const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2,
219  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* /*contact_points*/,
220  Vec3f* /*normal*/) const {
221  Halfspace s;
222  Vec3f p, d;
223  FCL_REAL depth;
224  int ret;
225  bool res = details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret);
226  distance = -depth;
227  return res;
228 }
229 
230 template <>
231 bool GJKSolver::shapeIntersect<Plane, Halfspace>(
232  const Plane& s1, const Transform3f& tf1, const Halfspace& s2,
233  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* /*contact_points*/,
234  Vec3f* /*normal*/) const {
235  Plane pl;
236  Vec3f p, d;
237  FCL_REAL depth;
238  int ret;
239  bool res =
240  details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
241  distance = -depth;
242  return res;
243 }
244 
245 SHAPE_INTERSECT_INVERTED(Halfspace, Plane)
246 
247 template <>
248 bool GJKSolver::shapeIntersect<Sphere, Plane>(
249  const Sphere& s1, const Transform3f& tf1, const Plane& s2,
250  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
251  Vec3f* normal) const {
252  Vec3f p1, p2, n;
253  bool res =
254  details::spherePlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
255  if (contact_points) *contact_points = p1;
256  if (normal) *normal = n;
257  return res;
258 }
259 
260 SHAPE_INTERSECT_INVERTED(Plane, Sphere)
261 
262 template <>
263 bool GJKSolver::shapeIntersect<Box, Plane>(
264  const Box& s1, const Transform3f& tf1, const Plane& s2,
265  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
266  Vec3f* normal) const {
267  Vec3f p1, p2, n;
268  bool res = details::boxPlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
269  if (contact_points) *contact_points = p1;
270  if (normal) *normal = n;
271  return res;
272 }
273 
274 SHAPE_INTERSECT_INVERTED(Plane, Box)
275 
276 template <>
277 bool GJKSolver::shapeIntersect<Capsule, Plane>(
278  const Capsule& s1, const Transform3f& tf1, const Plane& s2,
279  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
280  Vec3f* normal) const {
281  Vec3f p1, p2, n;
282  bool res =
283  details::capsulePlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
284  if (contact_points) *contact_points = p1;
285  if (normal) *normal = n;
286  return res;
287 }
288 
289 SHAPE_INTERSECT_INVERTED(Plane, Capsule)
290 
291 template <>
292 bool GJKSolver::shapeIntersect<Cylinder, Plane>(
293  const Cylinder& s1, const Transform3f& tf1, const Plane& s2,
294  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
295  Vec3f* normal) const {
296  Vec3f p1, p2, n;
297  bool res =
298  details::cylinderPlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
299  if (contact_points) *contact_points = p1;
300  if (normal) *normal = n;
301  return res;
302 }
303 
304 SHAPE_INTERSECT_INVERTED(Plane, Cylinder)
305 
306 template <>
307 bool GJKSolver::shapeIntersect<Cone, Plane>(
308  const Cone& s1, const Transform3f& tf1, const Plane& s2,
309  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
310  Vec3f* normal) const {
311  Vec3f p1, p2, n;
312  bool res = details::conePlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
313  if (contact_points) *contact_points = p1;
314  if (normal) *normal = n;
315  return res;
316 }
317 
318 SHAPE_INTERSECT_INVERTED(Plane, Cone)
319 
320 template <>
321 bool GJKSolver::shapeIntersect<Plane, Plane>(
322  const Plane& s1, const Transform3f& tf1, const Plane& s2,
323  const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
324  Vec3f* normal) const {
325  return details::planeIntersect(s1, tf1, s2, tf2, contact_points, &distance,
326  normal);
327 }
328 
329 template <>
330 bool GJKSolver::shapeTriangleInteraction(
331  const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
332  const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1,
333  Vec3f& p2, Vec3f& normal) const {
334  return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1),
335  tf2.transform(P2), tf2.transform(P3),
336  distance, p1, p2, normal);
337 }
338 
339 template <>
340 bool GJKSolver::shapeTriangleInteraction(
341  const Halfspace& s, const Transform3f& tf1, const Vec3f& P1,
342  const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
343  FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const {
344  return details::halfspaceTriangleIntersect(s, tf1, P1, P2, P3, tf2, distance,
345  p1, p2, normal);
346 }
347 
348 template <>
349 bool GJKSolver::shapeTriangleInteraction(const Plane& s, const Transform3f& tf1,
350  const Vec3f& P1, const Vec3f& P2,
351  const Vec3f& P3,
352  const Transform3f& tf2,
353  FCL_REAL& distance, Vec3f& p1,
354  Vec3f& p2, Vec3f& normal) const {
355  return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, distance, p1,
356  p2, normal);
357 }
358 
359 // Shape distance algorithms not using built-in GJK algorithm
360 //
361 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
362 // | | box | sphere | capsule | cone | cylinder | plane | half-space
363 // | triangle |
364 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
365 // | box | | O | | | | | | |
366 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
367 // | sphere |/////| O | O | | O | | | |
368 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
369 // | capsule |/////|////////| O | | | | | |
370 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
371 // | cone |/////|////////|/////////| | | | | |
372 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
373 // | cylinder |/////|////////|/////////|//////| | | | |
374 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
375 // | plane |/////|////////|/////////|//////|//////////| | | |
376 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
377 // | half-space |/////|////////|/////////|//////|//////////|///////| | |
378 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
379 // | triangle |/////|////////|/////////|//////|//////////|///////|////////////|
380 // |
381 // +------------+-----+--------+---------+------+----------+-------+------------+----------+
382 
383 template <>
384 bool GJKSolver::shapeDistance<Sphere, Capsule>(const Sphere& s1,
385  const Transform3f& tf1,
386  const Capsule& s2,
387  const Transform3f& tf2,
388  FCL_REAL& dist, Vec3f& p1,
389  Vec3f& p2, Vec3f& normal) const {
390  return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
391 }
392 
393 template <>
394 bool GJKSolver::shapeDistance<Capsule, Sphere>(const Capsule& s1,
395  const Transform3f& tf1,
396  const Sphere& s2,
397  const Transform3f& tf2,
398  FCL_REAL& dist, Vec3f& p1,
399  Vec3f& p2, Vec3f& normal) const {
400  return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1, normal);
401 }
402 
403 template <>
404 bool GJKSolver::shapeDistance<Box, Sphere>(const Box& s1,
405  const Transform3f& tf1,
406  const Sphere& s2,
407  const Transform3f& tf2,
408  FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
409  Vec3f& normal) const {
410  return !details::boxSphereDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
411 }
412 
413 template <>
414 bool GJKSolver::shapeDistance<Sphere, Box>(const Sphere& s1,
415  const Transform3f& tf1,
416  const Box& s2,
417  const Transform3f& tf2,
418  FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
419  Vec3f& normal) const {
420  bool collide =
421  details::boxSphereDistance(s2, tf2, s1, tf1, dist, p2, p1, normal);
422  normal *= -1;
423  return !collide;
424 }
425 
426 template <>
427 bool GJKSolver::shapeDistance<Sphere, Cylinder>(
428  const Sphere& s1, const Transform3f& tf1, const Cylinder& s2,
429  const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
430  Vec3f& normal) const {
431  return details::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2,
432  normal);
433 }
434 
435 template <>
436 bool GJKSolver::shapeDistance<Cylinder, Sphere>(
437  const Cylinder& s1, const Transform3f& tf1, const Sphere& s2,
438  const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
439  Vec3f& normal) const {
440  return details::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1,
441  normal);
442 }
443 
444 template <>
445 bool GJKSolver::shapeDistance<Sphere, Sphere>(const Sphere& s1,
446  const Transform3f& tf1,
447  const Sphere& s2,
448  const Transform3f& tf2,
449  FCL_REAL& dist, Vec3f& p1,
450  Vec3f& p2, Vec3f& normal) const {
451  return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
452 }
453 
454 template <>
455 bool GJKSolver::shapeDistance<Capsule, Capsule>(
456  const Capsule& /*s1*/, const Transform3f& /*tf1*/, const Capsule& /*s2*/,
457  const Transform3f& /*tf2*/, FCL_REAL& /*dist*/, Vec3f& /*p1*/,
458  Vec3f& /*p2*/, Vec3f& /*normal*/) const {
459  abort();
460 }
461 
462 template <>
463 bool GJKSolver::shapeDistance<TriangleP, TriangleP>(
464  const TriangleP& s1, const Transform3f& tf1, const TriangleP& s2,
465  const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
466  Vec3f& normal) const {
467  const TriangleP t1(tf1.transform(s1.a), tf1.transform(s1.b),
468  tf1.transform(s1.c)),
469  t2(tf2.transform(s2.a), tf2.transform(s2.b), tf2.transform(s2.c));
470 
471  details::MinkowskiDiff shape;
472  shape.set(&t1, &t2);
473 
474  Vec3f guess;
475  guess = (t1.a + t1.b + t1.c - t2.a - t2.b - t2.c) / 3;
476  support_func_guess_t support_hint;
477  bool enable_penetration = true;
478  details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
479  initialize_gjk(gjk, shape, t1, t2, guess, support_hint);
480 
481  details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
482  if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
483  enable_cached_guess) {
484  cached_guess = gjk.getGuessFromSimplex();
485  support_func_cached_guess = gjk.support_hint;
486  }
487 
488  gjk.getClosestPoints(shape, p1, p2);
489 
490  if ((gjk_status == details::GJK::Valid) ||
491  (gjk_status == details::GJK::Failed)) {
492  // TODO On degenerated case, the closest point may be wrong
493  // (i.e. an object face normal is colinear to gjk.ray
494  // assert (dist == (w0 - w1).norm());
495  dist = gjk.distance;
496 
497  return true;
498  } else if (gjk_status == details::GJK::Inside) {
499  if (enable_penetration) {
500  FCL_REAL penetrationDepth = details::computePenetration(
501  t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal);
502  dist = -penetrationDepth;
503  assert(dist <= 1e-6);
504  // GJK says Inside when below GJK.tolerance. So non intersecting
505  // triangle may trigger "Inside" and have no penetration.
506  return penetrationDepth < 0;
507  }
508  dist = 0;
509  return false;
510  }
511  assert(false && "should not reach this point");
512  return false;
513 }
514 } // namespace fcl
515 
516 } // namespace hpp
hpp::fcl::GJKSolver::initialize_gjk
void initialize_gjk(details::GJK &gjk, const details::MinkowskiDiff &shape, const S1 &s1, const S2 &s2, Vec3f &guess, support_func_guess_t &support_hint) const
initialize GJK
Definition: narrowphase.h:59
hpp::fcl::details::planeIntersect
bool planeIntersect(const Plane &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, Vec3f *, FCL_REAL *, Vec3f *)
Definition: details.h:2462
hpp::fcl::details::capsulePlaneIntersect
bool capsulePlaneIntersect(const Capsule &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:2009
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::details::sphereCapsuleDistance
bool sphereCapsuleDistance(const Sphere &s1, const Transform3f &tf1, const Capsule &s2, const Transform3f &tf2, FCL_REAL &dist, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:101
gjk.P1
tuple P1
Definition: test/scripts/gjk.py:22
hpp::fcl::details::GJK::support_hint
support_func_guess_t support_hint
Definition: gjk.h:172
octree.p1
tuple p1
Definition: octree.py:55
hpp::fcl::details::halfspaceTriangleIntersect
bool halfspaceTriangleIntersect(const Halfspace &s1, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1652
SHAPE_INTERSECT_INVERTED
#define SHAPE_INTERSECT_INVERTED(Shape1, Shape2)
Definition: narrowphase.cpp:81
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::details::conePlaneIntersect
bool conePlaneIntersect(const Cone &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:2185
hpp::fcl::GJKSolver::enable_cached_guess
bool enable_cached_guess
Whether smart guess can be provided Use gjk_initial_guess instead.
Definition: narrowphase.h:496
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:37
hpp::fcl::details::cylinderHalfspaceIntersect
bool cylinderHalfspaceIntersect(const Cylinder &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1513
hpp::fcl::details::boxPlaneIntersect
bool boxPlaneIntersect(const Box &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d so (R^T n) (a v1 + ...
Definition: details.h:1879
hpp::fcl::Halfspace
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: shape/geometric_shapes.h:729
hpp::fcl::details::sphereCapsuleIntersect
bool sphereCapsuleIntersect(const Sphere &s1, const Transform3f &tf1, const Capsule &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f *contact_points, Vec3f *normal_)
Definition: details.h:71
hpp::fcl::details::boxHalfspaceIntersect
bool boxHalfspaceIntersect(const Box &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2)
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) <= d so (R^T n) (a v1 +...
Definition: details.h:1404
hpp::fcl::details::MinkowskiDiff::set
void set(const ShapeBase *shape0, const ShapeBase *shape1)
Definition: src/narrowphase/gjk.cpp:505
gjk.P3
tuple P3
Definition: test/scripts/gjk.py:24
hpp::fcl::details::cylinderPlaneIntersect
bool cylinderPlaneIntersect(const Cylinder &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
cylinder-plane intersect n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d need one point to b...
Definition: details.h:2109
hpp::fcl::details::GJK
class for GJK algorithm
Definition: gjk.h:141
distance_lower_bound
Definition: distance_lower_bound.py:1
hpp::fcl::details::sphereHalfspaceIntersect
bool sphereHalfspaceIntersect(const Sphere &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1380
hpp::fcl::details::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: gjk.h:59
hpp::fcl::details::sphereCylinderDistance
bool sphereCylinderDistance(const Sphere &s1, const Transform3f &tf1, const Cylinder &s2, const Transform3f &tf2, FCL_REAL &dist, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:132
hpp::fcl::details::GJK::getClosestPoints
bool getClosestPoints(const MinkowskiDiff &shape, Vec3f &w0, Vec3f &w1)
Definition: src/narrowphase/gjk.cpp:617
hpp::fcl::details::GJK::getGuessFromSimplex
Vec3f getGuessFromSimplex() const
get the guess from current simplex
Definition: src/narrowphase/gjk.cpp:528
hpp::fcl::distance
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS.cpp:181
hpp::fcl::Transform3f::transform
Vec3f transform(const Eigen::MatrixBase< Derived > &v) const
transform a given vector by the transform
Definition: transform.h:153
hpp::fcl::details::coneHalfspaceIntersect
bool coneHalfspaceIntersect(const Cone &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1566
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
hpp::fcl::details::halfspaceIntersect
bool halfspaceIntersect(const Halfspace &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, Vec3f &p, Vec3f &d, Halfspace &s, FCL_REAL &penetration_depth, int &ret)
Definition: details.h:1756
hpp::fcl::details::capsuleHalfspaceIntersect
bool capsuleHalfspaceIntersect(const Capsule &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1466
hpp::fcl::details::GJK::Status
Status
Status of the GJK algorithm: Valid: GJK converged and the shapes are not in collision. Inside: GJK converged and the shapes are in collision. Failed: GJK did not converge.
Definition: gjk.h:165
hpp::fcl::details::sphereSphereDistance
bool sphereSphereDistance(const Sphere &s1, const Transform3f &tf1, const Sphere &s2, const Transform3f &tf2, FCL_REAL &dist, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:258
hpp::fcl::TriangleP
Triangle stores the points instead of only indices of points.
Definition: shape/geometric_shapes.h:71
hpp::fcl::details::sphereSphereIntersect
bool sphereSphereIntersect(const Sphere &s1, const Transform3f &tf1, const Sphere &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f *contact_points, Vec3f *normal)
Definition: details.h:232
geometric_shapes_utility.h
hpp::fcl::support_func_guess_t
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:72
hpp::fcl::details::GJK::distance
FCL_REAL distance
Definition: gjk.h:186
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: shape/geometric_shapes.h:414
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB...
Definition: shape/geometric_shapes.h:333
hpp::fcl::GJKSolver::cached_guess
Vec3f cached_guess
smart guess
Definition: narrowphase.h:499
hpp::fcl::details::spherePlaneIntersect
bool spherePlaneIntersect(const Sphere &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:1840
gjk
Definition: doc/gjk.py:1
hpp::fcl::GJKSolver::shapeTriangleInteraction
bool shapeTriangleInteraction(const S &s, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal) const
intersection checking between one shape and a triangle with transformation
Definition: narrowphase.h:178
res
res
hpp::fcl::collide
HPP_FCL_DLLAPI std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
Definition: src/collision.cpp:63
gjk.P2
tuple P2
Definition: test/scripts/gjk.py:23
hpp::fcl::details::planeHalfspaceIntersect
bool planeHalfspaceIntersect(const Plane &s1, const Transform3f &tf1, const Halfspace &s2, const Transform3f &tf2, Plane &pl, Vec3f &p, Vec3f &d, FCL_REAL &penetration_depth, int &ret)
return whether plane collides with halfspace if the separation plane of the halfspace is parallel wit...
Definition: details.h:1700
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::details::sphereTriangleIntersect
bool sphereTriangleIntersect(const Sphere &s, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal_)
Definition: details.h:324
hpp::fcl::details::planeTriangleIntersect
bool planeTriangleIntersect(const Plane &s1, const Transform3f &tf1, const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
Definition: details.h:2339
hpp::fcl::details::boxSphereDistance
bool boxSphereDistance(const Box &b, const Transform3f &tfb, const Sphere &s, const Transform3f &tfs, FCL_REAL &dist, Vec3f &pb, Vec3f &ps, Vec3f &normal)
Definition: details.h:1957
hpp::fcl::GJKSolver::gjk_tolerance
FCL_REAL gjk_tolerance
the threshold used in GJK to stop iteration
Definition: narrowphase.h:488
hpp::fcl::GJKSolver::support_func_cached_guess
support_func_guess_t support_func_cached_guess
smart guess for the support function
Definition: narrowphase.h:514
hpp::fcl::GJKSolver::gjk_max_iterations
size_t gjk_max_iterations
maximum number of iterations used for GJK iterations
Definition: narrowphase.h:491
hpp::fcl::details::computePenetration
FCL_REAL computePenetration(const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Vec3f &Q1, const Vec3f &Q2, const Vec3f &Q3, Vec3f &normal)
See the prototype below.
Definition: details.h:2477
hpp::fcl::GJKSolver::gjk_initial_guess
GJKInitialGuess gjk_initial_guess
which warm start to use for GJK
Definition: narrowphase.h:502
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:28
hpp::fcl::details::GJK::evaluate
Status evaluate(const MinkowskiDiff &shape, const Vec3f &guess, const support_func_guess_t &supportHint=support_func_guess_t::Zero())
GJK algorithm, given the initial value guess.
Definition: src/narrowphase/gjk.cpp:624

hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:01