Directory: | ./ |
---|---|
File: | src/broadphase/broadphase_bruteforce.cpp |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 48 | 78 | 61.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 |