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 |