| 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 2 taken 43 times.
✗ Branch 3 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/6✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12671 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 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 37884 times.
✓ Branch 1 taken 40590 times.
|
78474 | if (a < b) { |
| 833 | 37884 | obj1 = a; | |
| 834 | 37884 | obj2 = b; | |
| 835 | } else { | ||
| 836 | 40590 | obj1 = b; | |
| 837 | 40590 | 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 |