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

hpp-fcl
Author(s):
autogenerated on Fri Feb 14 2025 03:45:50