GCC Code Coverage Report


Directory: ./
File: src/distance_func_matrix.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 206 259 79.5%
Branches: 14 150 9.3%

Line Branch Exec Source
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
36 /** \author Jia Pan */
37
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 Scalar 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 Scalar 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 Scalar 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 204 Scalar 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
2/4
✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 102 times.
204 if (request.isSatisfied(result)) return result.min_distance;
119
1/2
✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
204 OrientedMeshShapeDistanceTraversalNode node;
120 204 const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
121 204 const T_SH* obj2 = static_cast<const T_SH*>(o2);
122
123
1/2
✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
204 initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
124
1/2
✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
204 ::coal::distance(&node);
125
126 204 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 Scalar distance(const CollisionGeometry* o1, const Transform3s& tf1,
134 const CollisionGeometry* o2, const Transform3s& tf2,
135 const GJKSolver* nsolver,
136 const DistanceRequest& request,
137 DistanceResult& result) {
138 return details::orientedBVHShapeDistance<
139 MeshShapeDistanceTraversalNodeRSS<T_SH>, RSS, T_SH>(
140 o1, tf1, o2, tf2, nsolver, request, result);
141 }
142 };
143
144 template <typename T_SH>
145 struct COAL_LOCAL BVHShapeDistancer<kIOS, T_SH> {
146 static Scalar distance(const CollisionGeometry* o1, const Transform3s& tf1,
147 const CollisionGeometry* o2, const Transform3s& tf2,
148 const GJKSolver* nsolver,
149 const DistanceRequest& request,
150 DistanceResult& result) {
151 return details::orientedBVHShapeDistance<
152 MeshShapeDistanceTraversalNodekIOS<T_SH>, kIOS, T_SH>(
153 o1, tf1, o2, tf2, nsolver, request, result);
154 }
155 };
156
157 template <typename T_SH>
158 struct COAL_LOCAL BVHShapeDistancer<OBBRSS, T_SH> {
159 204 static Scalar distance(const CollisionGeometry* o1, const Transform3s& tf1,
160 const CollisionGeometry* o2, const Transform3s& tf2,
161 const GJKSolver* nsolver,
162 const DistanceRequest& request,
163 DistanceResult& result) {
164 return details::orientedBVHShapeDistance<
165 204 MeshShapeDistanceTraversalNodeOBBRSS<T_SH>, OBBRSS, T_SH>(
166 204 o1, tf1, o2, tf2, nsolver, request, result);
167 }
168 };
169
170 template <typename T_HF, typename T_SH>
171 struct COAL_LOCAL HeightFieldShapeDistancer{static Scalar distance(
172 const CollisionGeometry* o1, const Transform3s& tf1,
173 const CollisionGeometry* o2, const Transform3s& tf2,
174 const GJKSolver* nsolver, const DistanceRequest& request,
175 DistanceResult& result){COAL_UNUSED_VARIABLE(o1);
176 COAL_UNUSED_VARIABLE(tf1);
177 COAL_UNUSED_VARIABLE(o2);
178 COAL_UNUSED_VARIABLE(tf2);
179 COAL_UNUSED_VARIABLE(nsolver);
180 COAL_UNUSED_VARIABLE(request);
181 // TODO(jcarpent)
182 COAL_THROW_PRETTY(
183 "Distance between a height field and a shape is not implemented",
184 std::invalid_argument);
185 // if(request.isSatisfied(result)) return result.min_distance;
186 // HeightFieldShapeDistanceTraversalNode<T_HF, T_SH> node;
187 //
188 // const HeightField<T_HF>* obj1 = static_cast<const HeightField<T_HF>*
189 // >(o1); const T_SH* obj2 = static_cast<const T_SH*>(o2);
190 //
191 // initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
192 // fcl::distance(&node);
193
194 return result.min_distance;
195 }
196 }
197 ;
198
199 template <typename T_BVH>
200 Scalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1,
201 const CollisionGeometry* o2, const Transform3s& tf2,
202 const DistanceRequest& request, DistanceResult& result) {
203 if (request.isSatisfied(result)) return result.min_distance;
204 MeshDistanceTraversalNode<T_BVH> node;
205 const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
206 const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
207 BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
208 Transform3s tf1_tmp = tf1;
209 BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
210 Transform3s tf2_tmp = tf2;
211
212 initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
213 distance(&node);
214 delete obj1_tmp;
215 delete obj2_tmp;
216
217 return result.min_distance;
218 }
219
220 namespace details {
221 template <typename OrientedMeshDistanceTraversalNode, typename T_BVH>
222 14290 Scalar orientedMeshDistance(const CollisionGeometry* o1, const Transform3s& tf1,
223 const CollisionGeometry* o2, const Transform3s& tf2,
224 const DistanceRequest& request,
225 DistanceResult& result) {
226
2/4
✓ Branch 1 taken 7145 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 7145 times.
14290 if (request.isSatisfied(result)) return result.min_distance;
227
1/2
✓ Branch 1 taken 7145 times.
✗ Branch 2 not taken.
14290 OrientedMeshDistanceTraversalNode node;
228 14290 const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
229 14290 const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
230
231
1/2
✓ Branch 1 taken 7145 times.
✗ Branch 2 not taken.
14290 initialize(node, *obj1, tf1, *obj2, tf2, request, result);
232
1/2
✓ Branch 1 taken 7145 times.
✗ Branch 2 not taken.
14290 distance(&node);
233
234 14290 return result.min_distance;
235 }
236
237 } // namespace details
238
239 template <>
240 Scalar BVHDistance<RSS>(const CollisionGeometry* o1, const Transform3s& tf1,
241 const CollisionGeometry* o2, const Transform3s& tf2,
242 const DistanceRequest& request,
243 DistanceResult& result) {
244 return details::orientedMeshDistance<MeshDistanceTraversalNodeRSS, RSS>(
245 o1, tf1, o2, tf2, request, result);
246 }
247
248 template <>
249 Scalar BVHDistance<kIOS>(const CollisionGeometry* o1, const Transform3s& tf1,
250 const CollisionGeometry* o2, const Transform3s& tf2,
251 const DistanceRequest& request,
252 DistanceResult& result) {
253 return details::orientedMeshDistance<MeshDistanceTraversalNodekIOS, kIOS>(
254 o1, tf1, o2, tf2, request, result);
255 }
256
257 template <>
258 7145 Scalar BVHDistance<OBBRSS>(const CollisionGeometry* o1, const Transform3s& tf1,
259 const CollisionGeometry* o2, const Transform3s& tf2,
260 const DistanceRequest& request,
261 DistanceResult& result) {
262 7145 return details::orientedMeshDistance<MeshDistanceTraversalNodeOBBRSS, OBBRSS>(
263 7145 o1, tf1, o2, tf2, request, result);
264 }
265
266 template <typename T_BVH>
267 14290 Scalar BVHDistance(const CollisionGeometry* o1, const Transform3s& tf1,
268 const CollisionGeometry* o2, const Transform3s& tf2,
269 const GJKSolver* /*nsolver*/, const DistanceRequest& request,
270 DistanceResult& result) {
271 14290 return BVHDistance<T_BVH>(o1, tf1, o2, tf2, request, result);
272 }
273
274 10 DistanceFunctionMatrix::DistanceFunctionMatrix() {
275
2/2
✓ Branch 0 taken 220 times.
✓ Branch 1 taken 10 times.
230 for (int i = 0; i < NODE_COUNT; ++i) {
276
2/2
✓ Branch 0 taken 4840 times.
✓ Branch 1 taken 220 times.
5060 for (int j = 0; j < NODE_COUNT; ++j) distance_matrix[i][j] = NULL;
277 }
278
279 10 distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance<Box, Box>;
280 10 distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance<Box, Sphere>;
281 10 distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance<Box, Capsule>;
282 10 distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance<Box, Cone>;
283 10 distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance<Box, Cylinder>;
284 10 distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance<Box, ConvexBase>;
285 10 distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance<Box, Plane>;
286 10 distance_matrix[GEOM_BOX][GEOM_HALFSPACE] =
287 &ShapeShapeDistance<Box, Halfspace>;
288 10 distance_matrix[GEOM_BOX][GEOM_ELLIPSOID] =
289 &ShapeShapeDistance<Box, Ellipsoid>;
290
291 10 distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance<Sphere, Box>;
292 10 distance_matrix[GEOM_SPHERE][GEOM_SPHERE] =
293 &ShapeShapeDistance<Sphere, Sphere>;
294 10 distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] =
295 &ShapeShapeDistance<Sphere, Capsule>;
296 10 distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance<Sphere, Cone>;
297 10 distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] =
298 &ShapeShapeDistance<Sphere, Cylinder>;
299 10 distance_matrix[GEOM_SPHERE][GEOM_CONVEX] =
300 &ShapeShapeDistance<Sphere, ConvexBase>;
301 10 distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance<Sphere, Plane>;
302 10 distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] =
303 &ShapeShapeDistance<Sphere, Halfspace>;
304 10 distance_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] =
305 &ShapeShapeDistance<Sphere, Ellipsoid>;
306
307 10 distance_matrix[GEOM_ELLIPSOID][GEOM_BOX] =
308 &ShapeShapeDistance<Ellipsoid, Box>;
309 10 distance_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] =
310 &ShapeShapeDistance<Ellipsoid, Sphere>;
311 10 distance_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] =
312 &ShapeShapeDistance<Ellipsoid, Capsule>;
313 10 distance_matrix[GEOM_ELLIPSOID][GEOM_CONE] =
314 &ShapeShapeDistance<Ellipsoid, Cone>;
315 10 distance_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] =
316 &ShapeShapeDistance<Ellipsoid, Cylinder>;
317 10 distance_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] =
318 &ShapeShapeDistance<Ellipsoid, ConvexBase>;
319 10 distance_matrix[GEOM_ELLIPSOID][GEOM_PLANE] =
320 &ShapeShapeDistance<Ellipsoid, Plane>;
321 10 distance_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] =
322 &ShapeShapeDistance<Ellipsoid, Halfspace>;
323 10 distance_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] =
324 &ShapeShapeDistance<Ellipsoid, Ellipsoid>;
325
326 10 distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance<Capsule, Box>;
327 10 distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] =
328 &ShapeShapeDistance<Capsule, Sphere>;
329 10 distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] =
330 &ShapeShapeDistance<Capsule, Capsule>;
331 10 distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance<Capsule, Cone>;
332 10 distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] =
333 &ShapeShapeDistance<Capsule, Cylinder>;
334 10 distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] =
335 &ShapeShapeDistance<Capsule, ConvexBase>;
336 10 distance_matrix[GEOM_CAPSULE][GEOM_PLANE] =
337 &ShapeShapeDistance<Capsule, Plane>;
338 10 distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] =
339 &ShapeShapeDistance<Capsule, Halfspace>;
340 10 distance_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] =
341 &ShapeShapeDistance<Capsule, Ellipsoid>;
342
343 10 distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance<Cone, Box>;
344 10 distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance<Cone, Sphere>;
345 10 distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance<Cone, Capsule>;
346 10 distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance<Cone, Cone>;
347 10 distance_matrix[GEOM_CONE][GEOM_CYLINDER] =
348 &ShapeShapeDistance<Cone, Cylinder>;
349 10 distance_matrix[GEOM_CONE][GEOM_CONVEX] =
350 &ShapeShapeDistance<Cone, ConvexBase>;
351 10 distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance<Cone, Plane>;
352 10 distance_matrix[GEOM_CONE][GEOM_HALFSPACE] =
353 &ShapeShapeDistance<Cone, Halfspace>;
354 10 distance_matrix[GEOM_CONE][GEOM_ELLIPSOID] =
355 &ShapeShapeDistance<Cone, Ellipsoid>;
356
357 10 distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance<Cylinder, Box>;
358 10 distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] =
359 &ShapeShapeDistance<Cylinder, Sphere>;
360 10 distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] =
361 &ShapeShapeDistance<Cylinder, Capsule>;
362 10 distance_matrix[GEOM_CYLINDER][GEOM_CONE] =
363 &ShapeShapeDistance<Cylinder, Cone>;
364 10 distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] =
365 &ShapeShapeDistance<Cylinder, Cylinder>;
366 10 distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] =
367 &ShapeShapeDistance<Cylinder, ConvexBase>;
368 10 distance_matrix[GEOM_CYLINDER][GEOM_PLANE] =
369 &ShapeShapeDistance<Cylinder, Plane>;
370 10 distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] =
371 &ShapeShapeDistance<Cylinder, Halfspace>;
372 10 distance_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] =
373 &ShapeShapeDistance<Cylinder, Ellipsoid>;
374
375 10 distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance<ConvexBase, Box>;
376 10 distance_matrix[GEOM_CONVEX][GEOM_SPHERE] =
377 &ShapeShapeDistance<ConvexBase, Sphere>;
378 10 distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] =
379 &ShapeShapeDistance<ConvexBase, Capsule>;
380 10 distance_matrix[GEOM_CONVEX][GEOM_CONE] =
381 &ShapeShapeDistance<ConvexBase, Cone>;
382 10 distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] =
383 &ShapeShapeDistance<ConvexBase, Cylinder>;
384 10 distance_matrix[GEOM_CONVEX][GEOM_CONVEX] =
385 &ShapeShapeDistance<ConvexBase, ConvexBase>;
386 10 distance_matrix[GEOM_CONVEX][GEOM_PLANE] =
387 &ShapeShapeDistance<ConvexBase, Plane>;
388 10 distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] =
389 &ShapeShapeDistance<ConvexBase, Halfspace>;
390 10 distance_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] =
391 &ShapeShapeDistance<ConvexBase, Ellipsoid>;
392
393 10 distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance<Plane, Box>;
394 10 distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance<Plane, Sphere>;
395 10 distance_matrix[GEOM_PLANE][GEOM_CAPSULE] =
396 &ShapeShapeDistance<Plane, Capsule>;
397 10 distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance<Plane, Cone>;
398 10 distance_matrix[GEOM_PLANE][GEOM_CYLINDER] =
399 &ShapeShapeDistance<Plane, Cylinder>;
400 10 distance_matrix[GEOM_PLANE][GEOM_CONVEX] =
401 &ShapeShapeDistance<Plane, ConvexBase>;
402 10 distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane, Plane>;
403 10 distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] =
404 &ShapeShapeDistance<Plane, Halfspace>;
405 10 distance_matrix[GEOM_PLANE][GEOM_ELLIPSOID] =
406 &ShapeShapeDistance<Plane, Ellipsoid>;
407
408 10 distance_matrix[GEOM_HALFSPACE][GEOM_BOX] =
409 &ShapeShapeDistance<Halfspace, Box>;
410 10 distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] =
411 &ShapeShapeDistance<Halfspace, Sphere>;
412 10 distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] =
413 &ShapeShapeDistance<Halfspace, Capsule>;
414 10 distance_matrix[GEOM_HALFSPACE][GEOM_CONE] =
415 &ShapeShapeDistance<Halfspace, Cone>;
416 10 distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] =
417 &ShapeShapeDistance<Halfspace, Cylinder>;
418 10 distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] =
419 &ShapeShapeDistance<Halfspace, ConvexBase>;
420 10 distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] =
421 &ShapeShapeDistance<Halfspace, Plane>;
422 10 distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] =
423 &ShapeShapeDistance<Halfspace, Halfspace>;
424 10 distance_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] =
425 &ShapeShapeDistance<Halfspace, Ellipsoid>;
426
427 /* AABB distance not implemented */
428 /*
429 distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer<AABB, Box>::distance;
430 distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer<AABB,
431 Sphere>::distance; distance_matrix[BV_AABB][GEOM_CAPSULE] =
432 &BVHShapeDistancer<AABB, Capsule>::distance;
433 distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer<AABB,
434 Cone>::distance; distance_matrix[BV_AABB][GEOM_CYLINDER] =
435 &BVHShapeDistancer<AABB, Cylinder>::distance;
436 distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB,
437 ConvexBase>::distance; distance_matrix[BV_AABB][GEOM_PLANE] =
438 &BVHShapeDistancer<AABB, Plane>::distance;
439 distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB,
440 Halfspace>::distance;
441 */
442
443 10 distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box>::distance;
444 10 distance_matrix[BV_OBB][GEOM_SPHERE] =
445 &BVHShapeDistancer<OBB, Sphere>::distance;
446 10 distance_matrix[BV_OBB][GEOM_CAPSULE] =
447 &BVHShapeDistancer<OBB, Capsule>::distance;
448 10 distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer<OBB, Cone>::distance;
449 10 distance_matrix[BV_OBB][GEOM_CYLINDER] =
450 &BVHShapeDistancer<OBB, Cylinder>::distance;
451 10 distance_matrix[BV_OBB][GEOM_CONVEX] =
452 &BVHShapeDistancer<OBB, ConvexBase>::distance;
453 10 distance_matrix[BV_OBB][GEOM_PLANE] =
454 &BVHShapeDistancer<OBB, Plane>::distance;
455 10 distance_matrix[BV_OBB][GEOM_HALFSPACE] =
456 &BVHShapeDistancer<OBB, Halfspace>::distance;
457 10 distance_matrix[BV_OBB][GEOM_ELLIPSOID] =
458 &BVHShapeDistancer<OBB, Ellipsoid>::distance;
459
460 10 distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box>::distance;
461 10 distance_matrix[BV_RSS][GEOM_SPHERE] =
462 &BVHShapeDistancer<RSS, Sphere>::distance;
463 10 distance_matrix[BV_RSS][GEOM_CAPSULE] =
464 &BVHShapeDistancer<RSS, Capsule>::distance;
465 10 distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer<RSS, Cone>::distance;
466 10 distance_matrix[BV_RSS][GEOM_CYLINDER] =
467 &BVHShapeDistancer<RSS, Cylinder>::distance;
468 10 distance_matrix[BV_RSS][GEOM_CONVEX] =
469 &BVHShapeDistancer<RSS, ConvexBase>::distance;
470 10 distance_matrix[BV_RSS][GEOM_PLANE] =
471 &BVHShapeDistancer<RSS, Plane>::distance;
472 10 distance_matrix[BV_RSS][GEOM_HALFSPACE] =
473 &BVHShapeDistancer<RSS, Halfspace>::distance;
474 10 distance_matrix[BV_RSS][GEOM_ELLIPSOID] =
475 &BVHShapeDistancer<RSS, Ellipsoid>::distance;
476
477 /* KDOP distance not implemented */
478 /*
479 distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer<KDOP<16>,
480 Box>::distance; distance_matrix[BV_KDOP16][GEOM_SPHERE] =
481 &BVHShapeDistancer<KDOP<16>, Sphere>::distance;
482 distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<16>,
483 Capsule>::distance; distance_matrix[BV_KDOP16][GEOM_CONE] =
484 &BVHShapeDistancer<KDOP<16>, Cone>::distance;
485 distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<16>,
486 Cylinder>::distance; distance_matrix[BV_KDOP16][GEOM_CONVEX] =
487 &BVHShapeDistancer<KDOP<16>, ConvexBase>::distance;
488 distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer<KDOP<16>,
489 Plane>::distance; distance_matrix[BV_KDOP16][GEOM_HALFSPACE] =
490 &BVHShapeDistancer<KDOP<16>, Halfspace>::distance;
491
492 distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer<KDOP<18>,
493 Box>::distance; distance_matrix[BV_KDOP18][GEOM_SPHERE] =
494 &BVHShapeDistancer<KDOP<18>, Sphere>::distance;
495 distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<18>,
496 Capsule>::distance; distance_matrix[BV_KDOP18][GEOM_CONE] =
497 &BVHShapeDistancer<KDOP<18>, Cone>::distance;
498 distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<18>,
499 Cylinder>::distance; distance_matrix[BV_KDOP18][GEOM_CONVEX] =
500 &BVHShapeDistancer<KDOP<18>, ConvexBase>::distance;
501 distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer<KDOP<18>,
502 Plane>::distance; distance_matrix[BV_KDOP18][GEOM_HALFSPACE] =
503 &BVHShapeDistancer<KDOP<18>, Halfspace>::distance;
504
505 distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer<KDOP<24>,
506 Box>::distance; distance_matrix[BV_KDOP24][GEOM_SPHERE] =
507 &BVHShapeDistancer<KDOP<24>, Sphere>::distance;
508 distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<24>,
509 Capsule>::distance; distance_matrix[BV_KDOP24][GEOM_CONE] =
510 &BVHShapeDistancer<KDOP<24>, Cone>::distance;
511 distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<24>,
512 Cylinder>::distance; distance_matrix[BV_KDOP24][GEOM_CONVEX] =
513 &BVHShapeDistancer<KDOP<24>, ConvexBase>::distance;
514 distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer<KDOP<24>,
515 Plane>::distance; distance_matrix[BV_KDOP24][GEOM_HALFSPACE] =
516 &BVHShapeDistancer<KDOP<24>, Halfspace>::distance;
517 */
518
519 10 distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer<kIOS, Box>::distance;
520 10 distance_matrix[BV_kIOS][GEOM_SPHERE] =
521 &BVHShapeDistancer<kIOS, Sphere>::distance;
522 10 distance_matrix[BV_kIOS][GEOM_CAPSULE] =
523 &BVHShapeDistancer<kIOS, Capsule>::distance;
524 10 distance_matrix[BV_kIOS][GEOM_CONE] =
525 &BVHShapeDistancer<kIOS, Cone>::distance;
526 10 distance_matrix[BV_kIOS][GEOM_CYLINDER] =
527 &BVHShapeDistancer<kIOS, Cylinder>::distance;
528 10 distance_matrix[BV_kIOS][GEOM_CONVEX] =
529 &BVHShapeDistancer<kIOS, ConvexBase>::distance;
530 10 distance_matrix[BV_kIOS][GEOM_PLANE] =
531 &BVHShapeDistancer<kIOS, Plane>::distance;
532 10 distance_matrix[BV_kIOS][GEOM_HALFSPACE] =
533 &BVHShapeDistancer<kIOS, Halfspace>::distance;
534 10 distance_matrix[BV_kIOS][GEOM_ELLIPSOID] =
535 &BVHShapeDistancer<kIOS, Ellipsoid>::distance;
536
537 10 distance_matrix[BV_OBBRSS][GEOM_BOX] =
538 &BVHShapeDistancer<OBBRSS, Box>::distance;
539 10 distance_matrix[BV_OBBRSS][GEOM_SPHERE] =
540 &BVHShapeDistancer<OBBRSS, Sphere>::distance;
541 10 distance_matrix[BV_OBBRSS][GEOM_CAPSULE] =
542 &BVHShapeDistancer<OBBRSS, Capsule>::distance;
543 10 distance_matrix[BV_OBBRSS][GEOM_CONE] =
544 &BVHShapeDistancer<OBBRSS, Cone>::distance;
545 10 distance_matrix[BV_OBBRSS][GEOM_CYLINDER] =
546 &BVHShapeDistancer<OBBRSS, Cylinder>::distance;
547 10 distance_matrix[BV_OBBRSS][GEOM_CONVEX] =
548 &BVHShapeDistancer<OBBRSS, ConvexBase>::distance;
549 10 distance_matrix[BV_OBBRSS][GEOM_PLANE] =
550 &BVHShapeDistancer<OBBRSS, Plane>::distance;
551 10 distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] =
552 &BVHShapeDistancer<OBBRSS, Halfspace>::distance;
553 10 distance_matrix[BV_OBBRSS][GEOM_ELLIPSOID] =
554 &BVHShapeDistancer<OBBRSS, Ellipsoid>::distance;
555
556 10 distance_matrix[HF_AABB][GEOM_BOX] =
557 &HeightFieldShapeDistancer<AABB, Box>::distance;
558 10 distance_matrix[HF_AABB][GEOM_SPHERE] =
559 &HeightFieldShapeDistancer<AABB, Sphere>::distance;
560 10 distance_matrix[HF_AABB][GEOM_CAPSULE] =
561 &HeightFieldShapeDistancer<AABB, Capsule>::distance;
562 10 distance_matrix[HF_AABB][GEOM_CONE] =
563 &HeightFieldShapeDistancer<AABB, Cone>::distance;
564 10 distance_matrix[HF_AABB][GEOM_CYLINDER] =
565 &HeightFieldShapeDistancer<AABB, Cylinder>::distance;
566 10 distance_matrix[HF_AABB][GEOM_CONVEX] =
567 &HeightFieldShapeDistancer<AABB, ConvexBase>::distance;
568 10 distance_matrix[HF_AABB][GEOM_PLANE] =
569 &HeightFieldShapeDistancer<AABB, Plane>::distance;
570 10 distance_matrix[HF_AABB][GEOM_HALFSPACE] =
571 &HeightFieldShapeDistancer<AABB, Halfspace>::distance;
572 10 distance_matrix[HF_AABB][GEOM_ELLIPSOID] =
573 &HeightFieldShapeDistancer<AABB, Ellipsoid>::distance;
574
575 10 distance_matrix[HF_OBBRSS][GEOM_BOX] =
576 &HeightFieldShapeDistancer<OBBRSS, Box>::distance;
577 10 distance_matrix[HF_OBBRSS][GEOM_SPHERE] =
578 &HeightFieldShapeDistancer<OBBRSS, Sphere>::distance;
579 10 distance_matrix[HF_OBBRSS][GEOM_CAPSULE] =
580 &HeightFieldShapeDistancer<OBBRSS, Capsule>::distance;
581 10 distance_matrix[HF_OBBRSS][GEOM_CONE] =
582 &HeightFieldShapeDistancer<OBBRSS, Cone>::distance;
583 10 distance_matrix[HF_OBBRSS][GEOM_CYLINDER] =
584 &HeightFieldShapeDistancer<OBBRSS, Cylinder>::distance;
585 10 distance_matrix[HF_OBBRSS][GEOM_CONVEX] =
586 &HeightFieldShapeDistancer<OBBRSS, ConvexBase>::distance;
587 10 distance_matrix[HF_OBBRSS][GEOM_PLANE] =
588 &HeightFieldShapeDistancer<OBBRSS, Plane>::distance;
589 10 distance_matrix[HF_OBBRSS][GEOM_HALFSPACE] =
590 &HeightFieldShapeDistancer<OBBRSS, Halfspace>::distance;
591 10 distance_matrix[HF_OBBRSS][GEOM_ELLIPSOID] =
592 &HeightFieldShapeDistancer<OBBRSS, Ellipsoid>::distance;
593
594 10 distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB>;
595 10 distance_matrix[BV_OBB][BV_OBB] = &BVHDistance<OBB>;
596 10 distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS>;
597 10 distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS>;
598 10 distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS>;
599
600 #ifdef COAL_HAS_OCTOMAP
601 10 distance_matrix[GEOM_OCTREE][GEOM_BOX] = &Distance<OcTree, Box>;
602 10 distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &Distance<OcTree, Sphere>;
603 10 distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &Distance<OcTree, Capsule>;
604 10 distance_matrix[GEOM_OCTREE][GEOM_CONE] = &Distance<OcTree, Cone>;
605 10 distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &Distance<OcTree, Cylinder>;
606 10 distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &Distance<OcTree, ConvexBase>;
607 10 distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &Distance<OcTree, Plane>;
608 10 distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &Distance<OcTree, Halfspace>;
609 10 distance_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &Distance<OcTree, Ellipsoid>;
610
611 10 distance_matrix[GEOM_BOX][GEOM_OCTREE] = &Distance<Box, OcTree>;
612 10 distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &Distance<Sphere, OcTree>;
613 10 distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &Distance<Capsule, OcTree>;
614 10 distance_matrix[GEOM_CONE][GEOM_OCTREE] = &Distance<Cone, OcTree>;
615 10 distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &Distance<Cylinder, OcTree>;
616 10 distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &Distance<ConvexBase, OcTree>;
617 10 distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &Distance<Plane, OcTree>;
618 10 distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &Distance<Halfspace, OcTree>;
619
620 10 distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &Distance<OcTree, OcTree>;
621
622 10 distance_matrix[GEOM_OCTREE][BV_AABB] = &Distance<OcTree, BVHModel<AABB> >;
623 10 distance_matrix[GEOM_OCTREE][BV_OBB] = &Distance<OcTree, BVHModel<OBB> >;
624 10 distance_matrix[GEOM_OCTREE][BV_RSS] = &Distance<OcTree, BVHModel<RSS> >;
625 10 distance_matrix[GEOM_OCTREE][BV_OBBRSS] =
626 &Distance<OcTree, BVHModel<OBBRSS> >;
627 10 distance_matrix[GEOM_OCTREE][BV_kIOS] = &Distance<OcTree, BVHModel<kIOS> >;
628 10 distance_matrix[GEOM_OCTREE][BV_KDOP16] =
629 &Distance<OcTree, BVHModel<KDOP<16> > >;
630 10 distance_matrix[GEOM_OCTREE][BV_KDOP18] =
631 &Distance<OcTree, BVHModel<KDOP<18> > >;
632 10 distance_matrix[GEOM_OCTREE][BV_KDOP24] =
633 &Distance<OcTree, BVHModel<KDOP<24> > >;
634
635 10 distance_matrix[BV_AABB][GEOM_OCTREE] = &Distance<BVHModel<AABB>, OcTree>;
636 10 distance_matrix[BV_OBB][GEOM_OCTREE] = &Distance<BVHModel<OBB>, OcTree>;
637 10 distance_matrix[BV_RSS][GEOM_OCTREE] = &Distance<BVHModel<RSS>, OcTree>;
638 10 distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &Distance<BVHModel<OBBRSS>, OcTree>;
639 10 distance_matrix[BV_kIOS][GEOM_OCTREE] = &Distance<BVHModel<kIOS>, OcTree>;
640 10 distance_matrix[BV_KDOP16][GEOM_OCTREE] =
641 &Distance<BVHModel<KDOP<16> >, OcTree>;
642 10 distance_matrix[BV_KDOP18][GEOM_OCTREE] =
643 &Distance<BVHModel<KDOP<18> >, OcTree>;
644 10 distance_matrix[BV_KDOP24][GEOM_OCTREE] =
645 &Distance<BVHModel<KDOP<24> >, OcTree>;
646 10 distance_matrix[GEOM_OCTREE][HF_AABB] = &distance_function_not_implemented;
647 10 distance_matrix[GEOM_OCTREE][HF_OBBRSS] = &distance_function_not_implemented;
648 10 distance_matrix[HF_AABB][GEOM_OCTREE] = &distance_function_not_implemented;
649 10 distance_matrix[HF_OBBRSS][GEOM_OCTREE] = &distance_function_not_implemented;
650 #endif
651 10 }
652 // template struct DistanceFunctionMatrix;
653 } // namespace coal
654