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