Directory: | ./ |
---|---|
File: | src/broadphase/broadphase_interval_tree.cpp |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 220 | 360 | 61.1% |
Branches: | 222 | 559 | 39.7% |
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_interval_tree.h" | ||
39 | |||
40 | namespace coal { | ||
41 | |||
42 | //============================================================================== | ||
43 | ✗ | void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) { | |
44 | // must sorted before | ||
45 | ✗ | setup(); | |
46 | |||
47 | EndPoint p; | ||
48 | ✗ | p.value = obj->getAABB().min_[0]; | |
49 | ✗ | auto start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p); | |
50 | ✗ | p.value = obj->getAABB().max_[0]; | |
51 | ✗ | auto end1 = std::upper_bound(start1, endpoints[0].end(), p); | |
52 | |||
53 | ✗ | if (start1 < end1) { | |
54 | ✗ | size_t start_id = (size_t)(start1 - endpoints[0].begin()); | |
55 | ✗ | size_t end_id = (size_t)(end1 - endpoints[0].begin()); | |
56 | ✗ | size_t cur_id = (size_t)(start_id); | |
57 | |||
58 | ✗ | for (size_t i = start_id; i < end_id; ++i) { | |
59 | ✗ | if (endpoints[0][(size_t)i].obj != obj) { | |
60 | ✗ | if (i == cur_id) | |
61 | ✗ | cur_id++; | |
62 | else { | ||
63 | ✗ | endpoints[0][(size_t)cur_id] = endpoints[0][(size_t)i]; | |
64 | ✗ | cur_id++; | |
65 | } | ||
66 | } | ||
67 | } | ||
68 | ✗ | if (cur_id < end_id) endpoints[0].resize(endpoints[0].size() - 2); | |
69 | } | ||
70 | |||
71 | ✗ | p.value = obj->getAABB().min_[1]; | |
72 | ✗ | auto start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p); | |
73 | ✗ | p.value = obj->getAABB().max_[1]; | |
74 | ✗ | auto end2 = std::upper_bound(start2, endpoints[1].end(), p); | |
75 | |||
76 | ✗ | if (start2 < end2) { | |
77 | ✗ | size_t start_id = (size_t)(start2 - endpoints[1].begin()); | |
78 | ✗ | size_t end_id = (size_t)(end2 - endpoints[1].begin()); | |
79 | ✗ | size_t cur_id = (size_t)(start_id); | |
80 | |||
81 | ✗ | for (size_t i = start_id; i < end_id; ++i) { | |
82 | ✗ | if (endpoints[1][i].obj != obj) { | |
83 | ✗ | if (i == cur_id) | |
84 | ✗ | cur_id++; | |
85 | else { | ||
86 | ✗ | endpoints[1][cur_id] = endpoints[1][i]; | |
87 | ✗ | cur_id++; | |
88 | } | ||
89 | } | ||
90 | } | ||
91 | ✗ | if (cur_id < end_id) endpoints[1].resize(endpoints[1].size() - 2); | |
92 | } | ||
93 | |||
94 | ✗ | p.value = obj->getAABB().min_[2]; | |
95 | ✗ | auto start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p); | |
96 | ✗ | p.value = obj->getAABB().max_[2]; | |
97 | ✗ | auto end3 = std::upper_bound(start3, endpoints[2].end(), p); | |
98 | |||
99 | ✗ | if (start3 < end3) { | |
100 | ✗ | size_t start_id = (size_t)(start3 - endpoints[2].begin()); | |
101 | ✗ | size_t end_id = (size_t)(end3 - endpoints[2].begin()); | |
102 | ✗ | size_t cur_id = (size_t)(start_id); | |
103 | |||
104 | ✗ | for (size_t i = start_id; i < end_id; ++i) { | |
105 | ✗ | if (endpoints[2][i].obj != obj) { | |
106 | ✗ | if (i == cur_id) | |
107 | ✗ | cur_id++; | |
108 | else { | ||
109 | ✗ | endpoints[2][cur_id] = endpoints[2][i]; | |
110 | ✗ | cur_id++; | |
111 | } | ||
112 | } | ||
113 | } | ||
114 | ✗ | if (cur_id < end_id) endpoints[2].resize(endpoints[2].size() - 2); | |
115 | } | ||
116 | |||
117 | // update the interval tree | ||
118 | ✗ | if (obj_interval_maps[0].find(obj) != obj_interval_maps[0].end()) { | |
119 | ✗ | SAPInterval* ivl1 = obj_interval_maps[0][obj]; | |
120 | ✗ | SAPInterval* ivl2 = obj_interval_maps[1][obj]; | |
121 | ✗ | SAPInterval* ivl3 = obj_interval_maps[2][obj]; | |
122 | |||
123 | ✗ | interval_trees[0]->deleteNode(ivl1); | |
124 | ✗ | interval_trees[1]->deleteNode(ivl2); | |
125 | ✗ | interval_trees[2]->deleteNode(ivl3); | |
126 | |||
127 | ✗ | delete ivl1; | |
128 | ✗ | delete ivl2; | |
129 | ✗ | delete ivl3; | |
130 | |||
131 | ✗ | obj_interval_maps[0].erase(obj); | |
132 | ✗ | obj_interval_maps[1].erase(obj); | |
133 | ✗ | obj_interval_maps[2].erase(obj); | |
134 | } | ||
135 | } | ||
136 | |||
137 | //============================================================================== | ||
138 |
4/4✓ Branch 2 taken 153 times.
✓ Branch 3 taken 51 times.
✓ Branch 5 taken 153 times.
✓ Branch 6 taken 51 times.
|
357 | IntervalTreeCollisionManager::IntervalTreeCollisionManager() : setup_(false) { |
139 |
2/2✓ Branch 0 taken 153 times.
✓ Branch 1 taken 51 times.
|
204 | for (int i = 0; i < 3; ++i) interval_trees[i] = nullptr; |
140 | 51 | } | |
141 | |||
142 | //============================================================================== | ||
143 |
6/8✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 150 times.
✓ Branch 4 taken 50 times.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 150 times.
✓ Branch 9 taken 50 times.
|
800 | IntervalTreeCollisionManager::~IntervalTreeCollisionManager() { clear(); } |
144 | |||
145 | //============================================================================== | ||
146 | 12671 | void IntervalTreeCollisionManager::registerObject(CollisionObject* obj) { | |
147 | EndPoint p, q; | ||
148 | |||
149 | 12671 | p.obj = obj; | |
150 | 12671 | q.obj = obj; | |
151 | 12671 | p.minmax = 0; | |
152 | 12671 | q.minmax = 1; | |
153 |
1/2✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
|
12671 | p.value = obj->getAABB().min_[0]; |
154 |
1/2✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
|
12671 | q.value = obj->getAABB().max_[0]; |
155 |
1/2✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
|
12671 | endpoints[0].push_back(p); |
156 |
1/2✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
|
12671 | endpoints[0].push_back(q); |
157 | |||
158 |
1/2✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
|
12671 | p.value = obj->getAABB().min_[1]; |
159 |
1/2✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
|
12671 | q.value = obj->getAABB().max_[1]; |
160 |
1/2✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
|
12671 | endpoints[1].push_back(p); |
161 |
1/2✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
|
12671 | endpoints[1].push_back(q); |
162 | |||
163 |
1/2✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
|
12671 | p.value = obj->getAABB().min_[2]; |
164 |
1/2✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
|
12671 | q.value = obj->getAABB().max_[2]; |
165 |
1/2✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
|
12671 | endpoints[2].push_back(p); |
166 |
1/2✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
|
12671 | endpoints[2].push_back(q); |
167 | 12671 | setup_ = false; | |
168 | 12671 | } | |
169 | |||
170 | //============================================================================== | ||
171 | 64 | void IntervalTreeCollisionManager::setup() { | |
172 |
1/2✓ Branch 0 taken 64 times.
✗ Branch 1 not taken.
|
64 | if (!setup_) { |
173 | 64 | std::sort(endpoints[0].begin(), endpoints[0].end()); | |
174 | 64 | std::sort(endpoints[1].begin(), endpoints[1].end()); | |
175 | 64 | std::sort(endpoints[2].begin(), endpoints[2].end()); | |
176 | |||
177 |
4/4✓ Branch 0 taken 39 times.
✓ Branch 1 taken 153 times.
✓ Branch 4 taken 192 times.
✓ Branch 5 taken 64 times.
|
256 | for (int i = 0; i < 3; ++i) delete interval_trees[i]; |
178 | |||
179 |
3/4✓ Branch 2 taken 192 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 192 times.
✓ Branch 5 taken 64 times.
|
256 | for (int i = 0; i < 3; ++i) interval_trees[i] = new detail::IntervalTree; |
180 | |||
181 |
2/2✓ Branch 1 taken 28030 times.
✓ Branch 2 taken 64 times.
|
28094 | for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) { |
182 | 28030 | EndPoint p = endpoints[0][i]; | |
183 | 28030 | CollisionObject* obj = p.obj; | |
184 |
2/2✓ Branch 0 taken 14015 times.
✓ Branch 1 taken 14015 times.
|
28030 | if (p.minmax == 0) { |
185 |
1/2✓ Branch 2 taken 14015 times.
✗ Branch 3 not taken.
|
14015 | SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], |
186 |
3/6✓ Branch 2 taken 14015 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14015 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14015 times.
✗ Branch 9 not taken.
|
14015 | obj->getAABB().max_[0], obj); |
187 |
1/2✓ Branch 2 taken 14015 times.
✗ Branch 3 not taken.
|
14015 | SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], |
188 |
3/6✓ Branch 2 taken 14015 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14015 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14015 times.
✗ Branch 9 not taken.
|
14015 | obj->getAABB().max_[1], obj); |
189 |
1/2✓ Branch 2 taken 14015 times.
✗ Branch 3 not taken.
|
14015 | SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], |
190 |
3/6✓ Branch 2 taken 14015 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14015 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14015 times.
✗ Branch 9 not taken.
|
14015 | obj->getAABB().max_[2], obj); |
191 | |||
192 |
1/2✓ Branch 1 taken 14015 times.
✗ Branch 2 not taken.
|
14015 | interval_trees[0]->insert(ivl1); |
193 |
1/2✓ Branch 1 taken 14015 times.
✗ Branch 2 not taken.
|
14015 | interval_trees[1]->insert(ivl2); |
194 |
1/2✓ Branch 1 taken 14015 times.
✗ Branch 2 not taken.
|
14015 | interval_trees[2]->insert(ivl3); |
195 | |||
196 |
1/2✓ Branch 1 taken 14015 times.
✗ Branch 2 not taken.
|
14015 | obj_interval_maps[0][obj] = ivl1; |
197 |
1/2✓ Branch 1 taken 14015 times.
✗ Branch 2 not taken.
|
14015 | obj_interval_maps[1][obj] = ivl2; |
198 |
1/2✓ Branch 1 taken 14015 times.
✗ Branch 2 not taken.
|
14015 | obj_interval_maps[2][obj] = ivl3; |
199 | } | ||
200 | } | ||
201 | |||
202 | 64 | setup_ = true; | |
203 | } | ||
204 | 64 | } | |
205 | |||
206 | //============================================================================== | ||
207 | 13 | void IntervalTreeCollisionManager::update() { | |
208 | 13 | setup_ = false; | |
209 | |||
210 |
2/2✓ Branch 1 taken 2688 times.
✓ Branch 2 taken 13 times.
|
2701 | for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) { |
211 |
2/2✓ Branch 1 taken 1344 times.
✓ Branch 2 taken 1344 times.
|
2688 | if (endpoints[0][i].minmax == 0) |
212 | 1344 | endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0]; | |
213 | else | ||
214 | 1344 | endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0]; | |
215 | } | ||
216 | |||
217 |
2/2✓ Branch 1 taken 2688 times.
✓ Branch 2 taken 13 times.
|
2701 | for (size_t i = 0, size = endpoints[1].size(); i < size; ++i) { |
218 |
2/2✓ Branch 1 taken 1344 times.
✓ Branch 2 taken 1344 times.
|
2688 | if (endpoints[1][i].minmax == 0) |
219 | 1344 | endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1]; | |
220 | else | ||
221 | 1344 | endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1]; | |
222 | } | ||
223 | |||
224 |
2/2✓ Branch 1 taken 2688 times.
✓ Branch 2 taken 13 times.
|
2701 | for (size_t i = 0, size = endpoints[2].size(); i < size; ++i) { |
225 |
2/2✓ Branch 1 taken 1344 times.
✓ Branch 2 taken 1344 times.
|
2688 | if (endpoints[2][i].minmax == 0) |
226 | 1344 | endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2]; | |
227 | else | ||
228 | 1344 | endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2]; | |
229 | } | ||
230 | |||
231 | 13 | setup(); | |
232 | 13 | } | |
233 | |||
234 | //============================================================================== | ||
235 | ✗ | void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) { | |
236 | ✗ | AABB old_aabb; | |
237 | ✗ | const AABB& new_aabb = updated_obj->getAABB(); | |
238 | ✗ | for (int i = 0; i < 3; ++i) { | |
239 | ✗ | const auto it = obj_interval_maps[i].find(updated_obj); | |
240 | ✗ | interval_trees[i]->deleteNode(it->second); | |
241 | ✗ | old_aabb.min_[i] = it->second->low; | |
242 | ✗ | old_aabb.max_[i] = it->second->high; | |
243 | ✗ | it->second->low = new_aabb.min_[i]; | |
244 | ✗ | it->second->high = new_aabb.max_[i]; | |
245 | ✗ | interval_trees[i]->insert(it->second); | |
246 | } | ||
247 | |||
248 | EndPoint dummy; | ||
249 | ✗ | typename std::vector<EndPoint>::iterator it; | |
250 | ✗ | for (int i = 0; i < 3; ++i) { | |
251 | ✗ | dummy.value = old_aabb.min_[i]; | |
252 | ✗ | it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); | |
253 | ✗ | for (; it != endpoints[i].end(); ++it) { | |
254 | ✗ | if (it->obj == updated_obj && it->minmax == 0) { | |
255 | ✗ | it->value = new_aabb.min_[i]; | |
256 | ✗ | break; | |
257 | } | ||
258 | } | ||
259 | |||
260 | ✗ | dummy.value = old_aabb.max_[i]; | |
261 | ✗ | it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); | |
262 | ✗ | for (; it != endpoints[i].end(); ++it) { | |
263 | ✗ | if (it->obj == updated_obj && it->minmax == 0) { | |
264 | ✗ | it->value = new_aabb.max_[i]; | |
265 | ✗ | break; | |
266 | } | ||
267 | } | ||
268 | |||
269 | ✗ | std::sort(endpoints[i].begin(), endpoints[i].end()); | |
270 | } | ||
271 | } | ||
272 | |||
273 | //============================================================================== | ||
274 | ✗ | void IntervalTreeCollisionManager::update( | |
275 | const std::vector<CollisionObject*>& updated_objs) { | ||
276 | ✗ | for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]); | |
277 | } | ||
278 | |||
279 | //============================================================================== | ||
280 | 50 | void IntervalTreeCollisionManager::clear() { | |
281 | 50 | endpoints[0].clear(); | |
282 | 50 | endpoints[1].clear(); | |
283 | 50 | endpoints[2].clear(); | |
284 | |||
285 |
1/2✓ Branch 0 taken 50 times.
✗ Branch 1 not taken.
|
50 | delete interval_trees[0]; |
286 | 50 | interval_trees[0] = nullptr; | |
287 |
1/2✓ Branch 0 taken 50 times.
✗ Branch 1 not taken.
|
50 | delete interval_trees[1]; |
288 | 50 | interval_trees[1] = nullptr; | |
289 |
1/2✓ Branch 0 taken 50 times.
✗ Branch 1 not taken.
|
50 | delete interval_trees[2]; |
290 | 50 | interval_trees[2] = nullptr; | |
291 | |||
292 |
2/2✓ Branch 0 taken 150 times.
✓ Branch 1 taken 50 times.
|
200 | for (int i = 0; i < 3; ++i) { |
293 | 300 | for (auto it = obj_interval_maps[i].cbegin(), | |
294 | 150 | end = obj_interval_maps[i].cend(); | |
295 |
2/2✓ Branch 2 taken 37113 times.
✓ Branch 3 taken 150 times.
|
37263 | it != end; ++it) { |
296 |
1/2✓ Branch 1 taken 37113 times.
✗ Branch 2 not taken.
|
37113 | delete it->second; |
297 | } | ||
298 | } | ||
299 | |||
300 |
2/2✓ Branch 1 taken 150 times.
✓ Branch 2 taken 50 times.
|
200 | for (int i = 0; i < 3; ++i) obj_interval_maps[i].clear(); |
301 | |||
302 | 50 | setup_ = false; | |
303 | 50 | } | |
304 | |||
305 | //============================================================================== | ||
306 | ✗ | void IntervalTreeCollisionManager::getObjects( | |
307 | std::vector<CollisionObject*>& objs) const { | ||
308 | ✗ | objs.resize(endpoints[0].size() / 2); | |
309 | ✗ | size_t j = 0; | |
310 | ✗ | for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) { | |
311 | ✗ | if (endpoints[0][i].minmax == 0) { | |
312 | ✗ | objs[j] = endpoints[0][i].obj; | |
313 | ✗ | j++; | |
314 | } | ||
315 | } | ||
316 | } | ||
317 | |||
318 | //============================================================================== | ||
319 | 3822 | void IntervalTreeCollisionManager::collide( | |
320 | CollisionObject* obj, CollisionCallBackBase* callback) const { | ||
321 | 3822 | callback->init(); | |
322 |
2/2✓ Branch 1 taken 60 times.
✓ Branch 2 taken 3762 times.
|
3822 | if (size() == 0) return; |
323 | 3762 | collide_(obj, callback); | |
324 | } | ||
325 | |||
326 | //============================================================================== | ||
327 | 3762 | bool IntervalTreeCollisionManager::collide_( | |
328 | CollisionObject* obj, CollisionCallBackBase* callback) const { | ||
329 | static const unsigned int CUTOFF = 100; | ||
330 | |||
331 |
3/6✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3762 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3762 times.
✗ Branch 8 not taken.
|
3762 | std::deque<detail::SimpleInterval*> results0, results1, results2; |
332 | |||
333 | results0 = | ||
334 |
3/6✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3762 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3762 times.
✗ Branch 10 not taken.
|
3762 | interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]); |
335 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3762 times.
|
3762 | if (results0.size() > CUTOFF) { |
336 | ✗ | results1 = interval_trees[1]->query(obj->getAABB().min_[1], | |
337 | ✗ | obj->getAABB().max_[1]); | |
338 | ✗ | if (results1.size() > CUTOFF) { | |
339 | ✗ | results2 = interval_trees[2]->query(obj->getAABB().min_[2], | |
340 | ✗ | obj->getAABB().max_[2]); | |
341 | ✗ | if (results2.size() > CUTOFF) { | |
342 | ✗ | size_t d1 = results0.size(); | |
343 | ✗ | size_t d2 = results1.size(); | |
344 | ✗ | size_t d3 = results2.size(); | |
345 | |||
346 | ✗ | if (d1 >= d2 && d1 >= d3) | |
347 | ✗ | return checkColl(results0.begin(), results0.end(), obj, callback); | |
348 | ✗ | else if (d2 >= d1 && d2 >= d3) | |
349 | ✗ | return checkColl(results1.begin(), results1.end(), obj, callback); | |
350 | else | ||
351 | ✗ | return checkColl(results2.begin(), results2.end(), obj, callback); | |
352 | } else | ||
353 | ✗ | return checkColl(results2.begin(), results2.end(), obj, callback); | |
354 | } else | ||
355 | ✗ | return checkColl(results1.begin(), results1.end(), obj, callback); | |
356 | } else | ||
357 |
1/2✓ Branch 5 taken 3762 times.
✗ Branch 6 not taken.
|
3762 | return checkColl(results0.begin(), results0.end(), obj, callback); |
358 | 3762 | } | |
359 | |||
360 | //============================================================================== | ||
361 | 80 | void IntervalTreeCollisionManager::distance( | |
362 | CollisionObject* obj, DistanceCallBackBase* callback) const { | ||
363 |
1/2✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
|
80 | callback->init(); |
364 |
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; |
365 | 80 | Scalar min_dist = (std::numeric_limits<Scalar>::max)(); | |
366 |
1/2✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
|
80 | distance_(obj, callback, min_dist); |
367 | } | ||
368 | |||
369 | //============================================================================== | ||
370 | 6060 | bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, | |
371 | DistanceCallBackBase* callback, | ||
372 | Scalar& min_dist) const { | ||
373 | static const unsigned int CUTOFF = 100; | ||
374 | |||
375 |
3/6✓ Branch 3 taken 6060 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6060 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6060 times.
✗ Branch 10 not taken.
|
6060 | Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; |
376 |
1/2✓ Branch 2 taken 6060 times.
✗ Branch 3 not taken.
|
6060 | AABB aabb = obj->getAABB(); |
377 |
2/2✓ Branch 1 taken 5974 times.
✓ Branch 2 taken 86 times.
|
6060 | if (min_dist < (std::numeric_limits<Scalar>::max)()) { |
378 |
1/2✓ Branch 1 taken 5974 times.
✗ Branch 2 not taken.
|
5974 | Vec3s min_dist_delta(min_dist, min_dist, min_dist); |
379 |
1/2✓ Branch 1 taken 5974 times.
✗ Branch 2 not taken.
|
5974 | aabb.expand(min_dist_delta); |
380 | } | ||
381 | |||
382 | 6060 | int status = 1; | |
383 | Scalar old_min_distance; | ||
384 | |||
385 | while (1) { | ||
386 | 6156 | bool dist_res = false; | |
387 | |||
388 | 6156 | old_min_distance = min_dist; | |
389 | |||
390 |
3/6✓ Branch 1 taken 6156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6156 times.
✗ Branch 8 not taken.
|
6156 | std::deque<detail::SimpleInterval*> results0, results1, results2; |
391 | |||
392 |
3/6✓ Branch 1 taken 6156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6156 times.
✗ Branch 8 not taken.
|
6156 | results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]); |
393 |
2/2✓ Branch 1 taken 4743 times.
✓ Branch 2 taken 1413 times.
|
6156 | if (results0.size() > CUTOFF) { |
394 |
3/6✓ Branch 1 taken 4743 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4743 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4743 times.
✗ Branch 8 not taken.
|
4743 | results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]); |
395 |
2/2✓ Branch 1 taken 4737 times.
✓ Branch 2 taken 6 times.
|
4743 | if (results1.size() > CUTOFF) { |
396 |
3/6✓ Branch 1 taken 4737 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4737 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4737 times.
✗ Branch 8 not taken.
|
4737 | results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]); |
397 |
2/2✓ Branch 1 taken 43 times.
✓ Branch 2 taken 4694 times.
|
4737 | if (results2.size() > CUTOFF) { |
398 | 43 | size_t d1 = results0.size(); | |
399 | 43 | size_t d2 = results1.size(); | |
400 | 43 | size_t d3 = results2.size(); | |
401 | |||
402 |
4/4✓ Branch 0 taken 20 times.
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 17 times.
✓ Branch 3 taken 3 times.
|
43 | if (d1 >= d2 && d1 >= d3) |
403 |
1/2✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
|
17 | dist_res = checkDist(results0.begin(), results0.end(), obj, |
404 | callback, min_dist); | ||
405 |
4/4✓ Branch 0 taken 23 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 12 times.
✓ Branch 3 taken 11 times.
|
26 | else if (d2 >= d1 && d2 >= d3) |
406 |
1/2✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
|
12 | dist_res = checkDist(results1.begin(), results1.end(), obj, |
407 | callback, min_dist); | ||
408 | else | ||
409 |
1/2✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
|
14 | dist_res = checkDist(results2.begin(), results2.end(), obj, |
410 | callback, min_dist); | ||
411 | } else | ||
412 |
1/2✓ Branch 5 taken 4694 times.
✗ Branch 6 not taken.
|
4694 | dist_res = checkDist(results2.begin(), results2.end(), obj, callback, |
413 | min_dist); | ||
414 | } else | ||
415 |
1/2✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
6 | dist_res = checkDist(results1.begin(), results1.end(), obj, callback, |
416 | min_dist); | ||
417 | } else | ||
418 | dist_res = | ||
419 |
1/2✓ Branch 5 taken 1413 times.
✗ Branch 6 not taken.
|
1413 | checkDist(results0.begin(), results0.end(), obj, callback, min_dist); |
420 | |||
421 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 6156 times.
|
6156 | if (dist_res) return true; |
422 | |||
423 | 6156 | results0.clear(); | |
424 | 6156 | results1.clear(); | |
425 | 6156 | results2.clear(); | |
426 | |||
427 |
2/2✓ Branch 0 taken 6070 times.
✓ Branch 1 taken 86 times.
|
6156 | if (status == 1) { |
428 |
2/2✓ Branch 1 taken 5974 times.
✓ Branch 2 taken 96 times.
|
6070 | if (old_min_distance < (std::numeric_limits<Scalar>::max)()) |
429 | 5974 | break; | |
430 | else { | ||
431 |
2/2✓ Branch 0 taken 86 times.
✓ Branch 1 taken 10 times.
|
96 | if (min_dist < old_min_distance) { |
432 |
1/2✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
|
86 | Vec3s min_dist_delta(min_dist, min_dist, min_dist); |
433 |
2/4✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 86 times.
✗ Branch 6 not taken.
|
86 | aabb = AABB(obj->getAABB(), min_dist_delta); |
434 | 86 | status = 0; | |
435 | } else { | ||
436 |
3/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✓ Branch 5 taken 7 times.
|
10 | if (aabb == obj->getAABB()) |
437 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | aabb.expand(delta); |
438 | else | ||
439 |
1/2✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
|
7 | aabb.expand(obj->getAABB(), 2.0); |
440 | } | ||
441 | } | ||
442 |
1/2✓ Branch 0 taken 86 times.
✗ Branch 1 not taken.
|
86 | } else if (status == 0) |
443 | 86 | break; | |
444 |
6/9✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6060 times.
✓ Branch 5 taken 96 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 6060 times.
✓ Branch 9 taken 96 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 6060 times.
|
18372 | } |
445 | |||
446 | 6060 | return false; | |
447 | } | ||
448 | |||
449 | //============================================================================== | ||
450 | 37 | void IntervalTreeCollisionManager::collide( | |
451 | CollisionCallBackBase* callback) const { | ||
452 |
1/2✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
|
37 | callback->init(); |
453 |
3/4✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 29 times.
|
40 | if (size() == 0) return; |
454 | |||
455 | 29 | std::set<CollisionObject*> active; | |
456 | 29 | std::set<std::pair<CollisionObject*, CollisionObject*> > overlap; | |
457 | 29 | size_t n = endpoints[0].size(); | |
458 | 29 | Scalar diff_x = endpoints[0][0].value - endpoints[0][n - 1].value; | |
459 | 29 | Scalar diff_y = endpoints[1][0].value - endpoints[1][n - 1].value; | |
460 | 29 | Scalar diff_z = endpoints[2][0].value - endpoints[2][n - 1].value; | |
461 | |||
462 | 29 | int axis = 0; | |
463 |
4/4✓ Branch 0 taken 14 times.
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 9 times.
✓ Branch 3 taken 5 times.
|
29 | if (diff_y > diff_x && diff_y > diff_z) |
464 | 9 | axis = 1; | |
465 |
4/4✓ Branch 0 taken 12 times.
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 8 times.
✓ Branch 3 taken 4 times.
|
20 | else if (diff_z > diff_y && diff_z > diff_x) |
466 | 8 | axis = 2; | |
467 | |||
468 |
2/2✓ Branch 0 taken 3958 times.
✓ Branch 1 taken 26 times.
|
3984 | for (unsigned int i = 0; i < n; ++i) { |
469 | 3958 | const EndPoint& endpoint = endpoints[axis][i]; | |
470 | 3958 | CollisionObject* index = endpoint.obj; | |
471 |
2/2✓ Branch 0 taken 1989 times.
✓ Branch 1 taken 1969 times.
|
3958 | if (endpoint.minmax == 0) { |
472 | 1989 | auto iter = active.begin(); | |
473 | 1989 | auto end = active.end(); | |
474 |
2/2✓ Branch 2 taken 6062 times.
✓ Branch 3 taken 1986 times.
|
8048 | for (; iter != end; ++iter) { |
475 | 6062 | CollisionObject* active_index = *iter; | |
476 | 6062 | const AABB& b0 = active_index->getAABB(); | |
477 | 6062 | const AABB& b1 = index->getAABB(); | |
478 | |||
479 | 6062 | int axis2 = (axis + 1) % 3; | |
480 | 6062 | int axis3 = (axis + 2) % 3; | |
481 | |||
482 |
8/10✓ Branch 1 taken 6062 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 186 times.
✓ Branch 4 taken 5876 times.
✓ Branch 6 taken 186 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 7 times.
✓ Branch 9 taken 179 times.
✓ Branch 10 taken 7 times.
✓ Branch 11 taken 6055 times.
|
6062 | if (b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3)) { |
483 | std::pair<typename std::set<std::pair<CollisionObject*, | ||
484 | CollisionObject*> >::iterator, | ||
485 | bool> | ||
486 | 7 | insert_res; | |
487 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 6 times.
|
7 | if (active_index < index) |
488 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | insert_res = overlap.insert(std::make_pair(active_index, index)); |
489 | else | ||
490 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | insert_res = overlap.insert(std::make_pair(index, active_index)); |
491 | |||
492 |
1/2✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
|
7 | if (insert_res.second) { |
493 |
3/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 4 times.
|
7 | if ((*callback)(active_index, index)) return; |
494 | } | ||
495 | } | ||
496 | } | ||
497 |
1/2✓ Branch 1 taken 1986 times.
✗ Branch 2 not taken.
|
1986 | active.insert(index); |
498 | } else | ||
499 |
1/2✓ Branch 1 taken 1969 times.
✗ Branch 2 not taken.
|
1969 | active.erase(index); |
500 | } | ||
501 |
4/4✓ Branch 1 taken 26 times.
✓ Branch 2 taken 3 times.
✓ Branch 4 taken 26 times.
✓ Branch 5 taken 3 times.
|
32 | } |
502 | |||
503 | //============================================================================== | ||
504 | 6 | void IntervalTreeCollisionManager::distance( | |
505 | DistanceCallBackBase* callback) const { | ||
506 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | callback->init(); |
507 |
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; |
508 | |||
509 | 6 | this->enable_tested_set_ = true; | |
510 | 6 | this->tested_set.clear(); | |
511 | 6 | Scalar min_dist = (std::numeric_limits<Scalar>::max)(); | |
512 | |||
513 |
2/2✓ Branch 1 taken 5980 times.
✓ Branch 2 taken 6 times.
|
5986 | for (size_t i = 0; i < endpoints[0].size(); ++i) |
514 |
2/4✓ Branch 2 taken 5980 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 5980 times.
|
5980 | if (distance_(endpoints[0][i].obj, callback, min_dist)) break; |
515 | |||
516 | 6 | this->enable_tested_set_ = false; | |
517 | 6 | this->tested_set.clear(); | |
518 | } | ||
519 | |||
520 | //============================================================================== | ||
521 | ✗ | void IntervalTreeCollisionManager::collide( | |
522 | BroadPhaseCollisionManager* other_manager_, | ||
523 | CollisionCallBackBase* callback) const { | ||
524 | ✗ | callback->init(); | |
525 | ✗ | IntervalTreeCollisionManager* other_manager = | |
526 | static_cast<IntervalTreeCollisionManager*>(other_manager_); | ||
527 | |||
528 | ✗ | if ((size() == 0) || (other_manager->size() == 0)) return; | |
529 | |||
530 | ✗ | if (this == other_manager) { | |
531 | ✗ | collide(callback); | |
532 | ✗ | return; | |
533 | } | ||
534 | |||
535 | ✗ | if (this->size() < other_manager->size()) { | |
536 | ✗ | for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) | |
537 | ✗ | if (other_manager->collide_(endpoints[0][i].obj, callback)) return; | |
538 | } else { | ||
539 | ✗ | for (size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) | |
540 | ✗ | if (collide_(other_manager->endpoints[0][i].obj, callback)) return; | |
541 | } | ||
542 | } | ||
543 | |||
544 | //============================================================================== | ||
545 | ✗ | void IntervalTreeCollisionManager::distance( | |
546 | BroadPhaseCollisionManager* other_manager_, | ||
547 | DistanceCallBackBase* callback) const { | ||
548 | ✗ | callback->init(); | |
549 | ✗ | IntervalTreeCollisionManager* other_manager = | |
550 | static_cast<IntervalTreeCollisionManager*>(other_manager_); | ||
551 | |||
552 | ✗ | if ((size() == 0) || (other_manager->size() == 0)) return; | |
553 | |||
554 | ✗ | if (this == other_manager) { | |
555 | ✗ | distance(callback); | |
556 | ✗ | return; | |
557 | } | ||
558 | |||
559 | ✗ | Scalar min_dist = (std::numeric_limits<Scalar>::max)(); | |
560 | |||
561 | ✗ | if (this->size() < other_manager->size()) { | |
562 | ✗ | for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) | |
563 | ✗ | if (other_manager->distance_(endpoints[0][i].obj, callback, min_dist)) | |
564 | ✗ | return; | |
565 | } else { | ||
566 | ✗ | for (size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) | |
567 | ✗ | if (distance_(other_manager->endpoints[0][i].obj, callback, min_dist)) | |
568 | ✗ | return; | |
569 | } | ||
570 | } | ||
571 | |||
572 | //============================================================================== | ||
573 | ✗ | bool IntervalTreeCollisionManager::empty() const { | |
574 | ✗ | return endpoints[0].empty(); | |
575 | } | ||
576 | |||
577 | //============================================================================== | ||
578 | 3945 | size_t IntervalTreeCollisionManager::size() const { | |
579 | 3945 | return endpoints[0].size() / 2; | |
580 | } | ||
581 | |||
582 | //============================================================================== | ||
583 | 3762 | bool IntervalTreeCollisionManager::checkColl( | |
584 | typename std::deque<detail::SimpleInterval*>::const_iterator pos_start, | ||
585 | typename std::deque<detail::SimpleInterval*>::const_iterator pos_end, | ||
586 | CollisionObject* obj, CollisionCallBackBase* callback) const { | ||
587 |
2/2✓ Branch 1 taken 14431 times.
✓ Branch 2 taken 3761 times.
|
18192 | while (pos_start < pos_end) { |
588 | 14431 | SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start); | |
589 |
1/2✓ Branch 0 taken 14431 times.
✗ Branch 1 not taken.
|
14431 | if (ivl->obj != obj) { |
590 |
2/2✓ Branch 3 taken 19 times.
✓ Branch 4 taken 14412 times.
|
14431 | if (ivl->obj->getAABB().overlap(obj->getAABB())) { |
591 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 18 times.
|
19 | if ((*callback)(ivl->obj, obj)) return true; |
592 | } | ||
593 | } | ||
594 | |||
595 | 14430 | pos_start++; | |
596 | } | ||
597 | |||
598 | 3761 | return false; | |
599 | } | ||
600 | |||
601 | //============================================================================== | ||
602 | 6156 | bool IntervalTreeCollisionManager::checkDist( | |
603 | typename std::deque<detail::SimpleInterval*>::const_iterator pos_start, | ||
604 | typename std::deque<detail::SimpleInterval*>::const_iterator pos_end, | ||
605 | CollisionObject* obj, DistanceCallBackBase* callback, | ||
606 | Scalar& min_dist) const { | ||
607 |
2/2✓ Branch 1 taken 90503 times.
✓ Branch 2 taken 6156 times.
|
96659 | while (pos_start < pos_end) { |
608 | 90503 | SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start); | |
609 |
2/2✓ Branch 0 taken 84517 times.
✓ Branch 1 taken 5986 times.
|
90503 | if (ivl->obj != obj) { |
610 |
2/2✓ Branch 0 taken 24428 times.
✓ Branch 1 taken 60089 times.
|
84517 | if (!this->enable_tested_set_) { |
611 |
2/2✓ Branch 3 taken 1565 times.
✓ Branch 4 taken 22863 times.
|
24428 | if (ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) { |
612 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1565 times.
|
1565 | if ((*callback)(ivl->obj, obj, min_dist)) return true; |
613 | } | ||
614 | } else { | ||
615 |
2/2✓ Branch 1 taken 18452 times.
✓ Branch 2 taken 41637 times.
|
60089 | if (!this->inTestedSet(ivl->obj, obj)) { |
616 |
2/2✓ Branch 3 taken 286 times.
✓ Branch 4 taken 18166 times.
|
18452 | if (ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) { |
617 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 286 times.
|
286 | if ((*callback)(ivl->obj, obj, min_dist)) return true; |
618 | } | ||
619 | |||
620 | 18452 | this->insertTestedSet(ivl->obj, obj); | |
621 | } | ||
622 | } | ||
623 | } | ||
624 | |||
625 | 90503 | pos_start++; | |
626 | } | ||
627 | |||
628 | 6156 | return false; | |
629 | } | ||
630 | |||
631 | //============================================================================== | ||
632 | 1086788 | bool IntervalTreeCollisionManager::EndPoint::operator<( | |
633 | const typename IntervalTreeCollisionManager::EndPoint& p) const { | ||
634 | 1086788 | return value < p.value; | |
635 | } | ||
636 | |||
637 | //============================================================================== | ||
638 | 42045 | IntervalTreeCollisionManager::SAPInterval::SAPInterval(Scalar low_, | |
639 | Scalar high_, | ||
640 | 42045 | CollisionObject* obj_) | |
641 | 42045 | : detail::SimpleInterval() { | |
642 | 42045 | this->low = low_; | |
643 | 42045 | this->high = high_; | |
644 | 42045 | obj = obj_; | |
645 | 42045 | } | |
646 | |||
647 | } // namespace coal | ||
648 |