Directory: | ./ |
---|---|
File: | src/broadphase/broadphase_SaP.cpp |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 326 | 468 | 69.7% |
Branches: | 306 | 614 | 49.8% |
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_SaP.h" | ||
39 | #include "coal/tracy.hh" | ||
40 | |||
41 | namespace coal { | ||
42 | |||
43 | //============================================================================== | ||
44 | ✗ | void SaPCollisionManager::unregisterObject(CollisionObject* obj) { | |
45 | ✗ | auto it = AABB_arr.begin(); | |
46 | ✗ | for (auto end = AABB_arr.end(); it != end; ++it) { | |
47 | ✗ | if ((*it)->obj == obj) break; | |
48 | } | ||
49 | |||
50 | ✗ | AABB_arr.erase(it); | |
51 | ✗ | obj_aabb_map.erase(obj); | |
52 | |||
53 | ✗ | if (it == AABB_arr.end()) return; | |
54 | |||
55 | ✗ | SaPAABB* curr = *it; | |
56 | ✗ | *it = nullptr; | |
57 | |||
58 | ✗ | for (int coord = 0; coord < 3; ++coord) { | |
59 | // first delete the lo endpoint of the interval. | ||
60 | ✗ | if (curr->lo->prev[coord] == nullptr) | |
61 | ✗ | elist[coord] = curr->lo->next[coord]; | |
62 | else | ||
63 | ✗ | curr->lo->prev[coord]->next[coord] = curr->lo->next[coord]; | |
64 | |||
65 | ✗ | curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord]; | |
66 | |||
67 | // then, delete the "hi" endpoint. | ||
68 | ✗ | if (curr->hi->prev[coord] == nullptr) | |
69 | ✗ | elist[coord] = curr->hi->next[coord]; | |
70 | else | ||
71 | ✗ | curr->hi->prev[coord]->next[coord] = curr->hi->next[coord]; | |
72 | |||
73 | ✗ | if (curr->hi->next[coord] != nullptr) | |
74 | ✗ | curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord]; | |
75 | } | ||
76 | |||
77 | ✗ | delete curr->lo; | |
78 | ✗ | delete curr->hi; | |
79 | ✗ | delete curr; | |
80 | |||
81 | ✗ | overlap_pairs.remove_if(isUnregistered(obj)); | |
82 | } | ||
83 | |||
84 | //============================================================================== | ||
85 |
2/2✓ Branch 2 taken 153 times.
✓ Branch 3 taken 51 times.
|
204 | SaPCollisionManager::SaPCollisionManager() { |
86 | 51 | elist[0] = nullptr; | |
87 | 51 | elist[1] = nullptr; | |
88 | 51 | elist[2] = nullptr; | |
89 | |||
90 | 51 | optimal_axis = 0; | |
91 | 51 | } | |
92 | |||
93 | //============================================================================== | ||
94 |
3/4✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 150 times.
✓ Branch 7 taken 50 times.
|
500 | SaPCollisionManager::~SaPCollisionManager() { clear(); } |
95 | |||
96 | //============================================================================== | ||
97 | 51 | void SaPCollisionManager::registerObjects( | |
98 | const std::vector<CollisionObject*>& other_objs) { | ||
99 |
2/2✓ Branch 1 taken 8 times.
✓ Branch 2 taken 43 times.
|
51 | if (other_objs.empty()) return; |
100 | |||
101 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 43 times.
|
43 | if (size() > 0) |
102 | ✗ | BroadPhaseCollisionManager::registerObjects(other_objs); | |
103 | else { | ||
104 |
1/2✓ Branch 3 taken 43 times.
✗ Branch 4 not taken.
|
43 | std::vector<EndPoint*> endpoints(2 * other_objs.size()); |
105 | |||
106 |
2/2✓ Branch 1 taken 12671 times.
✓ Branch 2 taken 43 times.
|
12714 | for (size_t i = 0; i < other_objs.size(); ++i) { |
107 |
2/4✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12671 times.
✗ Branch 5 not taken.
|
12671 | SaPAABB* sapaabb = new SaPAABB(); |
108 | 12671 | sapaabb->obj = other_objs[i]; | |
109 |
1/2✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
|
12671 | sapaabb->lo = new EndPoint(); |
110 |
1/2✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
|
12671 | sapaabb->hi = new EndPoint(); |
111 |
1/2✓ Branch 3 taken 12671 times.
✗ Branch 4 not taken.
|
12671 | sapaabb->cached = other_objs[i]->getAABB(); |
112 | 12671 | endpoints[2 * i] = sapaabb->lo; | |
113 | 12671 | endpoints[2 * i + 1] = sapaabb->hi; | |
114 | 12671 | sapaabb->lo->minmax = 0; | |
115 | 12671 | sapaabb->hi->minmax = 1; | |
116 | 12671 | sapaabb->lo->aabb = sapaabb; | |
117 | 12671 | sapaabb->hi->aabb = sapaabb; | |
118 |
1/2✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
|
12671 | AABB_arr.push_back(sapaabb); |
119 |
1/2✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
|
12671 | obj_aabb_map[other_objs[i]] = sapaabb; |
120 | } | ||
121 | |||
122 | Scalar scale[3]; | ||
123 |
2/2✓ Branch 0 taken 129 times.
✓ Branch 1 taken 43 times.
|
172 | for (int coord = 0; coord < 3; ++coord) { |
124 |
1/2✓ Branch 3 taken 129 times.
✗ Branch 4 not taken.
|
129 | std::sort( |
125 | endpoints.begin(), endpoints.end(), | ||
126 |
1/2✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
|
129 | std::bind(std::less<Scalar>(), |
127 |
1/2✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
|
129 | std::bind(static_cast<Scalar (EndPoint::*)(int) const>( |
128 | &EndPoint::getVal), | ||
129 | std::placeholders::_1, coord), | ||
130 |
1/2✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
|
129 | std::bind(static_cast<Scalar (EndPoint::*)(int) const>( |
131 | &EndPoint::getVal), | ||
132 | std::placeholders::_2, coord))); | ||
133 | |||
134 | 129 | endpoints[0]->prev[coord] = nullptr; | |
135 | 129 | endpoints[0]->next[coord] = endpoints[1]; | |
136 |
2/2✓ Branch 1 taken 75768 times.
✓ Branch 2 taken 129 times.
|
75897 | for (size_t i = 1; i < endpoints.size() - 1; ++i) { |
137 | 75768 | endpoints[i]->prev[coord] = endpoints[i - 1]; | |
138 | 75768 | endpoints[i]->next[coord] = endpoints[i + 1]; | |
139 | } | ||
140 | 129 | endpoints[endpoints.size() - 1]->prev[coord] = | |
141 | 129 | endpoints[endpoints.size() - 2]; | |
142 | 129 | endpoints[endpoints.size() - 1]->next[coord] = nullptr; | |
143 | |||
144 | 129 | elist[coord] = endpoints[0]; | |
145 | |||
146 |
1/2✓ Branch 2 taken 129 times.
✗ Branch 3 not taken.
|
129 | scale[coord] = endpoints.back()->aabb->cached.max_[coord] - |
147 |
1/2✓ Branch 2 taken 129 times.
✗ Branch 3 not taken.
|
129 | endpoints[0]->aabb->cached.min_[coord]; |
148 | } | ||
149 | |||
150 | 43 | int axis = 0; | |
151 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 23 times.
|
43 | if (scale[axis] < scale[1]) axis = 1; |
152 |
2/2✓ Branch 0 taken 21 times.
✓ Branch 1 taken 22 times.
|
43 | if (scale[axis] < scale[2]) axis = 2; |
153 | |||
154 | 43 | EndPoint* pos = elist[axis]; | |
155 | |||
156 |
2/2✓ Branch 0 taken 16282 times.
✓ Branch 1 taken 43 times.
|
16325 | while (pos != nullptr) { |
157 | 16282 | EndPoint* pos_next = nullptr; | |
158 | 16282 | SaPAABB* aabb = pos->aabb; | |
159 | 16282 | EndPoint* pos_it = pos->next[axis]; | |
160 | |||
161 |
2/2✓ Branch 0 taken 5458053 times.
✓ Branch 1 taken 3611 times.
|
5461664 | while (pos_it != nullptr) { |
162 |
2/2✓ Branch 0 taken 12671 times.
✓ Branch 1 taken 5445382 times.
|
5458053 | if (pos_it->aabb == aabb) { |
163 |
2/2✓ Branch 0 taken 3611 times.
✓ Branch 1 taken 9060 times.
|
12671 | if (pos_next == nullptr) pos_next = pos_it; |
164 | 12671 | break; | |
165 | } | ||
166 | |||
167 |
2/2✓ Branch 0 taken 2722182 times.
✓ Branch 1 taken 2723200 times.
|
5445382 | if (pos_it->minmax == 0) { |
168 |
2/2✓ Branch 0 taken 12628 times.
✓ Branch 1 taken 2709554 times.
|
2722182 | if (pos_next == nullptr) pos_next = pos_it; |
169 |
3/4✓ Branch 1 taken 2722182 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 78455 times.
✓ Branch 4 taken 2643727 times.
|
2722182 | if (pos_it->aabb->cached.overlap(aabb->cached)) |
170 |
1/2✓ Branch 1 taken 78455 times.
✗ Branch 2 not taken.
|
78455 | overlap_pairs.emplace_back(pos_it->aabb->obj, aabb->obj); |
171 | } | ||
172 | 5445382 | pos_it = pos_it->next[axis]; | |
173 | } | ||
174 | |||
175 | 16282 | pos = pos_next; | |
176 | } | ||
177 | 43 | } | |
178 | |||
179 | 43 | updateVelist(); | |
180 | } | ||
181 | |||
182 | //============================================================================== | ||
183 | ✗ | void SaPCollisionManager::registerObject(CollisionObject* obj) { | |
184 | // Initialize a new SaPAABB associated with current Collision Object | ||
185 | ✗ | SaPAABB* new_sap = new SaPAABB; | |
186 | ✗ | new_sap->cached = obj->getAABB(); | |
187 | ✗ | new_sap->obj = obj; | |
188 | |||
189 | ✗ | new_sap->lo = new EndPoint; | |
190 | ✗ | new_sap->lo->minmax = 0; | |
191 | ✗ | new_sap->lo->aabb = new_sap; | |
192 | |||
193 | ✗ | new_sap->hi = new EndPoint; | |
194 | ✗ | new_sap->hi->minmax = 1; | |
195 | ✗ | new_sap->hi->aabb = new_sap; | |
196 | ✗ | for (int coord = 0; coord < 3; ++coord) { | |
197 | ✗ | EndPoint* current = elist[coord]; | |
198 | |||
199 | // first insert the lo end point | ||
200 | ✗ | if (current == nullptr) // empty list | |
201 | { | ||
202 | ✗ | elist[coord] = new_sap->lo; | |
203 | ✗ | new_sap->lo->prev[coord] = new_sap->lo->next[coord] = nullptr; | |
204 | } else // otherwise, find the correct location in the list and insert | ||
205 | { | ||
206 | ✗ | EndPoint* curr_lo = new_sap->lo; | |
207 | ✗ | Scalar curr_lo_val = curr_lo->getVal()[coord]; | |
208 | ✗ | while ((current->getVal()[coord] < curr_lo_val) && | |
209 | ✗ | (current->next[coord] != nullptr)) | |
210 | ✗ | current = current->next[coord]; | |
211 | |||
212 | ✗ | if (current->getVal()[coord] >= curr_lo_val) { | |
213 | ✗ | curr_lo->prev[coord] = current->prev[coord]; | |
214 | ✗ | curr_lo->next[coord] = current; | |
215 | ✗ | if (current->prev[coord] == nullptr) // current was the first box | |
216 | ✗ | elist[coord] = | |
217 | curr_lo; // new_sap->lo becomes the new first box on the axis | ||
218 | else | ||
219 | ✗ | current->prev[coord]->next[coord] = curr_lo; | |
220 | |||
221 | ✗ | current->prev[coord] = | |
222 | curr_lo; // new_sap->lo becomes the predecessor of current | ||
223 | } else { // current->next[coord] == nullptr, so the current is just the | ||
224 | // cell before current-> | ||
225 | ✗ | curr_lo->prev[coord] = current; | |
226 | ✗ | curr_lo->next[coord] = nullptr; | |
227 | ✗ | current->next[coord] = curr_lo; | |
228 | } | ||
229 | } | ||
230 | |||
231 | // now insert hi end point | ||
232 | ✗ | current = new_sap->lo; | |
233 | |||
234 | ✗ | EndPoint* curr_hi = new_sap->hi; | |
235 | ✗ | Scalar curr_hi_val = curr_hi->getVal()[coord]; | |
236 | |||
237 | ✗ | if (coord == 0) { | |
238 | ✗ | while ((current->getVal()[coord] < curr_hi_val) && | |
239 | ✗ | (current->next[coord] != nullptr)) { | |
240 | ✗ | if (current != new_sap->lo) | |
241 | ✗ | if (current->aabb->cached.overlap(new_sap->cached)) | |
242 | ✗ | overlap_pairs.emplace_back(current->aabb->obj, obj); | |
243 | |||
244 | ✗ | current = current->next[coord]; | |
245 | } | ||
246 | } else { | ||
247 | ✗ | while ((current->getVal()[coord] < curr_hi_val) && | |
248 | ✗ | (current->next[coord] != nullptr)) | |
249 | ✗ | current = current->next[coord]; | |
250 | } | ||
251 | |||
252 | ✗ | if (current->getVal()[coord] >= curr_hi_val) { | |
253 | ✗ | curr_hi->prev[coord] = current->prev[coord]; | |
254 | ✗ | curr_hi->next[coord] = current; | |
255 | ✗ | if (current->prev[coord] != nullptr) | |
256 | ✗ | current->prev[coord]->next[coord] = curr_hi; | |
257 | |||
258 | ✗ | current->prev[coord] = curr_hi; | |
259 | } else { | ||
260 | ✗ | curr_hi->prev[coord] = current; | |
261 | ✗ | curr_hi->next[coord] = nullptr; | |
262 | ✗ | current->next[coord] = curr_hi; | |
263 | } | ||
264 | } | ||
265 | |||
266 | ✗ | AABB_arr.push_back(new_sap); | |
267 | |||
268 | ✗ | obj_aabb_map[obj] = new_sap; | |
269 | |||
270 | ✗ | updateVelist(); | |
271 | } | ||
272 | |||
273 | //============================================================================== | ||
274 | 64 | void SaPCollisionManager::setup() { | |
275 |
3/4✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 56 times.
|
64 | if (size() == 0) return; |
276 | |||
277 | Scalar scale[3]; | ||
278 |
2/4✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 56 times.
✗ Branch 7 not taken.
|
56 | scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0); |
279 |
2/4✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 56 times.
✗ Branch 7 not taken.
|
56 | scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1); |
280 |
2/4✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 56 times.
✗ Branch 7 not taken.
|
56 | scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2); |
281 | |||
282 | 56 | int axis = 0; | |
283 |
2/2✓ Branch 0 taken 24 times.
✓ Branch 1 taken 32 times.
|
56 | if (scale[axis] < scale[1]) axis = 1; |
284 |
2/2✓ Branch 0 taken 25 times.
✓ Branch 1 taken 31 times.
|
56 | if (scale[axis] < scale[2]) axis = 2; |
285 | 56 | optimal_axis = axis; | |
286 | } | ||
287 | |||
288 | //============================================================================== | ||
289 | 1344 | void SaPCollisionManager::update_(SaPAABB* updated_aabb) { | |
290 |
2/4✓ Branch 2 taken 1344 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1344 times.
|
1344 | if (updated_aabb->cached == updated_aabb->obj->getAABB()) return; |
291 | |||
292 | 1344 | SaPAABB* current = updated_aabb; | |
293 | |||
294 |
1/2✓ Branch 2 taken 1344 times.
✗ Branch 3 not taken.
|
1344 | const AABB current_aabb = current->obj->getAABB(); |
295 | |||
296 | 1344 | const Vec3s& new_min = current_aabb.min_; | |
297 | 1344 | const Vec3s& new_max = current_aabb.max_; | |
298 | |||
299 |
2/2✓ Branch 0 taken 4032 times.
✓ Branch 1 taken 1344 times.
|
5376 | for (int coord = 0; coord < 3; ++coord) { |
300 | int direction; // -1 reverse, 0 nochange, 1 forward | ||
301 | EndPoint* temp; | ||
302 | |||
303 |
4/6✓ Branch 1 taken 4032 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4032 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2040 times.
✓ Branch 7 taken 1992 times.
|
4032 | if (current->lo->getVal(coord) > new_min[coord]) |
304 | 2040 | direction = -1; | |
305 |
3/6✓ Branch 1 taken 1992 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1992 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1992 times.
✗ Branch 7 not taken.
|
1992 | else if (current->lo->getVal(coord) < new_min[coord]) |
306 | 1992 | direction = 1; | |
307 | else | ||
308 | ✗ | direction = 0; | |
309 | |||
310 |
2/2✓ Branch 0 taken 2040 times.
✓ Branch 1 taken 1992 times.
|
4032 | if (direction == -1) { |
311 | // first update the "lo" endpoint of the interval | ||
312 |
2/2✓ Branch 0 taken 2027 times.
✓ Branch 1 taken 13 times.
|
2040 | if (current->lo->prev[coord] != nullptr) { |
313 | 2027 | temp = current->lo->prev[coord]; | |
314 |
8/10✓ Branch 0 taken 11345 times.
✓ Branch 1 taken 12 times.
✓ Branch 3 taken 11345 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11345 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 9330 times.
✓ Branch 9 taken 2015 times.
✓ Branch 10 taken 9330 times.
✓ Branch 11 taken 2027 times.
|
11357 | while ((temp != nullptr) && (temp->getVal(coord) > new_min[coord])) { |
315 |
2/2✓ Branch 0 taken 4657 times.
✓ Branch 1 taken 4673 times.
|
9330 | if (temp->minmax == 1) |
316 |
3/4✓ Branch 1 taken 4657 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 4653 times.
|
4657 | if (temp->aabb->cached.overlap(current_aabb)) |
317 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); |
318 | 9330 | temp = temp->prev[coord]; | |
319 | } | ||
320 | |||
321 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 2015 times.
|
2027 | if (temp == nullptr) { |
322 | 12 | current->lo->prev[coord]->next[coord] = current->lo->next[coord]; | |
323 | 12 | current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; | |
324 | 12 | current->lo->prev[coord] = nullptr; | |
325 | 12 | current->lo->next[coord] = elist[coord]; | |
326 | 12 | elist[coord]->prev[coord] = current->lo; | |
327 | 12 | elist[coord] = current->lo; | |
328 | } else { | ||
329 | 2015 | current->lo->prev[coord]->next[coord] = current->lo->next[coord]; | |
330 | 2015 | current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; | |
331 | 2015 | current->lo->prev[coord] = temp; | |
332 | 2015 | current->lo->next[coord] = temp->next[coord]; | |
333 | 2015 | temp->next[coord]->prev[coord] = current->lo; | |
334 | 2015 | temp->next[coord] = current->lo; | |
335 | } | ||
336 | } | ||
337 | |||
338 | // Update the value of the lower bound along axis coord | ||
339 |
2/4✓ Branch 1 taken 2040 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2040 times.
✗ Branch 5 not taken.
|
2040 | current->lo->getVal(coord) = new_min[coord]; |
340 | |||
341 | // update hi end point | ||
342 |
1/2✓ Branch 0 taken 2040 times.
✗ Branch 1 not taken.
|
2040 | if (current->hi->prev[coord] != nullptr) { |
343 | 2040 | temp = current->hi->prev[coord]; | |
344 | |||
345 |
7/10✓ Branch 0 taken 11420 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 11420 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11420 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 9380 times.
✓ Branch 9 taken 2040 times.
✓ Branch 10 taken 9380 times.
✓ Branch 11 taken 2040 times.
|
11420 | while ((temp != nullptr) && (temp->getVal(coord) > new_max[coord])) { |
346 |
4/4✓ Branch 0 taken 4654 times.
✓ Branch 1 taken 4726 times.
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 9375 times.
|
14034 | if ((temp->minmax == 0) && |
347 |
3/4✓ Branch 1 taken 4654 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 4649 times.
|
4654 | (temp->aabb->cached.overlap(current->cached))) |
348 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
5 | removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); |
349 | 9380 | temp = temp->prev[coord]; | |
350 | } | ||
351 | |||
352 | 2040 | current->hi->prev[coord]->next[coord] = current->hi->next[coord]; | |
353 |
2/2✓ Branch 0 taken 2020 times.
✓ Branch 1 taken 20 times.
|
2040 | if (current->hi->next[coord] != nullptr) |
354 | 2020 | current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; | |
355 | 2040 | current->hi->prev[coord] = temp; // Wrong line | |
356 | 2040 | current->hi->next[coord] = temp->next[coord]; | |
357 |
2/2✓ Branch 0 taken 2028 times.
✓ Branch 1 taken 12 times.
|
2040 | if (temp->next[coord] != nullptr) |
358 | 2028 | temp->next[coord]->prev[coord] = current->hi; | |
359 | 2040 | temp->next[coord] = current->hi; | |
360 | |||
361 |
2/4✓ Branch 1 taken 2040 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2040 times.
✗ Branch 5 not taken.
|
2040 | current->hi->getVal(coord) = new_max[coord]; |
362 | } | ||
363 | |||
364 |
2/4✓ Branch 1 taken 2040 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2040 times.
✗ Branch 5 not taken.
|
2040 | current->hi->getVal(coord) = new_max[coord]; |
365 |
1/2✓ Branch 0 taken 1992 times.
✗ Branch 1 not taken.
|
1992 | } else if (direction == 1) { |
366 | // here, we first update the "hi" endpoint. | ||
367 |
2/2✓ Branch 0 taken 1974 times.
✓ Branch 1 taken 18 times.
|
1992 | if (current->hi->next[coord] != nullptr) { |
368 | 1974 | temp = current->hi->next[coord]; | |
369 |
4/4✓ Branch 0 taken 10624 times.
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 8674 times.
✓ Branch 3 taken 1974 times.
|
21272 | while ((temp->next[coord] != nullptr) && |
370 |
4/6✓ Branch 1 taken 10624 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10624 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8674 times.
✓ Branch 7 taken 1950 times.
|
10624 | (temp->getVal(coord) < new_max[coord])) { |
371 |
2/2✓ Branch 0 taken 4353 times.
✓ Branch 1 taken 4321 times.
|
8674 | if (temp->minmax == 0) |
372 |
3/4✓ Branch 1 taken 4353 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✓ Branch 4 taken 4346 times.
|
4353 | if (temp->aabb->cached.overlap(current_aabb)) |
373 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
|
7 | addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); |
374 | 8674 | temp = temp->next[coord]; | |
375 | } | ||
376 | |||
377 |
4/6✓ Branch 1 taken 1974 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1974 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 13 times.
✓ Branch 7 taken 1961 times.
|
1974 | if (temp->getVal(coord) < new_max[coord]) { |
378 | 13 | current->hi->prev[coord]->next[coord] = current->hi->next[coord]; | |
379 | 13 | current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; | |
380 | 13 | current->hi->prev[coord] = temp; | |
381 | 13 | current->hi->next[coord] = nullptr; | |
382 | 13 | temp->next[coord] = current->hi; | |
383 | } else { | ||
384 | 1961 | current->hi->prev[coord]->next[coord] = current->hi->next[coord]; | |
385 | 1961 | current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; | |
386 | 1961 | current->hi->prev[coord] = temp->prev[coord]; | |
387 | 1961 | current->hi->next[coord] = temp; | |
388 | 1961 | temp->prev[coord]->next[coord] = current->hi; | |
389 | 1961 | temp->prev[coord] = current->hi; | |
390 | } | ||
391 | } | ||
392 | |||
393 |
2/4✓ Branch 1 taken 1992 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1992 times.
✗ Branch 5 not taken.
|
1992 | current->hi->getVal(coord) = new_max[coord]; |
394 | |||
395 | // then, update the "lo" endpoint of the interval. | ||
396 |
1/2✓ Branch 0 taken 1992 times.
✗ Branch 1 not taken.
|
1992 | if (current->lo->next[coord] != nullptr) { |
397 | 1992 | temp = current->lo->next[coord]; | |
398 | |||
399 |
4/4✓ Branch 0 taken 10578 times.
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 8600 times.
✓ Branch 3 taken 1992 times.
|
21170 | while ((temp->next[coord] != nullptr) && |
400 |
4/6✓ Branch 1 taken 10578 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10578 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8600 times.
✓ Branch 7 taken 1978 times.
|
10578 | (temp->getVal(coord) < new_min[coord])) { |
401 |
4/4✓ Branch 0 taken 4360 times.
✓ Branch 1 taken 4240 times.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 8597 times.
|
12960 | if ((temp->minmax == 1) && |
402 |
3/4✓ Branch 1 taken 4360 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 4357 times.
|
4360 | (temp->aabb->cached.overlap(current->cached))) |
403 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); |
404 | 8600 | temp = temp->next[coord]; | |
405 | } | ||
406 | |||
407 |
2/2✓ Branch 0 taken 1969 times.
✓ Branch 1 taken 23 times.
|
1992 | if (current->lo->prev[coord] != nullptr) |
408 | 1969 | current->lo->prev[coord]->next[coord] = current->lo->next[coord]; | |
409 | else | ||
410 | 23 | elist[coord] = current->lo->next[coord]; | |
411 | 1992 | current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; | |
412 | 1992 | current->lo->prev[coord] = temp->prev[coord]; | |
413 | 1992 | current->lo->next[coord] = temp; | |
414 |
2/2✓ Branch 0 taken 1974 times.
✓ Branch 1 taken 18 times.
|
1992 | if (temp->prev[coord] != nullptr) |
415 | 1974 | temp->prev[coord]->next[coord] = current->lo; | |
416 | else | ||
417 | 18 | elist[coord] = current->lo; | |
418 | 1992 | temp->prev[coord] = current->lo; | |
419 |
2/4✓ Branch 1 taken 1992 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1992 times.
✗ Branch 5 not taken.
|
1992 | current->lo->getVal(coord) = new_min[coord]; |
420 | } | ||
421 | } | ||
422 | } | ||
423 | } | ||
424 | |||
425 | //============================================================================== | ||
426 | 56 | void SaPCollisionManager::updateVelist() { | |
427 |
2/2✓ Branch 0 taken 168 times.
✓ Branch 1 taken 56 times.
|
224 | for (int coord = 0; coord < 3; ++coord) { |
428 | 168 | velist[coord].resize(size() * 2); | |
429 | 168 | EndPoint* current = elist[coord]; | |
430 | 168 | size_t id = 0; | |
431 |
2/2✓ Branch 0 taken 84090 times.
✓ Branch 1 taken 168 times.
|
84258 | while (current) { |
432 | 84090 | velist[coord][id] = current; | |
433 | 84090 | current = current->next[coord]; | |
434 | 84090 | id++; | |
435 | } | ||
436 | } | ||
437 | 56 | } | |
438 | |||
439 | //============================================================================== | ||
440 | ✗ | void SaPCollisionManager::update(CollisionObject* updated_obj) { | |
441 | ✗ | update_(obj_aabb_map[updated_obj]); | |
442 | |||
443 | ✗ | updateVelist(); | |
444 | |||
445 | ✗ | setup(); | |
446 | } | ||
447 | |||
448 | //============================================================================== | ||
449 | ✗ | void SaPCollisionManager::update( | |
450 | const std::vector<CollisionObject*>& updated_objs) { | ||
451 | ✗ | for (size_t i = 0; i < updated_objs.size(); ++i) | |
452 | ✗ | update_(obj_aabb_map[updated_objs[i]]); | |
453 | |||
454 | ✗ | updateVelist(); | |
455 | |||
456 | ✗ | setup(); | |
457 | } | ||
458 | |||
459 | //============================================================================== | ||
460 | 13 | void SaPCollisionManager::update() { | |
461 |
2/2✓ Branch 4 taken 1344 times.
✓ Branch 5 taken 13 times.
|
1357 | for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { |
462 |
1/2✓ Branch 2 taken 1344 times.
✗ Branch 3 not taken.
|
1344 | update_(*it); |
463 | } | ||
464 | |||
465 | 13 | updateVelist(); | |
466 | |||
467 | 13 | setup(); | |
468 | 13 | } | |
469 | |||
470 | //============================================================================== | ||
471 | 50 | void SaPCollisionManager::clear() { | |
472 |
2/2✓ Branch 3 taken 12371 times.
✓ Branch 4 taken 50 times.
|
12421 | for (auto it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) { |
473 |
1/2✓ Branch 1 taken 12371 times.
✗ Branch 2 not taken.
|
12371 | delete (*it)->hi; |
474 |
1/2✓ Branch 1 taken 12371 times.
✗ Branch 2 not taken.
|
12371 | delete (*it)->lo; |
475 |
1/2✓ Branch 1 taken 12371 times.
✗ Branch 2 not taken.
|
12371 | delete *it; |
476 | 12371 | *it = nullptr; | |
477 | } | ||
478 | |||
479 | 50 | AABB_arr.clear(); | |
480 | 50 | overlap_pairs.clear(); | |
481 | |||
482 | 50 | elist[0] = nullptr; | |
483 | 50 | elist[1] = nullptr; | |
484 | 50 | elist[2] = nullptr; | |
485 | |||
486 | 50 | velist[0].clear(); | |
487 | 50 | velist[1].clear(); | |
488 | 50 | velist[2].clear(); | |
489 | |||
490 | 50 | obj_aabb_map.clear(); | |
491 | 50 | } | |
492 | |||
493 | //============================================================================== | ||
494 | ✗ | void SaPCollisionManager::getObjects( | |
495 | std::vector<CollisionObject*>& objs) const { | ||
496 | ✗ | objs.resize(AABB_arr.size()); | |
497 | ✗ | size_t i = 0; | |
498 | ✗ | for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; | |
499 | ✗ | ++it, ++i) { | |
500 | ✗ | objs[i] = (*it)->obj; | |
501 | } | ||
502 | } | ||
503 | |||
504 | //============================================================================== | ||
505 | 3762 | bool SaPCollisionManager::collide_(CollisionObject* obj, | |
506 | CollisionCallBackBase* callback) const { | ||
507 | 3762 | int axis = optimal_axis; | |
508 | 3762 | const AABB& obj_aabb = obj->getAABB(); | |
509 | |||
510 |
1/2✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
|
3762 | Scalar min_val = obj_aabb.min_[axis]; |
511 | // Scalar max_val = obj_aabb.max_[axis]; | ||
512 | |||
513 | EndPoint dummy; | ||
514 |
1/2✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
|
3762 | SaPAABB dummy_aabb; |
515 |
1/2✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
|
3762 | dummy_aabb.cached = obj_aabb; |
516 | 3762 | dummy.minmax = 1; | |
517 | 3762 | dummy.aabb = &dummy_aabb; | |
518 | |||
519 | // compute stop_pos by binary search, this is cheaper than check it in while | ||
520 | // iteration linearly | ||
521 |
1/2✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
|
3762 | const auto res_it = std::upper_bound( |
522 | 3762 | velist[axis].begin(), velist[axis].end(), &dummy, | |
523 |
1/2✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
|
3762 | std::bind(std::less<Scalar>(), |
524 |
1/2✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
|
3762 | std::bind(static_cast<Scalar (EndPoint::*)(int) const>( |
525 | &EndPoint::getVal), | ||
526 | std::placeholders::_1, axis), | ||
527 |
1/2✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
|
3762 | std::bind(static_cast<Scalar (EndPoint::*)(int) const>( |
528 | &EndPoint::getVal), | ||
529 | std::placeholders::_2, axis))); | ||
530 | |||
531 | 3762 | EndPoint* end_pos = nullptr; | |
532 |
2/2✓ Branch 2 taken 3721 times.
✓ Branch 3 taken 41 times.
|
3762 | if (res_it != velist[axis].end()) end_pos = *res_it; |
533 | |||
534 | 3762 | EndPoint* pos = elist[axis]; | |
535 | |||
536 |
2/2✓ Branch 0 taken 599528 times.
✓ Branch 1 taken 3761 times.
|
603289 | while (pos != end_pos) { |
537 |
1/2✓ Branch 0 taken 599528 times.
✗ Branch 1 not taken.
|
599528 | if (pos->aabb->obj != obj) { |
538 |
7/8✓ Branch 0 taken 303272 times.
✓ Branch 1 taken 296256 times.
✓ Branch 3 taken 303272 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 14014 times.
✓ Branch 6 taken 289258 times.
✓ Branch 7 taken 14014 times.
✓ Branch 8 taken 585514 times.
|
599528 | if ((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) { |
539 |
3/4✓ Branch 2 taken 14014 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 19 times.
✓ Branch 5 taken 13995 times.
|
14014 | if (pos->aabb->cached.overlap(obj->getAABB())) |
540 |
3/4✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 18 times.
|
19 | if ((*callback)(obj, pos->aabb->obj)) return true; |
541 | } | ||
542 | } | ||
543 | 599527 | pos = pos->next[axis]; | |
544 | } | ||
545 | |||
546 | 3761 | return false; | |
547 | } | ||
548 | |||
549 | //============================================================================== | ||
550 | 11 | void SaPCollisionManager::addToOverlapPairs(const SaPPair& p) { | |
551 | 11 | bool repeated = false; | |
552 |
2/2✓ Branch 3 taken 18 times.
✓ Branch 4 taken 8 times.
|
26 | for (auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; |
553 | 15 | ++it) { | |
554 |
3/4✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✓ Branch 5 taken 15 times.
|
18 | if (*it == p) { |
555 | 3 | repeated = true; | |
556 | 3 | break; | |
557 | } | ||
558 | } | ||
559 | |||
560 |
2/2✓ Branch 0 taken 8 times.
✓ Branch 1 taken 3 times.
|
11 | if (!repeated) overlap_pairs.push_back(p); |
561 | 11 | } | |
562 | |||
563 | //============================================================================== | ||
564 | 8 | void SaPCollisionManager::removeFromOverlapPairs(const SaPPair& p) { | |
565 |
2/2✓ Branch 3 taken 12 times.
✓ Branch 4 taken 2 times.
|
14 | for (auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; |
566 | 6 | ++it) { | |
567 |
3/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 6 times.
|
12 | if (*it == p) { |
568 | 6 | overlap_pairs.erase(it); | |
569 | 6 | break; | |
570 | } | ||
571 | } | ||
572 | |||
573 | // or overlap_pairs.remove_if(isNotValidPair(p)); | ||
574 | 8 | } | |
575 | |||
576 | //============================================================================== | ||
577 | 3822 | void SaPCollisionManager::collide(CollisionObject* obj, | |
578 | CollisionCallBackBase* callback) const { | ||
579 | COAL_TRACY_ZONE_SCOPED_N( | ||
580 | "coal::SaPCollisionManager::collide(CollisionObject*, " | ||
581 | "CollisionCallBackBase*)"); | ||
582 | 3822 | callback->init(); | |
583 |
2/2✓ Branch 1 taken 60 times.
✓ Branch 2 taken 3762 times.
|
3822 | if (size() == 0) return; |
584 | |||
585 | 3762 | collide_(obj, callback); | |
586 | } | ||
587 | |||
588 | //============================================================================== | ||
589 | 3070 | bool SaPCollisionManager::distance_(CollisionObject* obj, | |
590 | DistanceCallBackBase* callback, | ||
591 | Scalar& min_dist) const { | ||
592 |
3/6✓ Branch 3 taken 3070 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3070 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3070 times.
✗ Branch 10 not taken.
|
3070 | Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; |
593 |
1/2✓ Branch 2 taken 3070 times.
✗ Branch 3 not taken.
|
3070 | AABB aabb = obj->getAABB(); |
594 | |||
595 |
2/2✓ Branch 1 taken 2984 times.
✓ Branch 2 taken 86 times.
|
3070 | if (min_dist < (std::numeric_limits<Scalar>::max)()) { |
596 |
1/2✓ Branch 1 taken 2984 times.
✗ Branch 2 not taken.
|
2984 | Vec3s min_dist_delta(min_dist, min_dist, min_dist); |
597 |
1/2✓ Branch 1 taken 2984 times.
✗ Branch 2 not taken.
|
2984 | aabb.expand(min_dist_delta); |
598 | } | ||
599 | |||
600 | 3070 | int axis = optimal_axis; | |
601 | |||
602 | 3070 | int status = 1; | |
603 | Scalar old_min_distance; | ||
604 | |||
605 | 3070 | EndPoint* start_pos = elist[axis]; | |
606 | |||
607 | while (1) { | ||
608 | 3200 | old_min_distance = min_dist; | |
609 |
1/2✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
|
3200 | Scalar min_val = aabb.min_[axis]; |
610 | // Scalar max_val = aabb.max_[axis]; | ||
611 | |||
612 | EndPoint dummy; | ||
613 |
1/2✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
|
3200 | SaPAABB dummy_aabb; |
614 |
1/2✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
|
3200 | dummy_aabb.cached = aabb; |
615 | 3200 | dummy.minmax = 1; | |
616 | 3200 | dummy.aabb = &dummy_aabb; | |
617 | |||
618 |
1/2✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
|
3200 | const auto res_it = std::upper_bound( |
619 | 3200 | velist[axis].begin(), velist[axis].end(), &dummy, | |
620 |
1/2✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
|
3200 | std::bind(std::less<Scalar>(), |
621 |
1/2✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
|
3200 | std::bind(static_cast<Scalar (EndPoint::*)(int) const>( |
622 | &EndPoint::getVal), | ||
623 | std::placeholders::_1, axis), | ||
624 |
1/2✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
|
3200 | std::bind(static_cast<Scalar (EndPoint::*)(int) const>( |
625 | &EndPoint::getVal), | ||
626 | std::placeholders::_2, axis))); | ||
627 | |||
628 | 3200 | EndPoint* end_pos = nullptr; | |
629 |
2/2✓ Branch 2 taken 3180 times.
✓ Branch 3 taken 20 times.
|
3200 | if (res_it != velist[axis].end()) end_pos = *res_it; |
630 | |||
631 | 3200 | EndPoint* pos = start_pos; | |
632 | |||
633 |
2/2✓ Branch 0 taken 3259389 times.
✓ Branch 1 taken 3200 times.
|
3262589 | while (pos != end_pos) { |
634 | // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and | ||
635 | // then update start_pos to end_pos. but this seems slower. | ||
636 |
7/8✓ Branch 0 taken 1635306 times.
✓ Branch 1 taken 1624083 times.
✓ Branch 3 taken 1635306 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 28806 times.
✓ Branch 6 taken 1606500 times.
✓ Branch 7 taken 28806 times.
✓ Branch 8 taken 3230583 times.
|
3259389 | if ((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) { |
637 | 28806 | CollisionObject* curr_obj = pos->aabb->obj; | |
638 |
2/2✓ Branch 0 taken 25776 times.
✓ Branch 1 taken 3030 times.
|
28806 | if (curr_obj != obj) { |
639 |
2/2✓ Branch 0 taken 21284 times.
✓ Branch 1 taken 4492 times.
|
25776 | if (!this->enable_tested_set_) { |
640 |
3/4✓ Branch 2 taken 21284 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1399 times.
✓ Branch 5 taken 19885 times.
|
21284 | if (pos->aabb->cached.distance(obj->getAABB()) < min_dist) { |
641 |
2/4✓ Branch 1 taken 1399 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1399 times.
|
1399 | if ((*callback)(curr_obj, obj, min_dist)) return true; |
642 | } | ||
643 | } else { | ||
644 |
3/4✓ Branch 1 taken 4492 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2240 times.
✓ Branch 4 taken 2252 times.
|
4492 | if (!this->inTestedSet(curr_obj, obj)) { |
645 |
3/4✓ Branch 2 taken 2240 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 243 times.
✓ Branch 5 taken 1997 times.
|
2240 | if (pos->aabb->cached.distance(obj->getAABB()) < min_dist) { |
646 |
2/4✓ Branch 1 taken 243 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 243 times.
|
243 | if ((*callback)(curr_obj, obj, min_dist)) return true; |
647 | } | ||
648 | |||
649 |
1/2✓ Branch 1 taken 2240 times.
✗ Branch 2 not taken.
|
2240 | this->insertTestedSet(curr_obj, obj); |
650 | } | ||
651 | } | ||
652 | } | ||
653 | } | ||
654 | |||
655 | 3259389 | pos = pos->next[axis]; | |
656 | } | ||
657 | |||
658 |
2/2✓ Branch 0 taken 3114 times.
✓ Branch 1 taken 86 times.
|
3200 | if (status == 1) { |
659 |
2/2✓ Branch 1 taken 2984 times.
✓ Branch 2 taken 130 times.
|
3114 | if (old_min_distance < (std::numeric_limits<Scalar>::max)()) |
660 | 2984 | break; | |
661 | else { | ||
662 |
2/2✓ Branch 0 taken 86 times.
✓ Branch 1 taken 44 times.
|
130 | if (min_dist < old_min_distance) { |
663 |
1/2✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
|
86 | Vec3s min_dist_delta(min_dist, min_dist, min_dist); |
664 |
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); |
665 | 86 | status = 0; | |
666 | } else { | ||
667 |
3/4✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 9 times.
✓ Branch 5 taken 35 times.
|
44 | if (aabb == obj->getAABB()) |
668 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | aabb.expand(delta); |
669 | else | ||
670 |
1/2✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
|
35 | aabb.expand(obj->getAABB(), 2.0); |
671 | } | ||
672 | } | ||
673 |
1/2✓ Branch 0 taken 86 times.
✗ Branch 1 not taken.
|
86 | } else if (status == 0) |
674 | 86 | break; | |
675 | 130 | } | |
676 | |||
677 | 3070 | return false; | |
678 | } | ||
679 | |||
680 | //============================================================================== | ||
681 | 80 | void SaPCollisionManager::distance(CollisionObject* obj, | |
682 | DistanceCallBackBase* callback) const { | ||
683 | COAL_TRACY_ZONE_SCOPED_N( | ||
684 | "coal::SaPCollisionManager::distance(CollisionObject*, " | ||
685 | "DistanceCallBackBase*)"); | ||
686 |
1/2✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
|
80 | callback->init(); |
687 |
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; |
688 | |||
689 | 80 | Scalar min_dist = (std::numeric_limits<Scalar>::max)(); | |
690 | |||
691 |
1/2✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
|
80 | distance_(obj, callback, min_dist); |
692 | } | ||
693 | |||
694 | //============================================================================== | ||
695 | 37 | void SaPCollisionManager::collide(CollisionCallBackBase* callback) const { | |
696 | COAL_TRACY_ZONE_SCOPED_N( | ||
697 | "coal::SaPCollisionManager::collide(CollisionCallBackBase*)"); | ||
698 | 37 | callback->init(); | |
699 |
2/2✓ Branch 1 taken 8 times.
✓ Branch 2 taken 29 times.
|
37 | if (size() == 0) return; |
700 | |||
701 |
2/2✓ Branch 3 taken 7 times.
✓ Branch 4 taken 26 times.
|
33 | for (auto it = overlap_pairs.cbegin(), end = overlap_pairs.cend(); it != end; |
702 | 4 | ++it) { | |
703 | 7 | CollisionObject* obj1 = it->obj1; | |
704 | 7 | CollisionObject* obj2 = it->obj2; | |
705 | |||
706 |
3/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 4 times.
|
7 | if ((*callback)(obj1, obj2)) return; |
707 | } | ||
708 | } | ||
709 | |||
710 | //============================================================================== | ||
711 | 6 | void SaPCollisionManager::distance(DistanceCallBackBase* callback) const { | |
712 | COAL_TRACY_ZONE_SCOPED_N( | ||
713 | "coal::SaPCollisionManager::distance(DistanceCallBackBase*)"); | ||
714 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | callback->init(); |
715 |
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; |
716 | |||
717 | 6 | this->enable_tested_set_ = true; | |
718 | 6 | this->tested_set.clear(); | |
719 | |||
720 | 6 | Scalar min_dist = (std::numeric_limits<Scalar>::max)(); | |
721 | |||
722 |
2/2✓ Branch 4 taken 2990 times.
✓ Branch 5 taken 6 times.
|
2996 | for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { |
723 |
2/4✓ Branch 2 taken 2990 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2990 times.
|
2990 | if (distance_((*it)->obj, callback, min_dist)) break; |
724 | } | ||
725 | |||
726 | 6 | this->enable_tested_set_ = false; | |
727 | 6 | this->tested_set.clear(); | |
728 | } | ||
729 | |||
730 | //============================================================================== | ||
731 | ✗ | void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, | |
732 | CollisionCallBackBase* callback) const { | ||
733 | COAL_TRACY_ZONE_SCOPED_N( | ||
734 | "coal::SaPCollisionManager::collide(BroadPhaseCollisionManager*, " | ||
735 | "CollisionCallBackBase*)"); | ||
736 | ✗ | callback->init(); | |
737 | ✗ | SaPCollisionManager* other_manager = | |
738 | static_cast<SaPCollisionManager*>(other_manager_); | ||
739 | |||
740 | ✗ | if ((size() == 0) || (other_manager->size() == 0)) return; | |
741 | |||
742 | ✗ | if (this == other_manager) { | |
743 | ✗ | collide(callback); | |
744 | ✗ | return; | |
745 | } | ||
746 | |||
747 | ✗ | if (this->size() < other_manager->size()) { | |
748 | ✗ | for (auto it = AABB_arr.cbegin(); it != AABB_arr.cend(); ++it) { | |
749 | ✗ | if (other_manager->collide_((*it)->obj, callback)) return; | |
750 | } | ||
751 | } else { | ||
752 | ✗ | for (auto it = other_manager->AABB_arr.cbegin(), | |
753 | ✗ | end = other_manager->AABB_arr.cend(); | |
754 | ✗ | it != end; ++it) { | |
755 | ✗ | if (collide_((*it)->obj, callback)) return; | |
756 | } | ||
757 | } | ||
758 | } | ||
759 | |||
760 | //============================================================================== | ||
761 | ✗ | void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, | |
762 | DistanceCallBackBase* callback) const { | ||
763 | COAL_TRACY_ZONE_SCOPED_N( | ||
764 | "coal::SaPCollisionManager::distance(BroadPhaseCollisionManager*, " | ||
765 | "DistanceCallBackBase*)"); | ||
766 | ✗ | callback->init(); | |
767 | ✗ | SaPCollisionManager* other_manager = | |
768 | static_cast<SaPCollisionManager*>(other_manager_); | ||
769 | |||
770 | ✗ | if ((size() == 0) || (other_manager->size() == 0)) return; | |
771 | |||
772 | ✗ | if (this == other_manager) { | |
773 | ✗ | distance(callback); | |
774 | ✗ | return; | |
775 | } | ||
776 | |||
777 | ✗ | Scalar min_dist = (std::numeric_limits<Scalar>::max)(); | |
778 | |||
779 | ✗ | if (this->size() < other_manager->size()) { | |
780 | ✗ | for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { | |
781 | ✗ | if (other_manager->distance_((*it)->obj, callback, min_dist)) return; | |
782 | } | ||
783 | } else { | ||
784 | ✗ | for (auto it = other_manager->AABB_arr.cbegin(), | |
785 | ✗ | end = other_manager->AABB_arr.cend(); | |
786 | ✗ | it != end; ++it) { | |
787 | ✗ | if (distance_((*it)->obj, callback, min_dist)) return; | |
788 | } | ||
789 | } | ||
790 | } | ||
791 | |||
792 | //============================================================================== | ||
793 | ✗ | bool SaPCollisionManager::empty() const { return AABB_arr.empty(); } | |
794 | |||
795 | //============================================================================== | ||
796 | 4220 | size_t SaPCollisionManager::size() const { return AABB_arr.size(); } | |
797 | |||
798 | //============================================================================== | ||
799 | ✗ | const Vec3s& SaPCollisionManager::EndPoint::getVal() const { | |
800 | ✗ | if (minmax) | |
801 | ✗ | return aabb->cached.max_; | |
802 | else | ||
803 | ✗ | return aabb->cached.min_; | |
804 | } | ||
805 | |||
806 | //============================================================================== | ||
807 | ✗ | Vec3s& SaPCollisionManager::EndPoint::getVal() { | |
808 | ✗ | if (minmax) | |
809 | ✗ | return aabb->cached.max_; | |
810 | else | ||
811 | ✗ | return aabb->cached.min_; | |
812 | } | ||
813 | |||
814 | //============================================================================== | ||
815 | 2109752 | Scalar SaPCollisionManager::EndPoint::getVal(int i) const { | |
816 |
2/2✓ Branch 0 taken 1081916 times.
✓ Branch 1 taken 1027836 times.
|
2109752 | if (minmax) |
817 | 1081916 | return aabb->cached.max_[i]; | |
818 | else | ||
819 | 1027836 | return aabb->cached.min_[i]; | |
820 | } | ||
821 | |||
822 | //============================================================================== | ||
823 | 2000983 | Scalar& SaPCollisionManager::EndPoint::getVal(int i) { | |
824 |
2/2✓ Branch 0 taken 1967773 times.
✓ Branch 1 taken 33210 times.
|
2000983 | if (minmax) |
825 | 1967773 | return aabb->cached.max_[i]; | |
826 | else | ||
827 | 33210 | return aabb->cached.min_[i]; | |
828 | } | ||
829 | |||
830 | //============================================================================== | ||
831 | 78474 | SaPCollisionManager::SaPPair::SaPPair(CollisionObject* a, CollisionObject* b) { | |
832 |
2/2✓ Branch 0 taken 37800 times.
✓ Branch 1 taken 40674 times.
|
78474 | if (a < b) { |
833 | 37800 | obj1 = a; | |
834 | 37800 | obj2 = b; | |
835 | } else { | ||
836 | 40674 | obj1 = b; | |
837 | 40674 | obj2 = a; | |
838 | } | ||
839 | 78474 | } | |
840 | |||
841 | //============================================================================== | ||
842 | 30 | bool SaPCollisionManager::SaPPair::operator==(const SaPPair& other) const { | |
843 |
3/4✓ Branch 0 taken 9 times.
✓ Branch 1 taken 21 times.
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
|
30 | return ((obj1 == other.obj1) && (obj2 == other.obj2)); |
844 | } | ||
845 | |||
846 | //============================================================================== | ||
847 | ✗ | SaPCollisionManager::isUnregistered::isUnregistered(CollisionObject* obj_) | |
848 | ✗ | : obj(obj_) {} | |
849 | |||
850 | //============================================================================== | ||
851 | ✗ | bool SaPCollisionManager::isUnregistered::operator()( | |
852 | const SaPPair& pair) const { | ||
853 | ✗ | return (pair.obj1 == obj) || (pair.obj2 == obj); | |
854 | } | ||
855 | |||
856 | //============================================================================== | ||
857 | ✗ | SaPCollisionManager::isNotValidPair::isNotValidPair(CollisionObject* obj1_, | |
858 | ✗ | CollisionObject* obj2_) | |
859 | ✗ | : obj1(obj1_), obj2(obj2_) { | |
860 | // Do nothing | ||
861 | } | ||
862 | |||
863 | //============================================================================== | ||
864 | ✗ | bool SaPCollisionManager::isNotValidPair::operator()(const SaPPair& pair) { | |
865 | ✗ | return (pair.obj1 == obj1) && (pair.obj2 == obj2); | |
866 | } | ||
867 | |||
868 | } // namespace coal | ||
869 |