GCC Code Coverage Report

Directory: ./
File: src/broadphase/broadphase_bruteforce.cpp
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 48 80 60.0%
Functions: 10 16 62.5%
Branches: 41 104 39.4%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2016, 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/broadphase/broadphase_bruteforce.h"
39
40 #include <iterator>
41 #include <algorithm>
42 #include "coal/tracy.hh"
43
44 namespace coal {
45
46 //==============================================================================
47 59 NaiveCollisionManager::NaiveCollisionManager() {
48 // Do nothing
49 59 }
50
51 //==============================================================================
52 51 void NaiveCollisionManager::registerObjects(
53 const std::vector<CollisionObject*>& other_objs) {
54 51 std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs));
55 51 }
56
57 //==============================================================================
58 ✗ void NaiveCollisionManager::unregisterObject(CollisionObject* obj) {
59 ✗ objs.remove(obj);
60 ✗ }
61
62 //==============================================================================
63 7260 void NaiveCollisionManager::registerObject(CollisionObject* obj) {
64 7260 objs.push_back(obj);
65 7260 }
66
67 //==============================================================================
68 59 void NaiveCollisionManager::setup() {
69 // Do nothing
70 59 }
71
72 //==============================================================================
73 13 void NaiveCollisionManager::update() {
74 // Do nothing
75 13 }
76
77 //==============================================================================
78 ✗ void NaiveCollisionManager::clear() { objs.clear(); }
79
80 //==============================================================================
81 ✗ void NaiveCollisionManager::getObjects(
82 std::vector<CollisionObject*>& objs_) const {
83 ✗ objs_.resize(objs.size());
84 ✗ std::copy(objs.begin(), objs.end(), objs_.begin());
85 ✗ }
86
87 //==============================================================================
88 4646 void NaiveCollisionManager::collide(CollisionObject* obj,
89 CollisionCallBackBase* callback) const {
90 COAL_TRACY_ZONE_SCOPED_N(
91 "coal::NaiveCollisionManager::collide(CollisionObject*, "
92 "CollisionCallBackBase*)");
93 4646 callback->init();
94
2/2
✓ Branch 1 taken 60 times.
✓ Branch 2 taken 4586 times.
 4646 if (size() == 0) return;
95
96
2/2
✓ Branch 5 taken 1221354 times.
✓ Branch 6 taken 3841 times.
 1225195 for (auto* obj2 : objs) {
97
3/4
✓ Branch 1 taken 1221354 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 745 times.
✓ Branch 4 taken 1220609 times.
 1221354 if ((*callback)(obj, obj2)) return;
98 }
99 }
100
101 //==============================================================================
102 80 void NaiveCollisionManager::distance(CollisionObject* obj,
103 DistanceCallBackBase* callback) const {
104 COAL_TRACY_ZONE_SCOPED_N(
105 "coal::NaiveCollisionManager::distance(CollisionObject*, "
106 "DistanceCallBackBase*)");
107
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 callback->init();
108
2/4
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 80 times.
 80 if (size() == 0) return;
109
110 80 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
111
2/2
✓ Branch 5 taken 72600 times.
✓ Branch 6 taken 80 times.
 72680 for (auto* obj2 : objs) {
112
3/4
✓ Branch 3 taken 72600 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1300 times.
✓ Branch 6 taken 71300 times.
 72600 if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
113
2/4
✓ Branch 1 taken 1300 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1300 times.
 1300 if ((*callback)(obj, obj2, min_dist)) return;
114 }
115 }
116 }
117
118 //==============================================================================
119 37 void NaiveCollisionManager::collide(CollisionCallBackBase* callback) const {
120 COAL_TRACY_ZONE_SCOPED_N(
121 "coal::NaiveCollisionManager::collide(CollisionCallBackBase*)");
122 37 callback->init();
123
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 29 times.
 37 if (size() == 0) return;
124
125 58 for (typename std::list<CollisionObject*>::const_iterator it1 = objs.begin(),
126 29 end = objs.end();
127
2/2
✓ Branch 2 taken 2027 times.
✓ Branch 3 taken 26 times.
 2053 it1 != end; ++it1) {
128 2027 typename std::list<CollisionObject*>::const_iterator it2 = it1;
129 2027 it2++;
130
2/2
✓ Branch 2 taken 279572 times.
✓ Branch 3 taken 2024 times.
 281596 for (; it2 != end; ++it2) {
131
3/4
✓ Branch 5 taken 279572 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 7 times.
✓ Branch 8 taken 279565 times.
 279572 if ((*it1)->getAABB().overlap((*it2)->getAABB())) {
132
3/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
✓ Branch 6 taken 4 times.
 7 if ((*callback)(*it1, *it2)) return;
133 }
134 }
135 }
136 }
137
138 //==============================================================================
139 6 void NaiveCollisionManager::distance(DistanceCallBackBase* callback) const {
140 COAL_TRACY_ZONE_SCOPED_N(
141 "coal::NaiveCollisionManager::distance(CollisionCallBackBase*)");
142
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 callback->init();
143
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
 6 if (size() == 0) return;
144
145 6 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
146 12 for (typename std::list<CollisionObject*>::const_iterator it1 = objs.begin(),
147 6 end = objs.end();
148
2/2
✓ Branch 2 taken 2990 times.
✓ Branch 3 taken 6 times.
 2996 it1 != end; ++it1) {
149 2990 typename std::list<CollisionObject*>::const_iterator it2 = it1;
150 2990 it2++;
151
2/2
✓ Branch 2 taken 1546838 times.
✓ Branch 3 taken 2990 times.
 1549828 for (; it2 != end; ++it2) {
152
3/4
✓ Branch 5 taken 1546838 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 245 times.
✓ Branch 8 taken 1546593 times.
 1546838 if ((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist) {
153
2/4
✓ Branch 3 taken 245 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 245 times.
 245 if ((*callback)(*it1, *it2, min_dist)) return;
154 }
155 }
156 }
157 }
158
159 //==============================================================================
160 ✗ void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_,
161 CollisionCallBackBase* callback) const {
162 COAL_TRACY_ZONE_SCOPED_N(
163 "coal::NaiveCollisionManager::collide(BroadPhaseCollisionManager*, "
164 "CollisionCallBackBase*)");
165 ✗ callback->init();
166 ✗ NaiveCollisionManager* other_manager =
167 static_cast<NaiveCollisionManager*>(other_manager_);
168
169 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
170
171 ✗ if (this == other_manager) {
172 ✗ collide(callback);
173 ✗ return;
174 }
175
176 ✗ for (auto* obj1 : objs) {
177 ✗ for (auto* obj2 : other_manager->objs) {
178 ✗ if (obj1->getAABB().overlap(obj2->getAABB())) {
179 ✗ if ((*callback)(obj1, obj2)) return;
180 }
181 }
182 }
183 }
184
185 //==============================================================================
186 ✗ void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_,
187 DistanceCallBackBase* callback) const {
188 COAL_TRACY_ZONE_SCOPED_N(
189 "coal::NaiveCollisionManager::distance(BroadPhaseCollisionManager*, "
190 "DistanceCallBackBase*)");
191 ✗ callback->init();
192 ✗ NaiveCollisionManager* other_manager =
193 static_cast<NaiveCollisionManager*>(other_manager_);
194
195 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
196
197 ✗ if (this == other_manager) {
198 ✗ distance(callback);
199 ✗ return;
200 }
201
202 ✗ Scalar min_dist = (std::numeric_limits<Scalar>::max)();
203 ✗ for (auto* obj1 : objs) {
204 ✗ for (auto* obj2 : other_manager->objs) {
205 ✗ if (obj1->getAABB().distance(obj2->getAABB()) < min_dist) {
206 ✗ if ((*callback)(obj1, obj2, min_dist)) return;
207 }
208 }
209 }
210 }
211
212 //==============================================================================
213 ✗ bool NaiveCollisionManager::empty() const { return objs.empty(); }
214
215 //==============================================================================
216 4769 size_t NaiveCollisionManager::size() const { return objs.size(); }
217
218 } // namespace coal
219