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 
39 
40 #include <../src/collision_node.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>
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>
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) {
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) {
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) {
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) {
182  HPP_FCL_UNUSED_VARIABLE(nsolver);
183  HPP_FCL_UNUSED_VARIABLE(request);
184  // TODO(jcarpent)
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>
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>
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 <>
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 <>
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 <>
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>
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 
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>;
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>;
292  distance_matrix[GEOM_BOX][GEOM_HALFSPACE] =
294  distance_matrix[GEOM_BOX][GEOM_ELLIPSOID] =
295  &ShapeShapeDistance<Box, Ellipsoid>;
296 
298  distance_matrix[GEOM_SPHERE][GEOM_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] =
305  distance_matrix[GEOM_SPHERE][GEOM_CONVEX] =
306  &ShapeShapeDistance<Sphere, ConvexBase>;
308  distance_matrix[GEOM_SPHERE][GEOM_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] =
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] =
342  distance_matrix[GEOM_CAPSULE][GEOM_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>;
356  distance_matrix[GEOM_CONE][GEOM_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] =
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] =
374  distance_matrix[GEOM_CYLINDER][GEOM_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] =
394  distance_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] =
395  &ShapeShapeDistance<ConvexBase, Ellipsoid>;
396 
399  distance_matrix[GEOM_PLANE][GEOM_CAPSULE] =
402  distance_matrix[GEOM_PLANE][GEOM_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] =
413  distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] =
415  distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] =
417  distance_matrix[GEOM_HALFSPACE][GEOM_CONE] =
419  distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] =
421  distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] =
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 
446  distance_matrix[BV_OBB][GEOM_SPHERE] =
448  distance_matrix[BV_OBB][GEOM_CAPSULE] =
451  distance_matrix[BV_OBB][GEOM_CYLINDER] =
453  distance_matrix[BV_OBB][GEOM_CONVEX] =
455  distance_matrix[BV_OBB][GEOM_PLANE] =
457  distance_matrix[BV_OBB][GEOM_HALFSPACE] =
459  distance_matrix[BV_OBB][GEOM_ELLIPSOID] =
461 
463  distance_matrix[BV_RSS][GEOM_SPHERE] =
465  distance_matrix[BV_RSS][GEOM_CAPSULE] =
468  distance_matrix[BV_RSS][GEOM_CYLINDER] =
470  distance_matrix[BV_RSS][GEOM_CONVEX] =
472  distance_matrix[BV_RSS][GEOM_PLANE] =
474  distance_matrix[BV_RSS][GEOM_HALFSPACE] =
476  distance_matrix[BV_RSS][GEOM_ELLIPSOID] =
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 
522  distance_matrix[BV_kIOS][GEOM_SPHERE] =
524  distance_matrix[BV_kIOS][GEOM_CAPSULE] =
526  distance_matrix[BV_kIOS][GEOM_CONE] =
528  distance_matrix[BV_kIOS][GEOM_CYLINDER] =
530  distance_matrix[BV_kIOS][GEOM_CONVEX] =
532  distance_matrix[BV_kIOS][GEOM_PLANE] =
534  distance_matrix[BV_kIOS][GEOM_HALFSPACE] =
536  distance_matrix[BV_kIOS][GEOM_ELLIPSOID] =
538 
539  distance_matrix[BV_OBBRSS][GEOM_BOX] =
541  distance_matrix[BV_OBBRSS][GEOM_SPHERE] =
543  distance_matrix[BV_OBBRSS][GEOM_CAPSULE] =
545  distance_matrix[BV_OBBRSS][GEOM_CONE] =
547  distance_matrix[BV_OBBRSS][GEOM_CYLINDER] =
549  distance_matrix[BV_OBBRSS][GEOM_CONVEX] =
551  distance_matrix[BV_OBBRSS][GEOM_PLANE] =
553  distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] =
555  distance_matrix[BV_OBBRSS][GEOM_ELLIPSOID] =
557 
558  distance_matrix[HF_AABB][GEOM_BOX] =
560  distance_matrix[HF_AABB][GEOM_SPHERE] =
562  distance_matrix[HF_AABB][GEOM_CAPSULE] =
564  distance_matrix[HF_AABB][GEOM_CONE] =
566  distance_matrix[HF_AABB][GEOM_CYLINDER] =
568  distance_matrix[HF_AABB][GEOM_CONVEX] =
570  distance_matrix[HF_AABB][GEOM_PLANE] =
572  distance_matrix[HF_AABB][GEOM_HALFSPACE] =
574  distance_matrix[HF_AABB][GEOM_ELLIPSOID] =
576 
577  distance_matrix[HF_OBBRSS][GEOM_BOX] =
579  distance_matrix[HF_OBBRSS][GEOM_SPHERE] =
581  distance_matrix[HF_OBBRSS][GEOM_CAPSULE] =
583  distance_matrix[HF_OBBRSS][GEOM_CONE] =
585  distance_matrix[HF_OBBRSS][GEOM_CYLINDER] =
587  distance_matrix[HF_OBBRSS][GEOM_CONVEX] =
589  distance_matrix[HF_OBBRSS][GEOM_PLANE] =
591  distance_matrix[HF_OBBRSS][GEOM_HALFSPACE] =
593  distance_matrix[HF_OBBRSS][GEOM_ELLIPSOID] =
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
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
request to the distance computation
FCL_REAL ShapeShapeDistance< Plane, Cylinder >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
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
Main namespace.
FCL_REAL ShapeShapeDistance< Sphere, Plane >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL ShapeShapeDistance< ConvexBase, Halfspace >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL BVHDistance< RSS >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Cone, Halfspace >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
tuple tf2
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.
FCL_REAL ShapeShapeDistance< Capsule, Plane >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Cylinder, Halfspace >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL orientedBVHShapeDistance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Halfspace, Cone >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Plane, Capsule >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
static FCL_REAL distance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Cylinder, Sphere >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL ShapeShapeDistance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
FCL_REAL BVHDistance< kIOS >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Capsule, Capsule >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &request, DistanceResult &result)
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:53
static FCL_REAL distance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
Octree is one type of collision geometry which can encode uncertainty information in the sensor data...
Definition: octree.h:53
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
FCL_REAL ShapeShapeDistance< Halfspace, ConvexBase >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL BVHDistance< OBBRSS >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
#define HPP_FCL_UNUSED_VARIABLE(var)
double FCL_REAL
Definition: data_types.h:65
bool isSatisfied(const DistanceResult &result) const
FCL_REAL min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0...
A class for rectangle sphere-swept bounding volume.
Definition: BV/RSS.h:53
FCL_REAL ShapeShapeDistance< Sphere, Halfspace >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
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
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
FCL_REAL BVHDistance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Plane, Sphere >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Cylinder, Plane >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Halfspace, Capsule >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Halfspace, Cylinder >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
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
Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS...
FCL_REAL ShapeShapeDistance< Sphere, Cylinder >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
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
FCL_REAL ShapeShapeDistance< Halfspace, Sphere >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Box, Halfspace >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Halfspace, Box >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
static FCL_REAL distance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
The geometry for the object for collision or distance computation.
FCL_REAL orientedMeshDistance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const DistanceRequest &request, DistanceResult &result)
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: BV/OBBRSS.h:54
static FCL_REAL distance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)
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
FCL_REAL ShapeShapeDistance< Sphere, Sphere >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
FCL_REAL ShapeShapeDistance< Capsule, Halfspace >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
tuple tf1
#define HPP_FCL_THROW_PRETTY(message, exception)
static FCL_REAL distance(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *nsolver, const DistanceRequest &request, DistanceResult &result)


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