distance_func_matrix.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/distance_func_matrix.h>
39 
40 #include <../src/collision_node.h>
41 #include <hpp/fcl/internal/shape_shape_func.h>
42 #include <hpp/fcl/internal/traversal_node_setup.h>
43 #include <../src/traits_traversal.h>
44 
45 namespace hpp {
46 namespace fcl {
47 
48 #ifdef HPP_FCL_HAS_OCTOMAP
49 
50 template <typename TypeA, typename TypeB>
51 FCL_REAL Distance(const CollisionGeometry* o1, const Transform3f& tf1,
52  const CollisionGeometry* o2, const Transform3f& tf2,
53  const GJKSolver* nsolver, const DistanceRequest& request,
54  DistanceResult& result) {
55  if (request.isSatisfied(result)) return result.min_distance;
56  typename TraversalTraitsDistance<TypeA, TypeB>::CollisionTraversal_t node;
57  const TypeA* obj1 = static_cast<const TypeA*>(o1);
58  const TypeB* obj2 = static_cast<const TypeB*>(o2);
59  OcTreeSolver otsolver(nsolver);
60 
61  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
62  distance(&node);
63 
64  return result.min_distance;
65 }
66 
67 #endif
68 
69 template <typename T_SH1, typename T_SH2>
70 FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,
71  const CollisionGeometry* o2, const Transform3f& tf2,
72  const GJKSolver* nsolver,
73  const DistanceRequest& request,
74  DistanceResult& result) {
75  if (request.isSatisfied(result)) return result.min_distance;
76  ShapeDistanceTraversalNode<T_SH1, T_SH2> node;
77  const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
78  const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
79 
80  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
81  distance(&node);
82 
83  return result.min_distance;
84 }
85 
86 template <typename T_BVH, typename T_SH>
87 struct HPP_FCL_LOCAL BVHShapeDistancer {
88  static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
89  const CollisionGeometry* o2, const Transform3f& tf2,
90  const GJKSolver* nsolver,
91  const DistanceRequest& request,
92  DistanceResult& result) {
93  if (request.isSatisfied(result)) return result.min_distance;
94  MeshShapeDistanceTraversalNode<T_BVH, T_SH> node;
95  const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
96  BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
97  Transform3f tf1_tmp = tf1;
98  const T_SH* obj2 = static_cast<const T_SH*>(o2);
99 
100  initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
101  fcl::distance(&node);
102 
103  delete obj1_tmp;
104  return result.min_distance;
105  }
106 };
107 
108 namespace details {
109 
110 template <typename OrientedMeshShapeDistanceTraversalNode, typename T_BVH,
111  typename T_SH>
112 FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1,
113  const Transform3f& tf1,
114  const CollisionGeometry* o2,
115  const Transform3f& tf2,
116  const GJKSolver* nsolver,
117  const DistanceRequest& request,
118  DistanceResult& result) {
119  if (request.isSatisfied(result)) return result.min_distance;
120  OrientedMeshShapeDistanceTraversalNode node;
121  const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
122  const T_SH* obj2 = static_cast<const T_SH*>(o2);
123 
124  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
125  fcl::distance(&node);
126 
127  return result.min_distance;
128 }
129 
130 } // namespace details
131 
132 template <typename T_SH>
133 struct HPP_FCL_LOCAL BVHShapeDistancer<RSS, T_SH> {
134  static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
135  const CollisionGeometry* o2, const Transform3f& tf2,
136  const GJKSolver* nsolver,
137  const DistanceRequest& request,
138  DistanceResult& result) {
139  return details::orientedBVHShapeDistance<
140  MeshShapeDistanceTraversalNodeRSS<T_SH>, RSS, T_SH>(
141  o1, tf1, o2, tf2, nsolver, request, result);
142  }
143 };
144 
145 template <typename T_SH>
146 struct HPP_FCL_LOCAL BVHShapeDistancer<kIOS, T_SH> {
147  static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
148  const CollisionGeometry* o2, const Transform3f& tf2,
149  const GJKSolver* nsolver,
150  const DistanceRequest& request,
151  DistanceResult& result) {
152  return details::orientedBVHShapeDistance<
153  MeshShapeDistanceTraversalNodekIOS<T_SH>, kIOS, T_SH>(
154  o1, tf1, o2, tf2, nsolver, request, result);
155  }
156 };
157 
158 template <typename T_SH>
159 struct HPP_FCL_LOCAL BVHShapeDistancer<OBBRSS, T_SH> {
160  static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
161  const CollisionGeometry* o2, const Transform3f& tf2,
162  const GJKSolver* nsolver,
163  const DistanceRequest& request,
164  DistanceResult& result) {
165  return details::orientedBVHShapeDistance<
166  MeshShapeDistanceTraversalNodeOBBRSS<T_SH>, OBBRSS, T_SH>(
167  o1, tf1, o2, tf2, nsolver, request, result);
168  }
169 };
170 
171 template <typename T_HF, typename T_SH>
172 struct HPP_FCL_LOCAL HeightFieldShapeDistancer {
173  static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
174  const CollisionGeometry* o2, const Transform3f& tf2,
175  const GJKSolver* nsolver,
176  const DistanceRequest& request,
177  DistanceResult& result) {
178  HPP_FCL_UNUSED_VARIABLE(o1);
179  HPP_FCL_UNUSED_VARIABLE(tf1);
180  HPP_FCL_UNUSED_VARIABLE(o2);
181  HPP_FCL_UNUSED_VARIABLE(tf2);
182  HPP_FCL_UNUSED_VARIABLE(nsolver);
183  HPP_FCL_UNUSED_VARIABLE(request);
184  // TODO(jcarpent)
185  HPP_FCL_THROW_PRETTY(
186  "Distance between a height field and a shape is not implemented",
187  std::invalid_argument);
188  // if(request.isSatisfied(result)) return result.min_distance;
189  // HeightFieldShapeDistanceTraversalNode<T_HF, T_SH> node;
190  //
191  // const HeightField<T_HF>* obj1 = static_cast<const HeightField<T_HF>*
192  // >(o1); const T_SH* obj2 = static_cast<const T_SH*>(o2);
193  //
194  // initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
195  // fcl::distance(&node);
196 
197  return result.min_distance;
198  }
199 };
200 
201 template <typename T_BVH>
202 FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1,
203  const CollisionGeometry* o2, const Transform3f& tf2,
204  const DistanceRequest& request, DistanceResult& result) {
205  if (request.isSatisfied(result)) return result.min_distance;
206  MeshDistanceTraversalNode<T_BVH> node;
207  const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
208  const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
209  BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
210  Transform3f tf1_tmp = tf1;
211  BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
212  Transform3f tf2_tmp = tf2;
213 
214  initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
215  distance(&node);
216  delete obj1_tmp;
217  delete obj2_tmp;
218 
219  return result.min_distance;
220 }
221 
222 namespace details {
223 template <typename OrientedMeshDistanceTraversalNode, typename T_BVH>
224 FCL_REAL orientedMeshDistance(const CollisionGeometry* o1,
225  const Transform3f& tf1,
226  const CollisionGeometry* o2,
227  const Transform3f& tf2,
228  const DistanceRequest& request,
229  DistanceResult& result) {
230  if (request.isSatisfied(result)) return result.min_distance;
231  OrientedMeshDistanceTraversalNode node;
232  const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
233  const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
234 
235  initialize(node, *obj1, tf1, *obj2, tf2, request, result);
236  distance(&node);
237 
238  return result.min_distance;
239 }
240 
241 } // namespace details
242 
243 template <>
244 FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const Transform3f& tf1,
245  const CollisionGeometry* o2, const Transform3f& tf2,
246  const DistanceRequest& request,
247  DistanceResult& result) {
248  return details::orientedMeshDistance<MeshDistanceTraversalNodeRSS, RSS>(
249  o1, tf1, o2, tf2, request, result);
250 }
251 
252 template <>
253 FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1,
254  const CollisionGeometry* o2, const Transform3f& tf2,
255  const DistanceRequest& request,
256  DistanceResult& result) {
257  return details::orientedMeshDistance<MeshDistanceTraversalNodekIOS, kIOS>(
258  o1, tf1, o2, tf2, request, result);
259 }
260 
261 template <>
262 FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1,
263  const Transform3f& tf1,
264  const CollisionGeometry* o2,
265  const Transform3f& tf2,
266  const DistanceRequest& request,
267  DistanceResult& result) {
268  return details::orientedMeshDistance<MeshDistanceTraversalNodeOBBRSS, OBBRSS>(
269  o1, tf1, o2, tf2, request, result);
270 }
271 
272 template <typename T_BVH>
273 FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1,
274  const CollisionGeometry* o2, const Transform3f& tf2,
275  const GJKSolver* /*nsolver*/,
276  const DistanceRequest& request, DistanceResult& result) {
277  return BVHDistance<T_BVH>(o1, tf1, o2, tf2, request, result);
278 }
279 
280 DistanceFunctionMatrix::DistanceFunctionMatrix() {
281  for (int i = 0; i < NODE_COUNT; ++i) {
282  for (int j = 0; j < NODE_COUNT; ++j) distance_matrix[i][j] = NULL;
283  }
284 
285  distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance<Box, Box>;
286  distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance<Box, Sphere>;
287  distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance<Box, Capsule>;
288  distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance<Box, Cone>;
289  distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance<Box, Cylinder>;
290  distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance<Box, ConvexBase>;
291  distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance<Box, Plane>;
292  distance_matrix[GEOM_BOX][GEOM_HALFSPACE] =
293  &ShapeShapeDistance<Box, Halfspace>;
294  distance_matrix[GEOM_BOX][GEOM_ELLIPSOID] =
295  &ShapeShapeDistance<Box, Ellipsoid>;
296 
297  distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance<Sphere, Box>;
298  distance_matrix[GEOM_SPHERE][GEOM_SPHERE] =
299  &ShapeShapeDistance<Sphere, Sphere>;
300  distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] =
301  &ShapeShapeDistance<Sphere, Capsule>;
302  distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance<Sphere, Cone>;
303  distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] =
304  &ShapeShapeDistance<Sphere, Cylinder>;
305  distance_matrix[GEOM_SPHERE][GEOM_CONVEX] =
306  &ShapeShapeDistance<Sphere, ConvexBase>;
307  distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance<Sphere, Plane>;
308  distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] =
309  &ShapeShapeDistance<Sphere, Halfspace>;
310  distance_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] =
311  &ShapeShapeDistance<Sphere, Ellipsoid>;
312 
313  distance_matrix[GEOM_ELLIPSOID][GEOM_BOX] =
314  &ShapeShapeDistance<Ellipsoid, Box>;
315  distance_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] =
316  &ShapeShapeDistance<Ellipsoid, Sphere>;
317  distance_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] =
318  &ShapeShapeDistance<Ellipsoid, Capsule>;
319  distance_matrix[GEOM_ELLIPSOID][GEOM_CONE] =
320  &ShapeShapeDistance<Ellipsoid, Cone>;
321  distance_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] =
322  &ShapeShapeDistance<Ellipsoid, Cylinder>;
323  distance_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] =
324  &ShapeShapeDistance<Ellipsoid, ConvexBase>;
325  // TODO Louis: Ellipsoid - Plane
326  // TODO Louis: Ellipsoid - Halfspace
327  distance_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] =
328  &ShapeShapeDistance<Ellipsoid, Ellipsoid>;
329 
330  distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance<Capsule, Box>;
331  distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] =
332  &ShapeShapeDistance<Capsule, Sphere>;
333  distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] =
334  &ShapeShapeDistance<Capsule, Capsule>;
335  distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance<Capsule, Cone>;
336  distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] =
337  &ShapeShapeDistance<Capsule, Cylinder>;
338  distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] =
339  &ShapeShapeDistance<Capsule, ConvexBase>;
340  distance_matrix[GEOM_CAPSULE][GEOM_PLANE] =
341  &ShapeShapeDistance<Capsule, Plane>;
342  distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] =
343  &ShapeShapeDistance<Capsule, Halfspace>;
344  distance_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] =
345  &ShapeShapeDistance<Capsule, Ellipsoid>;
346 
347  distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance<Cone, Box>;
348  distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance<Cone, Sphere>;
349  distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance<Cone, Capsule>;
350  distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance<Cone, Cone>;
351  distance_matrix[GEOM_CONE][GEOM_CYLINDER] =
352  &ShapeShapeDistance<Cone, Cylinder>;
353  distance_matrix[GEOM_CONE][GEOM_CONVEX] =
354  &ShapeShapeDistance<Cone, ConvexBase>;
355  distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance<Cone, Plane>;
356  distance_matrix[GEOM_CONE][GEOM_HALFSPACE] =
357  &ShapeShapeDistance<Cone, Halfspace>;
358  distance_matrix[GEOM_CONE][GEOM_ELLIPSOID] =
359  &ShapeShapeDistance<Cone, Ellipsoid>;
360 
361  distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance<Cylinder, Box>;
362  distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] =
363  &ShapeShapeDistance<Cylinder, Sphere>;
364  distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] =
365  &ShapeShapeDistance<Cylinder, Capsule>;
366  distance_matrix[GEOM_CYLINDER][GEOM_CONE] =
367  &ShapeShapeDistance<Cylinder, Cone>;
368  distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] =
369  &ShapeShapeDistance<Cylinder, Cylinder>;
370  distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] =
371  &ShapeShapeDistance<Cylinder, ConvexBase>;
372  distance_matrix[GEOM_CYLINDER][GEOM_PLANE] =
373  &ShapeShapeDistance<Cylinder, Plane>;
374  distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] =
375  &ShapeShapeDistance<Cylinder, Halfspace>;
376  distance_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] =
377  &ShapeShapeDistance<Cylinder, Ellipsoid>;
378 
379  distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance<ConvexBase, Box>;
380  distance_matrix[GEOM_CONVEX][GEOM_SPHERE] =
381  &ShapeShapeDistance<ConvexBase, Sphere>;
382  distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] =
383  &ShapeShapeDistance<ConvexBase, Capsule>;
384  distance_matrix[GEOM_CONVEX][GEOM_CONE] =
385  &ShapeShapeDistance<ConvexBase, Cone>;
386  distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] =
387  &ShapeShapeDistance<ConvexBase, Cylinder>;
388  distance_matrix[GEOM_CONVEX][GEOM_CONVEX] =
389  &ShapeShapeDistance<ConvexBase, ConvexBase>;
390  distance_matrix[GEOM_CONVEX][GEOM_PLANE] =
391  &ShapeShapeDistance<ConvexBase, Plane>;
392  distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] =
393  &ShapeShapeDistance<ConvexBase, Halfspace>;
394  distance_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] =
395  &ShapeShapeDistance<ConvexBase, Ellipsoid>;
396 
397  distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance<Plane, Box>;
398  distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance<Plane, Sphere>;
399  distance_matrix[GEOM_PLANE][GEOM_CAPSULE] =
400  &ShapeShapeDistance<Plane, Capsule>;
401  distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance<Plane, Cone>;
402  distance_matrix[GEOM_PLANE][GEOM_CYLINDER] =
403  &ShapeShapeDistance<Plane, Cylinder>;
404  distance_matrix[GEOM_PLANE][GEOM_CONVEX] =
405  &ShapeShapeDistance<Plane, ConvexBase>;
406  distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane, Plane>;
407  distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] =
408  &ShapeShapeDistance<Plane, Halfspace>;
409  // TODO Louis: Ellipsoid - Plane
410 
411  distance_matrix[GEOM_HALFSPACE][GEOM_BOX] =
412  &ShapeShapeDistance<Halfspace, Box>;
413  distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] =
414  &ShapeShapeDistance<Halfspace, Sphere>;
415  distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] =
416  &ShapeShapeDistance<Halfspace, Capsule>;
417  distance_matrix[GEOM_HALFSPACE][GEOM_CONE] =
418  &ShapeShapeDistance<Halfspace, Cone>;
419  distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] =
420  &ShapeShapeDistance<Halfspace, Cylinder>;
421  distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] =
422  &ShapeShapeDistance<Halfspace, ConvexBase>;
423  distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] =
424  &ShapeShapeDistance<Halfspace, Plane>;
425  distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] =
426  &ShapeShapeDistance<Halfspace, Halfspace>;
427  // TODO Louis: Ellipsoid - Halfspace
428 
429  /* AABB distance not implemented */
430  /*
431  distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer<AABB, Box>::distance;
432  distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer<AABB,
433  Sphere>::distance; distance_matrix[BV_AABB][GEOM_CAPSULE] =
434  &BVHShapeDistancer<AABB, Capsule>::distance;
435  distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer<AABB,
436  Cone>::distance; distance_matrix[BV_AABB][GEOM_CYLINDER] =
437  &BVHShapeDistancer<AABB, Cylinder>::distance;
438  distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB,
439  ConvexBase>::distance; distance_matrix[BV_AABB][GEOM_PLANE] =
440  &BVHShapeDistancer<AABB, Plane>::distance;
441  distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB,
442  Halfspace>::distance;
443  */
444 
445  distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box>::distance;
446  distance_matrix[BV_OBB][GEOM_SPHERE] =
447  &BVHShapeDistancer<OBB, Sphere>::distance;
448  distance_matrix[BV_OBB][GEOM_CAPSULE] =
449  &BVHShapeDistancer<OBB, Capsule>::distance;
450  distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer<OBB, Cone>::distance;
451  distance_matrix[BV_OBB][GEOM_CYLINDER] =
452  &BVHShapeDistancer<OBB, Cylinder>::distance;
453  distance_matrix[BV_OBB][GEOM_CONVEX] =
454  &BVHShapeDistancer<OBB, ConvexBase>::distance;
455  distance_matrix[BV_OBB][GEOM_PLANE] =
456  &BVHShapeDistancer<OBB, Plane>::distance;
457  distance_matrix[BV_OBB][GEOM_HALFSPACE] =
458  &BVHShapeDistancer<OBB, Halfspace>::distance;
459  distance_matrix[BV_OBB][GEOM_ELLIPSOID] =
460  &BVHShapeDistancer<OBB, Ellipsoid>::distance;
461 
462  distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box>::distance;
463  distance_matrix[BV_RSS][GEOM_SPHERE] =
464  &BVHShapeDistancer<RSS, Sphere>::distance;
465  distance_matrix[BV_RSS][GEOM_CAPSULE] =
466  &BVHShapeDistancer<RSS, Capsule>::distance;
467  distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer<RSS, Cone>::distance;
468  distance_matrix[BV_RSS][GEOM_CYLINDER] =
469  &BVHShapeDistancer<RSS, Cylinder>::distance;
470  distance_matrix[BV_RSS][GEOM_CONVEX] =
471  &BVHShapeDistancer<RSS, ConvexBase>::distance;
472  distance_matrix[BV_RSS][GEOM_PLANE] =
473  &BVHShapeDistancer<RSS, Plane>::distance;
474  distance_matrix[BV_RSS][GEOM_HALFSPACE] =
475  &BVHShapeDistancer<RSS, Halfspace>::distance;
476  distance_matrix[BV_RSS][GEOM_ELLIPSOID] =
477  &BVHShapeDistancer<RSS, Ellipsoid>::distance;
478 
479  /* KDOP distance not implemented */
480  /*
481  distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer<KDOP<16>,
482  Box>::distance; distance_matrix[BV_KDOP16][GEOM_SPHERE] =
483  &BVHShapeDistancer<KDOP<16>, Sphere>::distance;
484  distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<16>,
485  Capsule>::distance; distance_matrix[BV_KDOP16][GEOM_CONE] =
486  &BVHShapeDistancer<KDOP<16>, Cone>::distance;
487  distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<16>,
488  Cylinder>::distance; distance_matrix[BV_KDOP16][GEOM_CONVEX] =
489  &BVHShapeDistancer<KDOP<16>, ConvexBase>::distance;
490  distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer<KDOP<16>,
491  Plane>::distance; distance_matrix[BV_KDOP16][GEOM_HALFSPACE] =
492  &BVHShapeDistancer<KDOP<16>, Halfspace>::distance;
493 
494  distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer<KDOP<18>,
495  Box>::distance; distance_matrix[BV_KDOP18][GEOM_SPHERE] =
496  &BVHShapeDistancer<KDOP<18>, Sphere>::distance;
497  distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<18>,
498  Capsule>::distance; distance_matrix[BV_KDOP18][GEOM_CONE] =
499  &BVHShapeDistancer<KDOP<18>, Cone>::distance;
500  distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<18>,
501  Cylinder>::distance; distance_matrix[BV_KDOP18][GEOM_CONVEX] =
502  &BVHShapeDistancer<KDOP<18>, ConvexBase>::distance;
503  distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer<KDOP<18>,
504  Plane>::distance; distance_matrix[BV_KDOP18][GEOM_HALFSPACE] =
505  &BVHShapeDistancer<KDOP<18>, Halfspace>::distance;
506 
507  distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer<KDOP<24>,
508  Box>::distance; distance_matrix[BV_KDOP24][GEOM_SPHERE] =
509  &BVHShapeDistancer<KDOP<24>, Sphere>::distance;
510  distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<24>,
511  Capsule>::distance; distance_matrix[BV_KDOP24][GEOM_CONE] =
512  &BVHShapeDistancer<KDOP<24>, Cone>::distance;
513  distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<24>,
514  Cylinder>::distance; distance_matrix[BV_KDOP24][GEOM_CONVEX] =
515  &BVHShapeDistancer<KDOP<24>, ConvexBase>::distance;
516  distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer<KDOP<24>,
517  Plane>::distance; distance_matrix[BV_KDOP24][GEOM_HALFSPACE] =
518  &BVHShapeDistancer<KDOP<24>, Halfspace>::distance;
519  */
520 
521  distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer<kIOS, Box>::distance;
522  distance_matrix[BV_kIOS][GEOM_SPHERE] =
523  &BVHShapeDistancer<kIOS, Sphere>::distance;
524  distance_matrix[BV_kIOS][GEOM_CAPSULE] =
525  &BVHShapeDistancer<kIOS, Capsule>::distance;
526  distance_matrix[BV_kIOS][GEOM_CONE] =
527  &BVHShapeDistancer<kIOS, Cone>::distance;
528  distance_matrix[BV_kIOS][GEOM_CYLINDER] =
529  &BVHShapeDistancer<kIOS, Cylinder>::distance;
530  distance_matrix[BV_kIOS][GEOM_CONVEX] =
531  &BVHShapeDistancer<kIOS, ConvexBase>::distance;
532  distance_matrix[BV_kIOS][GEOM_PLANE] =
533  &BVHShapeDistancer<kIOS, Plane>::distance;
534  distance_matrix[BV_kIOS][GEOM_HALFSPACE] =
535  &BVHShapeDistancer<kIOS, Halfspace>::distance;
536  distance_matrix[BV_kIOS][GEOM_ELLIPSOID] =
537  &BVHShapeDistancer<kIOS, Ellipsoid>::distance;
538 
539  distance_matrix[BV_OBBRSS][GEOM_BOX] =
540  &BVHShapeDistancer<OBBRSS, Box>::distance;
541  distance_matrix[BV_OBBRSS][GEOM_SPHERE] =
542  &BVHShapeDistancer<OBBRSS, Sphere>::distance;
543  distance_matrix[BV_OBBRSS][GEOM_CAPSULE] =
544  &BVHShapeDistancer<OBBRSS, Capsule>::distance;
545  distance_matrix[BV_OBBRSS][GEOM_CONE] =
546  &BVHShapeDistancer<OBBRSS, Cone>::distance;
547  distance_matrix[BV_OBBRSS][GEOM_CYLINDER] =
548  &BVHShapeDistancer<OBBRSS, Cylinder>::distance;
549  distance_matrix[BV_OBBRSS][GEOM_CONVEX] =
550  &BVHShapeDistancer<OBBRSS, ConvexBase>::distance;
551  distance_matrix[BV_OBBRSS][GEOM_PLANE] =
552  &BVHShapeDistancer<OBBRSS, Plane>::distance;
553  distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] =
554  &BVHShapeDistancer<OBBRSS, Halfspace>::distance;
555  distance_matrix[BV_OBBRSS][GEOM_ELLIPSOID] =
556  &BVHShapeDistancer<OBBRSS, Ellipsoid>::distance;
557 
558  distance_matrix[HF_AABB][GEOM_BOX] =
559  &HeightFieldShapeDistancer<AABB, Box>::distance;
560  distance_matrix[HF_AABB][GEOM_SPHERE] =
561  &HeightFieldShapeDistancer<AABB, Sphere>::distance;
562  distance_matrix[HF_AABB][GEOM_CAPSULE] =
563  &HeightFieldShapeDistancer<AABB, Capsule>::distance;
564  distance_matrix[HF_AABB][GEOM_CONE] =
565  &HeightFieldShapeDistancer<AABB, Cone>::distance;
566  distance_matrix[HF_AABB][GEOM_CYLINDER] =
567  &HeightFieldShapeDistancer<AABB, Cylinder>::distance;
568  distance_matrix[HF_AABB][GEOM_CONVEX] =
569  &HeightFieldShapeDistancer<AABB, ConvexBase>::distance;
570  distance_matrix[HF_AABB][GEOM_PLANE] =
571  &HeightFieldShapeDistancer<AABB, Plane>::distance;
572  distance_matrix[HF_AABB][GEOM_HALFSPACE] =
573  &HeightFieldShapeDistancer<AABB, Halfspace>::distance;
574  distance_matrix[HF_AABB][GEOM_ELLIPSOID] =
575  &HeightFieldShapeDistancer<AABB, Ellipsoid>::distance;
576 
577  distance_matrix[HF_OBBRSS][GEOM_BOX] =
578  &HeightFieldShapeDistancer<OBBRSS, Box>::distance;
579  distance_matrix[HF_OBBRSS][GEOM_SPHERE] =
580  &HeightFieldShapeDistancer<OBBRSS, Sphere>::distance;
581  distance_matrix[HF_OBBRSS][GEOM_CAPSULE] =
582  &HeightFieldShapeDistancer<OBBRSS, Capsule>::distance;
583  distance_matrix[HF_OBBRSS][GEOM_CONE] =
584  &HeightFieldShapeDistancer<OBBRSS, Cone>::distance;
585  distance_matrix[HF_OBBRSS][GEOM_CYLINDER] =
586  &HeightFieldShapeDistancer<OBBRSS, Cylinder>::distance;
587  distance_matrix[HF_OBBRSS][GEOM_CONVEX] =
588  &HeightFieldShapeDistancer<OBBRSS, ConvexBase>::distance;
589  distance_matrix[HF_OBBRSS][GEOM_PLANE] =
590  &HeightFieldShapeDistancer<OBBRSS, Plane>::distance;
591  distance_matrix[HF_OBBRSS][GEOM_HALFSPACE] =
592  &HeightFieldShapeDistancer<OBBRSS, Halfspace>::distance;
593  distance_matrix[HF_OBBRSS][GEOM_ELLIPSOID] =
594  &HeightFieldShapeDistancer<OBBRSS, Ellipsoid>::distance;
595 
596  distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB>;
597  distance_matrix[BV_OBB][BV_OBB] = &BVHDistance<OBB>;
598  distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS>;
599  distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS>;
600  distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS>;
601 
602 #ifdef HPP_FCL_HAS_OCTOMAP
603  distance_matrix[GEOM_OCTREE][GEOM_BOX] = &Distance<OcTree, Box>;
604  distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &Distance<OcTree, Sphere>;
605  distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &Distance<OcTree, Capsule>;
606  distance_matrix[GEOM_OCTREE][GEOM_CONE] = &Distance<OcTree, Cone>;
607  distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &Distance<OcTree, Cylinder>;
608  distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &Distance<OcTree, ConvexBase>;
609  distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &Distance<OcTree, Plane>;
610  distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &Distance<OcTree, Halfspace>;
611  distance_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &Distance<OcTree, Ellipsoid>;
612 
613  distance_matrix[GEOM_BOX][GEOM_OCTREE] = &Distance<Box, OcTree>;
614  distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &Distance<Sphere, OcTree>;
615  distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &Distance<Capsule, OcTree>;
616  distance_matrix[GEOM_CONE][GEOM_OCTREE] = &Distance<Cone, OcTree>;
617  distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &Distance<Cylinder, OcTree>;
618  distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &Distance<ConvexBase, OcTree>;
619  distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &Distance<Plane, OcTree>;
620  distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &Distance<Halfspace, OcTree>;
621 
622  distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &Distance<OcTree, OcTree>;
623 
624  distance_matrix[GEOM_OCTREE][BV_AABB] = &Distance<OcTree, BVHModel<AABB> >;
625  distance_matrix[GEOM_OCTREE][BV_OBB] = &Distance<OcTree, BVHModel<OBB> >;
626  distance_matrix[GEOM_OCTREE][BV_RSS] = &Distance<OcTree, BVHModel<RSS> >;
627  distance_matrix[GEOM_OCTREE][BV_OBBRSS] =
628  &Distance<OcTree, BVHModel<OBBRSS> >;
629  distance_matrix[GEOM_OCTREE][BV_kIOS] = &Distance<OcTree, BVHModel<kIOS> >;
630  distance_matrix[GEOM_OCTREE][BV_KDOP16] =
631  &Distance<OcTree, BVHModel<KDOP<16> > >;
632  distance_matrix[GEOM_OCTREE][BV_KDOP18] =
633  &Distance<OcTree, BVHModel<KDOP<18> > >;
634  distance_matrix[GEOM_OCTREE][BV_KDOP24] =
635  &Distance<OcTree, BVHModel<KDOP<24> > >;
636 
637  distance_matrix[BV_AABB][GEOM_OCTREE] = &Distance<BVHModel<AABB>, OcTree>;
638  distance_matrix[BV_OBB][GEOM_OCTREE] = &Distance<BVHModel<OBB>, OcTree>;
639  distance_matrix[BV_RSS][GEOM_OCTREE] = &Distance<BVHModel<RSS>, OcTree>;
640  distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &Distance<BVHModel<OBBRSS>, OcTree>;
641  distance_matrix[BV_kIOS][GEOM_OCTREE] = &Distance<BVHModel<kIOS>, OcTree>;
642  distance_matrix[BV_KDOP16][GEOM_OCTREE] =
643  &Distance<BVHModel<KDOP<16> >, OcTree>;
644  distance_matrix[BV_KDOP18][GEOM_OCTREE] =
645  &Distance<BVHModel<KDOP<18> >, OcTree>;
646  distance_matrix[BV_KDOP24][GEOM_OCTREE] =
647  &Distance<BVHModel<KDOP<24> >, OcTree>;
648 #endif
649 }
650 // template struct DistanceFunctionMatrix;
651 } // namespace fcl
652 
653 } // namespace hpp
hpp::fcl::GEOM_BOX
Definition: collision_object.h:75
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
hpp::fcl::DistanceRequest
request to the distance computation
Definition: collision_data.h:392
traversal_node_setup.h
hpp::fcl::ShapeShapeDistance< Plane, Cylinder >
FCL_REAL ShapeShapeDistance< Plane, Cylinder >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: cylinder_plane.cpp:69
hpp::fcl::BV_OBBRSS
Definition: collision_object.h:71
hpp::fcl::ShapeShapeDistance< Sphere, Box >
FCL_REAL ShapeShapeDistance< Sphere, Box >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: box_sphere.cpp:69
hpp::fcl::GEOM_CONE
Definition: collision_object.h:78
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::ShapeShapeDistance< Sphere, Plane >
FCL_REAL ShapeShapeDistance< Sphere, Plane >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: sphere_plane.cpp:52
hpp::fcl::ShapeShapeDistance< ConvexBase, Halfspace >
FCL_REAL ShapeShapeDistance< ConvexBase, Halfspace >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: convex_halfspace.cpp:46
hpp::fcl::BVHDistance< RSS >
FCL_REAL BVHDistance< RSS >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
Definition: distance_func_matrix.cpp:244
hpp::fcl::ShapeShapeDistance< Cone, Halfspace >
FCL_REAL ShapeShapeDistance< Cone, Halfspace >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: cone_halfspace.cpp:52
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:37
initialize
bool initialize(MeshCollisionTraversalNode< BV, RelativeTransformationIsIdentity > &node, BVHModel< BV > &model1, Transform3f &tf1, BVHModel< BV > &model2, Transform3f &tf2, CollisionResult &result, bool use_refit=false, bool refit_bottomup=false)
Initialize traversal node for collision between two meshes, given the current transforms.
Definition: traversal_node_setup.h:423
hpp::fcl::ShapeShapeDistance< Capsule, Plane >
FCL_REAL ShapeShapeDistance< Capsule, Plane >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: capsule_plane.cpp:52
hpp::fcl::ShapeShapeDistance< Cylinder, Halfspace >
FCL_REAL ShapeShapeDistance< Cylinder, Halfspace >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: cylinder_halfspace.cpp:52
hpp::fcl::details::orientedBVHShapeDistance
FCL_REAL orientedBVHShapeDistance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
Definition: distance_func_matrix.cpp:112
hpp::fcl::ShapeShapeDistance< Halfspace, Cone >
FCL_REAL ShapeShapeDistance< Halfspace, Cone >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: cone_halfspace.cpp:69
hpp::fcl::ShapeShapeDistance< Plane, Capsule >
FCL_REAL ShapeShapeDistance< Plane, Capsule >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: capsule_plane.cpp:69
hpp::fcl::BVHShapeDistancer::distance
static FCL_REAL distance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
Definition: distance_func_matrix.cpp:88
hpp::fcl::ShapeShapeDistance< Cylinder, Sphere >
FCL_REAL ShapeShapeDistance< Cylinder, Sphere >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: sphere_cylinder.cpp:69
shape_shape_func.h
hpp::fcl::GEOM_OCTREE
Definition: collision_object.h:84
hpp::fcl::BV_RSS
Definition: collision_object.h:69
hpp::fcl::ShapeShapeDistance
FCL_REAL ShapeShapeDistance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
Definition: distance_func_matrix.cpp:70
hpp::fcl::BVHDistance< kIOS >
FCL_REAL BVHDistance< kIOS >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
Definition: distance_func_matrix.cpp:253
hpp::fcl::ShapeShapeDistance< Capsule, Capsule >
FCL_REAL ShapeShapeDistance< Capsule, Capsule >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &request, DistanceResult &result)
Definition: src/distance/capsule_capsule.cpp:80
hpp::fcl::BV_AABB
Definition: collision_object.h:67
hpp::fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:53
distance_func_matrix.h
hpp::fcl::BVHShapeDistancer< RSS, T_SH >::distance
static FCL_REAL distance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
Definition: distance_func_matrix.cpp:134
hpp::fcl::HF_AABB
Definition: collision_object.h:86
hpp::fcl::OcTree
Octree is one type of collision geometry which can encode uncertainty information in the sensor data...
Definition: octree.h:53
hpp::fcl::GEOM_HALFSPACE
Definition: collision_object.h:82
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::ShapeShapeDistance< Halfspace, ConvexBase >
FCL_REAL ShapeShapeDistance< Halfspace, ConvexBase >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: convex_halfspace.cpp:64
hpp::fcl::BVHDistance< OBBRSS >
FCL_REAL BVHDistance< OBBRSS >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
Definition: distance_func_matrix.cpp:262
HPP_FCL_UNUSED_VARIABLE
#define HPP_FCL_UNUSED_VARIABLE(var)
Definition: include/hpp/fcl/fwd.hh:55
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::BV_KDOP16
Definition: collision_object.h:72
hpp::fcl::DistanceRequest::isSatisfied
bool isSatisfied(const DistanceResult &result) const
Definition: collision_data.cpp:47
hpp::fcl::DistanceResult::min_distance
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0...
Definition: collision_data.h:424
hpp::fcl::HF_OBBRSS
Definition: collision_object.h:87
hpp::fcl::RSS
A class for rectangle sphere-swept bounding volume.
Definition: BV/RSS.h:53
hpp::fcl::ShapeShapeDistance< Sphere, Halfspace >
FCL_REAL ShapeShapeDistance< Sphere, Halfspace >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: sphere_halfspace.cpp:52
hpp::fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH/BVH_model.h:273
hpp::fcl::BV_kIOS
Definition: collision_object.h:70
hpp::fcl::ShapeShapeDistance< Box, Sphere >
FCL_REAL ShapeShapeDistance< Box, Sphere >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: box_sphere.cpp:52
hpp::fcl::ShapeShapeDistance< Cone, Plane >
FCL_REAL ShapeShapeDistance< Cone, Plane >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: cone_plane.cpp:52
hpp::fcl::BVHDistance
FCL_REAL BVHDistance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
Definition: distance_func_matrix.cpp:202
hpp::fcl::BV_KDOP18
Definition: collision_object.h:73
hpp::fcl::GEOM_SPHERE
Definition: collision_object.h:76
hpp::fcl::ShapeShapeDistance< Plane, Sphere >
FCL_REAL ShapeShapeDistance< Plane, Sphere >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: sphere_plane.cpp:69
hpp::fcl::ShapeShapeDistance< Cylinder, Plane >
FCL_REAL ShapeShapeDistance< Cylinder, Plane >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: cylinder_plane.cpp:52
hpp::fcl::GEOM_ELLIPSOID
Definition: collision_object.h:85
hpp::fcl::ShapeShapeDistance< Halfspace, Capsule >
FCL_REAL ShapeShapeDistance< Halfspace, Capsule >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: capsule_halfspace.cpp:69
hpp::fcl::ShapeShapeDistance< Halfspace, Cylinder >
FCL_REAL ShapeShapeDistance< Halfspace, Cylinder >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: cylinder_halfspace.cpp:69
hpp::fcl::GEOM_CYLINDER
Definition: collision_object.h:79
hpp::fcl::ShapeShapeDistance< Plane, Cone >
FCL_REAL ShapeShapeDistance< Plane, Cone >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: cone_plane.cpp:69
MeshShapeDistanceTraversalNodeRSS
Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS...
Definition: traversal_node_bvh_shape.h:540
hpp::fcl::ShapeShapeDistance< Sphere, Cylinder >
FCL_REAL ShapeShapeDistance< Sphere, Cylinder >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: sphere_cylinder.cpp:52
hpp::fcl::NODE_COUNT
Definition: collision_object.h:88
hpp::fcl::GJKSolver
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
hpp::fcl::ShapeShapeDistance< Plane, Box >
FCL_REAL ShapeShapeDistance< Plane, Box >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: box_plane.cpp:69
hpp::fcl::ShapeShapeDistance< Halfspace, Sphere >
FCL_REAL ShapeShapeDistance< Halfspace, Sphere >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: sphere_halfspace.cpp:69
hpp::fcl::ShapeShapeDistance< Box, Halfspace >
FCL_REAL ShapeShapeDistance< Box, Halfspace >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: box_halfspace.cpp:52
hpp::fcl::ShapeShapeDistance< Halfspace, Box >
FCL_REAL ShapeShapeDistance< Halfspace, Box >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: box_halfspace.cpp:69
hpp::fcl::BV_OBB
Definition: collision_object.h:68
hpp::fcl::BVHShapeDistancer< kIOS, T_SH >::distance
static FCL_REAL distance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
Definition: distance_func_matrix.cpp:147
hpp::fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_object.h:95
hpp::fcl::details::orientedMeshDistance
FCL_REAL orientedMeshDistance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
Definition: distance_func_matrix.cpp:224
hpp::fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: BV/OBBRSS.h:54
hpp::fcl::BVHShapeDistancer< OBBRSS, T_SH >::distance
static FCL_REAL distance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
Definition: distance_func_matrix.cpp:160
hpp::fcl::ShapeShapeDistance< Box, Plane >
FCL_REAL ShapeShapeDistance< Box, Plane >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: box_plane.cpp:52
hpp::fcl::ShapeShapeDistance< Sphere, Sphere >
FCL_REAL ShapeShapeDistance< Sphere, Sphere >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: sphere_sphere.cpp:56
hpp::fcl::GEOM_PLANE
Definition: collision_object.h:81
hpp::fcl::GEOM_CONVEX
Definition: collision_object.h:80
hpp::fcl::GEOM_CAPSULE
Definition: collision_object.h:77
hpp::fcl::ShapeShapeDistance< Capsule, Halfspace >
FCL_REAL ShapeShapeDistance< Capsule, Halfspace >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: capsule_halfspace.cpp:52
gjk.tf1
tuple tf1
Definition: test/scripts/gjk.py:28
HPP_FCL_THROW_PRETTY
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: include/hpp/fcl/fwd.hh:57
hpp::fcl::HeightFieldShapeDistancer::distance
static FCL_REAL distance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
Definition: distance_func_matrix.cpp:173
hpp::fcl::BV_KDOP24
Definition: collision_object.h:74

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