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::CachedGuess
@ CachedGuess
Definition: data_types.h:80
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::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
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::GJK
class for GJK algorithm
Definition: gjk.h:141
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::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::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::TriangleP::c
Vec3f c
Definition: shape/geometric_shapes.h:109
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
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:27
hpp::fcl::Sphere
Center at zero point sphere.
Definition: shape/geometric_shapes.h:196
gjk.P2
tuple P2
Definition: test/scripts/gjk.py:22
hpp::fcl::details::MinkowskiDiff::set
void set(const ShapeBase *shape0, const ShapeBase *shape1)
Definition: src/narrowphase/gjk.cpp:505
hpp::fcl::GJKSolver::gjk_initial_guess
GJKInitialGuess gjk_initial_guess
which warm start to use for GJK
Definition: narrowphase.h:502
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::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
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::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
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: shape/geometric_shapes.h:501
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::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
hpp::fcl::TriangleP::b
Vec3f b
Definition: shape/geometric_shapes.h:109
geometric_shapes_utility.h
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
narrowphase.h
distance_lower_bound
Definition: distance_lower_bound.py:1
hpp::fcl::details::MinkowskiDiff
Minkowski difference class of two shapes.
Definition: gjk.h:59
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,...
Definition: src/collision.cpp:63
res
res
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::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
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
hpp::fcl::details::GJK::Inside
@ Inside
Definition: gjk.h:165
hpp::fcl::details::GJK::Valid
@ Valid
Definition: gjk.h:165
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::GJKSolver::enable_cached_guess
bool enable_cached_guess
Whether smart guess can be provided @Deprecated Use gjk_initial_guess instead.
Definition: narrowphase.h:496
gjk.P1
tuple P1
Definition: test/scripts/gjk.py:21
hpp::fcl::details::GJK::Status
Status
Status of the GJK algorithm: Valid: GJK converged and the shapes are not in collision....
Definition: gjk.h:165
hpp::fcl::support_func_guess_t
Eigen::Vector2i support_func_guess_t
Definition: data_types.h:72
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
Main namespace.
Definition: broadphase_bruteforce.h:44
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
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
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
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: shape/geometric_shapes.h:414
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
octree.p1
tuple p1
Definition: octree.py:54
hpp::fcl::TriangleP::a
Vec3f a
Definition: shape/geometric_shapes.h:109
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::details::GJK::Failed
@ Failed
Definition: gjk.h:165
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::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::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::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
details.h
intersect.h
hpp::fcl::TriangleP
Triangle stores the points instead of only indices of points.
Definition: shape/geometric_shapes.h:71
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::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
gjk.P3
tuple P3
Definition: test/scripts/gjk.py:23
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::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_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::cached_guess
Vec3f cached_guess
smart guess
Definition: narrowphase.h:499
SHAPE_INTERSECT_INVERTED
#define SHAPE_INTERSECT_INVERTED(Shape1, Shape2)
Definition: narrowphase.cpp:81
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::Box
Center at zero point, axis aligned box.
Definition: shape/geometric_shapes.h:125
gjk
Definition: doc/gjk.py:1
hpp::fcl::Plane
Infinite plane.
Definition: shape/geometric_shapes.h:810

hpp-fcl
Author(s):
autogenerated on Fri Jan 26 2024 03:46:14