GCC Code Coverage Report

Directory: ./
File: src/collision.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 62 82 75.6%
Branches: 44 254 17.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/collision.h"
39 #include "coal/collision_utility.h"
40 #include "coal/collision_func_matrix.h"
41 #include "coal/narrowphase/narrowphase.h"
42
43 #include "coal/tracy.hh"
44
45 namespace coal {
46
47 1247939 CollisionFunctionMatrix& getCollisionFunctionLookTable() {
48
4/8
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 1247917 times.
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 1247939 static CollisionFunctionMatrix table;
49 1247939 return table;
50 }
51
52 // reorder collision results in the order the call has been made.
53 20 void CollisionResult::swapObjects() {
54 20 for (std::vector<Contact>::iterator it = contacts.begin();
55
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 20 times.
 20 it != contacts.end(); ++it) {
56 ✗ std::swap(it->o1, it->o2);
57 ✗ std::swap(it->b1, it->b2);
58 ✗ it->nearest_points[0].swap(it->nearest_points[1]);
59 ✗ it->normal *= -1;
60 }
61 20 }
62
63 1230586 std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
64 const CollisionRequest& request, CollisionResult& result) {
65 1230586 return collide(o1->collisionGeometryPtr(), o1->getTransform(),
66 o2->collisionGeometryPtr(), o2->getTransform(), request,
67 1230586 result);
68 }
69
70 1247936 std::size_t collide(const CollisionGeometry* o1, const Transform3s& tf1,
71 const CollisionGeometry* o2, const Transform3s& tf2,
72 const CollisionRequest& request, CollisionResult& result) {
73 COAL_TRACY_ZONE_SCOPED_N("coal::collide");
74 // If security margin is set to -infinity, return that there is no collision
75
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1247936 times.
 1247936 if (request.security_margin == -std::numeric_limits<Scalar>::infinity()) {
76 ✗ result.clear();
77 ✗ return false;
78 }
79
80
1/2
✓ Branch 1 taken 1247936 times.
✗ Branch 2 not taken.
 1247936 GJKSolver solver(request);
81
82
1/2
✓ Branch 1 taken 1247936 times.
✗ Branch 2 not taken.
 1247936 const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
83 std::size_t res;
84
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1247936 times.
 1247936 if (request.num_max_contacts == 0) {
85 ✗ COAL_THROW_PRETTY("Invalid number of max contacts (current value is 0).",
86 std::invalid_argument);
87 res = 0;
88 } else {
89
1/2
✓ Branch 1 taken 1247936 times.
✗ Branch 2 not taken.
 1247936 OBJECT_TYPE object_type1 = o1->getObjectType();
90
1/2
✓ Branch 1 taken 1247936 times.
✗ Branch 2 not taken.
 1247936 OBJECT_TYPE object_type2 = o2->getObjectType();
91
1/2
✓ Branch 1 taken 1247936 times.
✗ Branch 2 not taken.
 1247936 NODE_TYPE node_type1 = o1->getNodeType();
92
1/2
✓ Branch 1 taken 1247936 times.
✗ Branch 2 not taken.
 1247936 NODE_TYPE node_type2 = o2->getNodeType();
93
94
4/4
✓ Branch 0 taken 1234495 times.
✓ Branch 1 taken 13441 times.
✓ Branch 2 taken 1234475 times.
✓ Branch 3 taken 20 times.
 1247936 if (object_type1 == OT_GEOM &&
95
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1234475 times.
 1234475 (object_type2 == OT_BVH || object_type2 == OT_HFIELD)) {
96
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20 times.
 20 if (!looktable.collision_matrix[node_type2][node_type1]) {
97 ✗ COAL_THROW_PRETTY("Collision function between node type "
98 << std::string(get_node_type_name(node_type1))
99 << " and node type "
100 << std::string(get_node_type_name(node_type2))
101 << " is not yet supported.",
102 std::invalid_argument);
103 res = 0;
104 } else {
105
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 res = looktable.collision_matrix[node_type2][node_type1](
106 o2, tf2, o1, tf1, &solver, request, result);
107
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 result.swapObjects();
108
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 20 result.nearest_points[0].swap(result.nearest_points[1]);
109
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 result.normal *= -1;
110 }
111 20 } else {
112
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1247916 times.
 1247916 if (!looktable.collision_matrix[node_type1][node_type2]) {
113 ✗ COAL_THROW_PRETTY("Collision function between node type "
114 << std::string(get_node_type_name(node_type1))
115 << " and node type "
116 << std::string(get_node_type_name(node_type2))
117 << " is not yet supported.",
118 std::invalid_argument);
119 res = 0;
120 } else
121
1/2
✓ Branch 1 taken 1247916 times.
✗ Branch 2 not taken.
 1247916 res = looktable.collision_matrix[node_type1][node_type2](
122 o1, tf1, o2, tf2, &solver, request, result);
123 }
124 }
125 // Cache narrow phase solver result. If the option in the request is selected,
126 // also store the solver result in the request for the next call.
127
1/2
✓ Branch 1 taken 1247936 times.
✗ Branch 2 not taken.
 1247936 result.cached_gjk_guess = solver.cached_guess;
128
1/2
✓ Branch 1 taken 1247936 times.
✗ Branch 2 not taken.
 1247936 result.cached_support_func_guess = solver.support_func_cached_guess;
129
1/2
✓ Branch 1 taken 1247936 times.
✗ Branch 2 not taken.
 1247936 request.updateGuess(result);
130
131 1247936 return res;
132 1247936 }
133
134 3 ComputeCollision::ComputeCollision(const CollisionGeometry* o1,
135 3 const CollisionGeometry* o2)
136 3 : o1(o1), o2(o2) {
137
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
138
139
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 OBJECT_TYPE object_type1 = o1->getObjectType();
140
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 NODE_TYPE node_type1 = o1->getNodeType();
141
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 OBJECT_TYPE object_type2 = o2->getObjectType();
142
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 NODE_TYPE node_type2 = o2->getNodeType();
143
144
3/4
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 4 swap_geoms = object_type1 == OT_GEOM &&
145
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 (object_type2 == OT_BVH || object_type2 == OT_HFIELD);
146
147
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 3 if ((swap_geoms && !looktable.collision_matrix[node_type2][node_type1]) ||
148
2/4
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
 3 (!swap_geoms && !looktable.collision_matrix[node_type1][node_type2])) {
149 ✗ COAL_THROW_PRETTY("Collision function between node type "
150 << std::string(get_node_type_name(node_type1))
151 << " and node type "
152 << std::string(get_node_type_name(node_type2))
153 << " is not yet supported.",
154 std::invalid_argument);
155 }
156
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 3 if (swap_geoms)
157 ✗ func = looktable.collision_matrix[node_type2][node_type1];
158 else
159 3 func = looktable.collision_matrix[node_type1][node_type2];
160 3 }
161
162 482 std::size_t ComputeCollision::run(const Transform3s& tf1,
163 const Transform3s& tf2,
164 const CollisionRequest& request,
165 CollisionResult& result) const {
166 COAL_TRACY_ZONE_SCOPED_N("coal::ComputeCollision::run");
167 // If security margin is set to -infinity, return that there is no collision
168
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 482 times.
 482 if (request.security_margin == -std::numeric_limits<Scalar>::infinity()) {
169 ✗ result.clear();
170 ✗ return false;
171 }
172 std::size_t res;
173
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 482 times.
 482 if (swap_geoms) {
174 ✗ res = func(o2, tf2, o1, tf1, &solver, request, result);
175 ✗ result.swapObjects();
176 ✗ result.nearest_points[0].swap(result.nearest_points[1]);
177 ✗ result.normal *= -1;
178 } else {
179 482 res = func(o1, tf1, o2, tf2, &solver, request, result);
180 }
181 // Cache narrow phase solver result. If the option in the request is selected,
182 // also store the solver result in the request for the next call.
183 482 result.cached_gjk_guess = solver.cached_guess;
184 482 result.cached_support_func_guess = solver.support_func_cached_guess;
185 482 request.updateGuess(result);
186
187 482 return res;
188 }
189
190 482 std::size_t ComputeCollision::operator()(const Transform3s& tf1,
191 const Transform3s& tf2,
192 const CollisionRequest& request,
193 CollisionResult& result) const
194
195 {
196 482 solver.set(request);
197
198 std::size_t res;
199
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 482 times.
 482 if (request.enable_timings) {
200 ✗ Timer timer;
201 ✗ res = run(tf1, tf2, request, result);
202 ✗ result.timings = timer.elapsed();
203 } else
204 482 res = run(tf1, tf2, request, result);
205
206 482 return res;
207 }
208
209 } // namespace coal
210