GCC Code Coverage Report


Directory: ./
File: src/distance.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 23 66 34.8%
Branches: 19 208 9.1%

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.h"
39 #include "coal/collision_utility.h"
40 #include "coal/distance_func_matrix.h"
41 #include "coal/narrowphase/narrowphase.h"
42
43 #include "coal/tracy.hh"
44
45 namespace coal {
46
47 45878 DistanceFunctionMatrix& getDistanceFunctionLookTable() {
48
4/8
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 45868 times.
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
45878 static DistanceFunctionMatrix table;
49 45878 return table;
50 }
51
52 35226 Scalar distance(const CollisionObject* o1, const CollisionObject* o2,
53 const DistanceRequest& request, DistanceResult& result) {
54 35226 return distance(o1->collisionGeometryPtr(), o1->getTransform(),
55 o2->collisionGeometryPtr(), o2->getTransform(), request,
56 35226 result);
57 }
58
59 45878 Scalar distance(const CollisionGeometry* o1, const Transform3s& tf1,
60 const CollisionGeometry* o2, const Transform3s& tf2,
61 const DistanceRequest& request, DistanceResult& result) {
62 COAL_TRACY_ZONE_SCOPED_N("coal::distance");
63
1/2
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
45878 GJKSolver solver(request);
64
65
1/2
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
45878 const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable();
66
67
1/2
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
45878 OBJECT_TYPE object_type1 = o1->getObjectType();
68
1/2
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
45878 NODE_TYPE node_type1 = o1->getNodeType();
69
1/2
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
45878 OBJECT_TYPE object_type2 = o2->getObjectType();
70
1/2
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
45878 NODE_TYPE node_type2 = o2->getNodeType();
71
72 45878 Scalar res = (std::numeric_limits<Scalar>::max)();
73
74
3/4
✓ Branch 0 taken 38631 times.
✓ Branch 1 taken 7247 times.
✓ Branch 2 taken 38631 times.
✗ Branch 3 not taken.
45878 if (object_type1 == OT_GEOM &&
75
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 38631 times.
38631 (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) {
76 if (!looktable.distance_matrix[node_type2][node_type1]) {
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 } else {
84 res = looktable.distance_matrix[node_type2][node_type1](
85 o2, tf2, o1, tf1, &solver, request, result);
86 std::swap(result.o1, result.o2);
87 result.nearest_points[0].swap(result.nearest_points[1]);
88 result.normal *= -1;
89 }
90 } else {
91
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 45878 times.
45878 if (!looktable.distance_matrix[node_type1][node_type2]) {
92 COAL_THROW_PRETTY("Distance function between node type "
93 << std::string(get_node_type_name(node_type1))
94 << " and node type "
95 << std::string(get_node_type_name(node_type2))
96 << " is not yet supported.",
97 std::invalid_argument);
98 } else {
99
1/2
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
45878 res = looktable.distance_matrix[node_type1][node_type2](
100 o1, tf1, o2, tf2, &solver, request, result);
101 }
102 }
103 // Cache narrow phase solver result. If the option in the request is selected,
104 // also store the solver result in the request for the next call.
105
1/2
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
45878 result.cached_gjk_guess = solver.cached_guess;
106
1/2
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
45878 result.cached_support_func_guess = solver.support_func_cached_guess;
107
1/2
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
45878 request.updateGuess(result);
108 45878 return res;
109 45878 }
110
111 ComputeDistance::ComputeDistance(const CollisionGeometry* o1,
112 const CollisionGeometry* o2)
113 : o1(o1), o2(o2) {
114 const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable();
115
116 OBJECT_TYPE object_type1 = o1->getObjectType();
117 NODE_TYPE node_type1 = o1->getNodeType();
118 OBJECT_TYPE object_type2 = o2->getObjectType();
119 NODE_TYPE node_type2 = o2->getNodeType();
120
121 swap_geoms = object_type1 == OT_GEOM &&
122 (object_type2 == OT_BVH || object_type2 == OT_HFIELD);
123
124 if ((swap_geoms && !looktable.distance_matrix[node_type2][node_type1]) ||
125 (!swap_geoms && !looktable.distance_matrix[node_type1][node_type2])) {
126 COAL_THROW_PRETTY("Distance function between node type "
127 << std::string(get_node_type_name(node_type1))
128 << " and node type "
129 << std::string(get_node_type_name(node_type2))
130 << " is not yet supported.",
131 std::invalid_argument);
132 }
133 if (swap_geoms)
134 func = looktable.distance_matrix[node_type2][node_type1];
135 else
136 func = looktable.distance_matrix[node_type1][node_type2];
137 }
138
139 Scalar ComputeDistance::run(const Transform3s& tf1, const Transform3s& tf2,
140 const DistanceRequest& request,
141 DistanceResult& result) const {
142 COAL_TRACY_ZONE_SCOPED_N("coal::ComputeDistance::run");
143 Scalar res;
144
145 if (swap_geoms) {
146 res = func(o2, tf2, o1, tf1, &solver, request, result);
147 std::swap(result.o1, result.o2);
148 result.nearest_points[0].swap(result.nearest_points[1]);
149 result.normal *= -1;
150 } else {
151 res = func(o1, tf1, o2, tf2, &solver, request, result);
152 }
153 // Cache narrow phase solver result. If the option in the request is selected,
154 // also store the solver result in the request for the next call.
155 result.cached_gjk_guess = solver.cached_guess;
156 result.cached_support_func_guess = solver.support_func_cached_guess;
157 request.updateGuess(result);
158 return res;
159 }
160
161 Scalar ComputeDistance::operator()(const Transform3s& tf1,
162 const Transform3s& tf2,
163 const DistanceRequest& request,
164 DistanceResult& result) const {
165 solver.set(request);
166
167 Scalar res;
168 if (request.enable_timings) {
169 Timer timer;
170 res = run(tf1, tf2, request, result);
171 result.timings = timer.elapsed();
172 } else
173 res = run(tf1, tf2, request, result);
174 return res;
175 }
176
177 } // namespace coal
178