| 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 | #ifndef COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H | ||
| 39 | #define COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H | ||
| 40 | |||
| 41 | #include "coal/broadphase/broadphase_spatialhash.h" | ||
| 42 | |||
| 43 | namespace coal { | ||
| 44 | |||
| 45 | //============================================================================== | ||
| 46 | template <typename HashTable> | ||
| 47 | 51 | SpatialHashingCollisionManager<HashTable>::SpatialHashingCollisionManager( | |
| 48 | Scalar cell_size, const Vec3s& scene_min, const Vec3s& scene_max, | ||
| 49 | unsigned int default_table_size) | ||
| 50 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
51 | : scene_limit(AABB(scene_min, scene_max)), |
| 51 |
3/8✓ Branch 6 taken 51 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 51 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 51 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
102 | hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) { |
| 52 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
51 | hash_table->init(default_table_size); |
| 53 | 51 | } | |
| 54 | |||
| 55 | //============================================================================== | ||
| 56 | template <typename HashTable> | ||
| 57 | 200 | SpatialHashingCollisionManager<HashTable>::~SpatialHashingCollisionManager() { | |
| 58 | 100 | delete hash_table; | |
| 59 |
1/2✓ Branch 0 taken 50 times.
✗ Branch 1 not taken.
|
300 | } |
| 60 | |||
| 61 | //============================================================================== | ||
| 62 | template <typename HashTable> | ||
| 63 | 12671 | void SpatialHashingCollisionManager<HashTable>::registerObject( | |
| 64 | CollisionObject* obj) { | ||
| 65 |
1/2✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
|
12671 | objs.push_back(obj); |
| 66 | |||
| 67 | 12671 | const AABB& obj_aabb = obj->getAABB(); | |
| 68 |
1/2✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
|
12671 | AABB overlap_aabb; |
| 69 | |||
| 70 |
2/4✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12671 times.
✗ Branch 4 not taken.
|
12671 | if (scene_limit.overlap(obj_aabb, overlap_aabb)) { |
| 71 |
2/4✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 12671 times.
|
12671 | if (!scene_limit.contain(obj_aabb)) |
| 72 | ✗ | objs_partially_penetrating_scene_limit.push_back(obj); | |
| 73 | |||
| 74 |
2/4✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12671 times.
✗ Branch 5 not taken.
|
12671 | hash_table->insert(overlap_aabb, obj); |
| 75 | } else { | ||
| 76 | ✗ | objs_outside_scene_limit.push_back(obj); | |
| 77 | } | ||
| 78 | |||
| 79 |
2/4✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12671 times.
✗ Branch 5 not taken.
|
12671 | obj_aabb_map[obj] = obj_aabb; |
| 80 | 12671 | } | |
| 81 | |||
| 82 | //============================================================================== | ||
| 83 | template <typename HashTable> | ||
| 84 | ✗ | void SpatialHashingCollisionManager<HashTable>::unregisterObject( | |
| 85 | CollisionObject* obj) { | ||
| 86 | ✗ | objs.remove(obj); | |
| 87 | |||
| 88 | ✗ | const AABB& obj_aabb = obj->getAABB(); | |
| 89 | ✗ | AABB overlap_aabb; | |
| 90 | |||
| 91 | ✗ | if (scene_limit.overlap(obj_aabb, overlap_aabb)) { | |
| 92 | ✗ | if (!scene_limit.contain(obj_aabb)) | |
| 93 | ✗ | objs_partially_penetrating_scene_limit.remove(obj); | |
| 94 | |||
| 95 | ✗ | hash_table->remove(overlap_aabb, obj); | |
| 96 | } else { | ||
| 97 | ✗ | objs_outside_scene_limit.remove(obj); | |
| 98 | } | ||
| 99 | |||
| 100 | ✗ | obj_aabb_map.erase(obj); | |
| 101 | ✗ | } | |
| 102 | |||
| 103 | //============================================================================== | ||
| 104 | template <typename HashTable> | ||
| 105 | 51 | void SpatialHashingCollisionManager<HashTable>::setup() { | |
| 106 | // Do nothing | ||
| 107 | 51 | } | |
| 108 | |||
| 109 | //============================================================================== | ||
| 110 | template <typename HashTable> | ||
| 111 | 13 | void SpatialHashingCollisionManager<HashTable>::update() { | |
| 112 | 13 | hash_table->clear(); | |
| 113 | 13 | objs_partially_penetrating_scene_limit.clear(); | |
| 114 | 13 | objs_outside_scene_limit.clear(); | |
| 115 | |||
| 116 |
2/2✓ Branch 4 taken 1344 times.
✓ Branch 5 taken 13 times.
|
1357 | for (auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) { |
| 117 | 1344 | CollisionObject* obj = *it; | |
| 118 | 1344 | const AABB& obj_aabb = obj->getAABB(); | |
| 119 |
1/2✓ Branch 1 taken 1344 times.
✗ Branch 2 not taken.
|
1344 | AABB overlap_aabb; |
| 120 | |||
| 121 |
3/4✓ Branch 1 taken 1344 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1333 times.
✓ Branch 4 taken 11 times.
|
1344 | if (scene_limit.overlap(obj_aabb, overlap_aabb)) { |
| 122 |
3/4✓ Branch 1 taken 1333 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 44 times.
✓ Branch 4 taken 1289 times.
|
1333 | if (!scene_limit.contain(obj_aabb)) |
| 123 |
1/2✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
|
44 | objs_partially_penetrating_scene_limit.push_back(obj); |
| 124 | |||
| 125 |
2/4✓ Branch 1 taken 1333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1333 times.
✗ Branch 5 not taken.
|
1333 | hash_table->insert(overlap_aabb, obj); |
| 126 | } else { | ||
| 127 |
1/2✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
|
11 | objs_outside_scene_limit.push_back(obj); |
| 128 | } | ||
| 129 | |||
| 130 |
2/4✓ Branch 1 taken 1344 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1344 times.
✗ Branch 5 not taken.
|
1344 | obj_aabb_map[obj] = obj_aabb; |
| 131 | } | ||
| 132 | 13 | } | |
| 133 | |||
| 134 | //============================================================================== | ||
| 135 | template <typename HashTable> | ||
| 136 | ✗ | void SpatialHashingCollisionManager<HashTable>::update( | |
| 137 | CollisionObject* updated_obj) { | ||
| 138 | ✗ | const AABB& new_aabb = updated_obj->getAABB(); | |
| 139 | ✗ | const AABB& old_aabb = obj_aabb_map[updated_obj]; | |
| 140 | |||
| 141 | ✗ | AABB old_overlap_aabb; | |
| 142 | const auto is_old_aabb_overlapping = | ||
| 143 | ✗ | scene_limit.overlap(old_aabb, old_overlap_aabb); | |
| 144 | ✗ | if (is_old_aabb_overlapping) | |
| 145 | ✗ | hash_table->remove(old_overlap_aabb, updated_obj); | |
| 146 | |||
| 147 | ✗ | AABB new_overlap_aabb; | |
| 148 | const auto is_new_aabb_overlapping = | ||
| 149 | ✗ | scene_limit.overlap(new_aabb, new_overlap_aabb); | |
| 150 | ✗ | if (is_new_aabb_overlapping) | |
| 151 | ✗ | hash_table->insert(new_overlap_aabb, updated_obj); | |
| 152 | |||
| 153 | ObjectStatus old_status; | ||
| 154 | ✗ | if (is_old_aabb_overlapping) { | |
| 155 | ✗ | if (scene_limit.contain(old_aabb)) | |
| 156 | ✗ | old_status = Inside; | |
| 157 | else | ||
| 158 | ✗ | old_status = PartiallyPenetrating; | |
| 159 | } else { | ||
| 160 | ✗ | old_status = Outside; | |
| 161 | } | ||
| 162 | |||
| 163 | ✗ | if (is_new_aabb_overlapping) { | |
| 164 | ✗ | if (scene_limit.contain(new_aabb)) { | |
| 165 | ✗ | if (old_status == PartiallyPenetrating) { | |
| 166 | // Status change: PartiallyPenetrating --> Inside | ||
| 167 | // Required action(s): | ||
| 168 | // - remove object from "objs_partially_penetrating_scene_limit" | ||
| 169 | |||
| 170 | ✗ | auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(), | |
| 171 | objs_partially_penetrating_scene_limit.end(), | ||
| 172 | updated_obj); | ||
| 173 | ✗ | objs_partially_penetrating_scene_limit.erase(find_it); | |
| 174 | ✗ | } else if (old_status == Outside) { | |
| 175 | // Status change: Outside --> Inside | ||
| 176 | // Required action(s): | ||
| 177 | // - remove object from "objs_outside_scene_limit" | ||
| 178 | |||
| 179 | ✗ | auto find_it = std::find(objs_outside_scene_limit.begin(), | |
| 180 | objs_outside_scene_limit.end(), updated_obj); | ||
| 181 | ✗ | objs_outside_scene_limit.erase(find_it); | |
| 182 | } | ||
| 183 | } else { | ||
| 184 | ✗ | if (old_status == Inside) { | |
| 185 | // Status change: Inside --> PartiallyPenetrating | ||
| 186 | // Required action(s): | ||
| 187 | // - add object to "objs_partially_penetrating_scene_limit" | ||
| 188 | |||
| 189 | ✗ | objs_partially_penetrating_scene_limit.push_back(updated_obj); | |
| 190 | ✗ | } else if (old_status == Outside) { | |
| 191 | // Status change: Outside --> PartiallyPenetrating | ||
| 192 | // Required action(s): | ||
| 193 | // - remove object from "objs_outside_scene_limit" | ||
| 194 | // - add object to "objs_partially_penetrating_scene_limit" | ||
| 195 | |||
| 196 | ✗ | auto find_it = std::find(objs_outside_scene_limit.begin(), | |
| 197 | objs_outside_scene_limit.end(), updated_obj); | ||
| 198 | ✗ | objs_outside_scene_limit.erase(find_it); | |
| 199 | |||
| 200 | ✗ | objs_partially_penetrating_scene_limit.push_back(updated_obj); | |
| 201 | } | ||
| 202 | } | ||
| 203 | } else { | ||
| 204 | ✗ | if (old_status == Inside) { | |
| 205 | // Status change: Inside --> Outside | ||
| 206 | // Required action(s): | ||
| 207 | // - add object to "objs_outside_scene_limit" | ||
| 208 | |||
| 209 | ✗ | objs_outside_scene_limit.push_back(updated_obj); | |
| 210 | ✗ | } else if (old_status == PartiallyPenetrating) { | |
| 211 | // Status change: PartiallyPenetrating --> Outside | ||
| 212 | // Required action(s): | ||
| 213 | // - remove object from "objs_partially_penetrating_scene_limit" | ||
| 214 | // - add object to "objs_outside_scene_limit" | ||
| 215 | |||
| 216 | auto find_it = | ||
| 217 | ✗ | std::find(objs_partially_penetrating_scene_limit.begin(), | |
| 218 | objs_partially_penetrating_scene_limit.end(), updated_obj); | ||
| 219 | ✗ | objs_partially_penetrating_scene_limit.erase(find_it); | |
| 220 | |||
| 221 | ✗ | objs_outside_scene_limit.push_back(updated_obj); | |
| 222 | } | ||
| 223 | } | ||
| 224 | |||
| 225 | ✗ | obj_aabb_map[updated_obj] = new_aabb; | |
| 226 | ✗ | } | |
| 227 | |||
| 228 | //============================================================================== | ||
| 229 | template <typename HashTable> | ||
| 230 | ✗ | void SpatialHashingCollisionManager<HashTable>::update( | |
| 231 | const std::vector<CollisionObject*>& updated_objs) { | ||
| 232 | ✗ | for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]); | |
| 233 | ✗ | } | |
| 234 | |||
| 235 | //============================================================================== | ||
| 236 | template <typename HashTable> | ||
| 237 | ✗ | void SpatialHashingCollisionManager<HashTable>::clear() { | |
| 238 | ✗ | objs.clear(); | |
| 239 | ✗ | hash_table->clear(); | |
| 240 | ✗ | objs_outside_scene_limit.clear(); | |
| 241 | ✗ | obj_aabb_map.clear(); | |
| 242 | ✗ | } | |
| 243 | |||
| 244 | //============================================================================== | ||
| 245 | template <typename HashTable> | ||
| 246 | ✗ | void SpatialHashingCollisionManager<HashTable>::getObjects( | |
| 247 | std::vector<CollisionObject*>& objs_) const { | ||
| 248 | ✗ | objs_.resize(objs.size()); | |
| 249 | ✗ | std::copy(objs.begin(), objs.end(), objs_.begin()); | |
| 250 | ✗ | } | |
| 251 | |||
| 252 | //============================================================================== | ||
| 253 | template <typename HashTable> | ||
| 254 | 3822 | void SpatialHashingCollisionManager<HashTable>::collide( | |
| 255 | CollisionObject* obj, CollisionCallBackBase* callback) const { | ||
| 256 |
2/2✓ Branch 1 taken 60 times.
✓ Branch 2 taken 3762 times.
|
3822 | if (size() == 0) return; |
| 257 | 3762 | collide_(obj, callback); | |
| 258 | } | ||
| 259 | |||
| 260 | //============================================================================== | ||
| 261 | template <typename HashTable> | ||
| 262 | 80 | void SpatialHashingCollisionManager<HashTable>::distance( | |
| 263 | CollisionObject* obj, DistanceCallBackBase* callback) const { | ||
| 264 |
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; |
| 265 | 80 | Scalar min_dist = (std::numeric_limits<Scalar>::max)(); | |
| 266 |
1/2✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
|
80 | distance_(obj, callback, min_dist); |
| 267 | } | ||
| 268 | |||
| 269 | //============================================================================== | ||
| 270 | template <typename HashTable> | ||
| 271 | 3762 | bool SpatialHashingCollisionManager<HashTable>::collide_( | |
| 272 | CollisionObject* obj, CollisionCallBackBase* callback) const { | ||
| 273 | 3762 | const auto& obj_aabb = obj->getAABB(); | |
| 274 |
1/2✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
|
3762 | AABB overlap_aabb; |
| 275 | |||
| 276 |
3/4✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3455 times.
✓ Branch 4 taken 307 times.
|
3762 | if (scene_limit.overlap(obj_aabb, overlap_aabb)) { |
| 277 |
2/4✓ Branch 1 taken 3455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3455 times.
✗ Branch 5 not taken.
|
3455 | const auto query_result = hash_table->query(overlap_aabb); |
| 278 |
2/2✓ Branch 5 taken 207 times.
✓ Branch 6 taken 3454 times.
|
3661 | for (const auto& obj2 : query_result) { |
| 279 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 207 times.
|
207 | if (obj == obj2) continue; |
| 280 | |||
| 281 |
3/4✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 206 times.
|
207 | if ((*callback)(obj, obj2)) return true; |
| 282 | } | ||
| 283 | |||
| 284 |
3/4✓ Branch 1 taken 3454 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 145 times.
✓ Branch 4 taken 3309 times.
|
3454 | if (!scene_limit.contain(obj_aabb)) { |
| 285 |
2/2✓ Branch 5 taken 86 times.
✓ Branch 6 taken 145 times.
|
231 | for (const auto& obj2 : objs_outside_scene_limit) { |
| 286 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 86 times.
|
86 | if (obj == obj2) continue; |
| 287 | |||
| 288 |
2/4✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 86 times.
|
86 | if ((*callback)(obj, obj2)) return true; |
| 289 | } | ||
| 290 | } | ||
| 291 |
2/2✓ Branch 1 taken 3454 times.
✓ Branch 2 taken 1 times.
|
3455 | } else { |
| 292 |
2/2✓ Branch 5 taken 239 times.
✓ Branch 6 taken 307 times.
|
546 | for (const auto& obj2 : objs_partially_penetrating_scene_limit) { |
| 293 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 239 times.
|
239 | if (obj == obj2) continue; |
| 294 | |||
| 295 |
2/4✓ Branch 1 taken 239 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 239 times.
|
239 | if ((*callback)(obj, obj2)) return true; |
| 296 | } | ||
| 297 | |||
| 298 |
2/2✓ Branch 5 taken 89 times.
✓ Branch 6 taken 307 times.
|
396 | for (const auto& obj2 : objs_outside_scene_limit) { |
| 299 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 89 times.
|
89 | if (obj == obj2) continue; |
| 300 | |||
| 301 |
2/4✓ Branch 1 taken 89 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 89 times.
|
89 | if ((*callback)(obj, obj2)) return true; |
| 302 | } | ||
| 303 | } | ||
| 304 | |||
| 305 | 3761 | return false; | |
| 306 | } | ||
| 307 | |||
| 308 | //============================================================================== | ||
| 309 | template <typename HashTable> | ||
| 310 | 3070 | bool SpatialHashingCollisionManager<HashTable>::distance_( | |
| 311 | CollisionObject* obj, DistanceCallBackBase* callback, | ||
| 312 | Scalar& min_dist) const { | ||
| 313 |
2/4✓ Branch 3 taken 3070 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3070 times.
✗ Branch 7 not taken.
|
3070 | auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; |
| 314 |
1/2✓ Branch 2 taken 3070 times.
✗ Branch 3 not taken.
|
3070 | auto aabb = obj->getAABB(); |
| 315 |
2/2✓ Branch 1 taken 2984 times.
✓ Branch 2 taken 86 times.
|
3070 | if (min_dist < (std::numeric_limits<Scalar>::max)()) { |
| 316 |
1/2✓ Branch 1 taken 2984 times.
✗ Branch 2 not taken.
|
2984 | Vec3s min_dist_delta(min_dist, min_dist, min_dist); |
| 317 |
1/2✓ Branch 1 taken 2984 times.
✗ Branch 2 not taken.
|
2984 | aabb.expand(min_dist_delta); |
| 318 | } | ||
| 319 | |||
| 320 |
1/2✓ Branch 1 taken 3070 times.
✗ Branch 2 not taken.
|
3070 | AABB overlap_aabb; |
| 321 | |||
| 322 | 3070 | auto status = 1; | |
| 323 | Scalar old_min_distance; | ||
| 324 | |||
| 325 | 331 | while (1) { | |
| 326 | 3401 | old_min_distance = min_dist; | |
| 327 | |||
| 328 |
3/4✓ Branch 1 taken 3401 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3396 times.
✓ Branch 4 taken 5 times.
|
3401 | if (scene_limit.overlap(aabb, overlap_aabb)) { |
| 329 |
4/8✓ Branch 1 taken 3396 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3396 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3396 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 3396 times.
|
3396 | if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb), |
| 330 | callback, min_dist)) { | ||
| 331 | ✗ | return true; | |
| 332 | } | ||
| 333 | |||
| 334 |
3/4✓ Branch 1 taken 3396 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3087 times.
✓ Branch 4 taken 309 times.
|
3396 | if (!scene_limit.contain(aabb)) { |
| 335 |
2/4✓ Branch 1 taken 3087 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3087 times.
|
3087 | if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback, |
| 336 | min_dist)) { | ||
| 337 | ✗ | return true; | |
| 338 | } | ||
| 339 | } | ||
| 340 | } else { | ||
| 341 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
|
5 | if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit, |
| 342 | callback, min_dist)) { | ||
| 343 | ✗ | return true; | |
| 344 | } | ||
| 345 | |||
| 346 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
|
5 | if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback, |
| 347 | min_dist)) { | ||
| 348 | ✗ | return true; | |
| 349 | } | ||
| 350 | } | ||
| 351 | |||
| 352 |
2/2✓ Branch 0 taken 3315 times.
✓ Branch 1 taken 86 times.
|
3401 | if (status == 1) { |
| 353 |
2/2✓ Branch 1 taken 2984 times.
✓ Branch 2 taken 331 times.
|
3315 | if (old_min_distance < (std::numeric_limits<Scalar>::max)()) { |
| 354 | 2984 | break; | |
| 355 | } else { | ||
| 356 |
2/2✓ Branch 0 taken 86 times.
✓ Branch 1 taken 245 times.
|
331 | if (min_dist < old_min_distance) { |
| 357 |
1/2✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
|
86 | Vec3s min_dist_delta(min_dist, min_dist, min_dist); |
| 358 |
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); |
| 359 | 86 | status = 0; | |
| 360 | } else { | ||
| 361 |
3/4✓ Branch 2 taken 245 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 45 times.
✓ Branch 5 taken 200 times.
|
245 | if (aabb == obj->getAABB()) |
| 362 |
2/4✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
|
45 | aabb.expand(delta); |
| 363 | else | ||
| 364 |
1/2✓ Branch 2 taken 200 times.
✗ Branch 3 not taken.
|
200 | aabb.expand(obj->getAABB(), 2.0); |
| 365 | } | ||
| 366 | } | ||
| 367 |
1/2✓ Branch 0 taken 86 times.
✗ Branch 1 not taken.
|
86 | } else if (status == 0) { |
| 368 | 86 | break; | |
| 369 | } | ||
| 370 | } | ||
| 371 | |||
| 372 | 3070 | return false; | |
| 373 | } | ||
| 374 | |||
| 375 | //============================================================================== | ||
| 376 | template <typename HashTable> | ||
| 377 | 37 | void SpatialHashingCollisionManager<HashTable>::collide( | |
| 378 | CollisionCallBackBase* callback) const { | ||
| 379 |
2/2✓ Branch 1 taken 8 times.
✓ Branch 2 taken 29 times.
|
37 | if (size() == 0) return; |
| 380 | |||
| 381 |
2/2✓ Branch 5 taken 2234 times.
✓ Branch 6 taken 26 times.
|
2260 | for (const auto& obj1 : objs) { |
| 382 | 2234 | const auto& obj_aabb = obj1->getAABB(); | |
| 383 |
1/2✓ Branch 1 taken 2234 times.
✗ Branch 2 not taken.
|
2234 | AABB overlap_aabb; |
| 384 | |||
| 385 |
3/4✓ Branch 1 taken 2234 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2224 times.
✓ Branch 4 taken 10 times.
|
2234 | if (scene_limit.overlap(obj_aabb, overlap_aabb)) { |
| 386 |
2/4✓ Branch 1 taken 2224 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2224 times.
✗ Branch 5 not taken.
|
2224 | auto query_result = hash_table->query(overlap_aabb); |
| 387 |
2/2✓ Branch 5 taken 2485 times.
✓ Branch 6 taken 2221 times.
|
4706 | for (const auto& obj2 : query_result) { |
| 388 |
2/2✓ Branch 0 taken 135 times.
✓ Branch 1 taken 2350 times.
|
2485 | if (obj1 < obj2) { |
| 389 |
3/4✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 132 times.
|
135 | if ((*callback)(obj1, obj2)) return; |
| 390 | } | ||
| 391 | } | ||
| 392 | |||
| 393 |
3/4✓ Branch 1 taken 2221 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 40 times.
✓ Branch 4 taken 2181 times.
|
2221 | if (!scene_limit.contain(obj_aabb)) { |
| 394 |
2/2✓ Branch 5 taken 33 times.
✓ Branch 6 taken 40 times.
|
73 | for (const auto& obj2 : objs_outside_scene_limit) { |
| 395 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 17 times.
|
33 | if (obj1 < obj2) { |
| 396 |
2/4✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
|
16 | if ((*callback)(obj1, obj2)) return; |
| 397 | } | ||
| 398 | } | ||
| 399 | } | ||
| 400 |
2/2✓ Branch 1 taken 2221 times.
✓ Branch 2 taken 3 times.
|
2224 | } else { |
| 401 |
2/2✓ Branch 5 taken 30 times.
✓ Branch 6 taken 10 times.
|
40 | for (const auto& obj2 : objs_partially_penetrating_scene_limit) { |
| 402 |
2/2✓ Branch 0 taken 15 times.
✓ Branch 1 taken 15 times.
|
30 | if (obj1 < obj2) { |
| 403 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
|
15 | if ((*callback)(obj1, obj2)) return; |
| 404 | } | ||
| 405 | } | ||
| 406 | |||
| 407 |
2/2✓ Branch 5 taken 23 times.
✓ Branch 6 taken 10 times.
|
33 | for (const auto& obj2 : objs_outside_scene_limit) { |
| 408 |
2/2✓ Branch 0 taken 7 times.
✓ Branch 1 taken 16 times.
|
23 | if (obj1 < obj2) { |
| 409 |
2/4✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 7 times.
|
7 | if ((*callback)(obj1, obj2)) return; |
| 410 | } | ||
| 411 | } | ||
| 412 | } | ||
| 413 | } | ||
| 414 | } | ||
| 415 | |||
| 416 | //============================================================================== | ||
| 417 | template <typename HashTable> | ||
| 418 | 6 | void SpatialHashingCollisionManager<HashTable>::distance( | |
| 419 | DistanceCallBackBase* callback) const { | ||
| 420 |
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; |
| 421 | |||
| 422 | 6 | this->enable_tested_set_ = true; | |
| 423 | 6 | this->tested_set.clear(); | |
| 424 | |||
| 425 | 6 | Scalar min_dist = (std::numeric_limits<Scalar>::max)(); | |
| 426 | |||
| 427 |
2/2✓ Branch 5 taken 2990 times.
✓ Branch 6 taken 6 times.
|
2996 | for (const auto& obj : objs) { |
| 428 |
2/4✓ Branch 1 taken 2990 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2990 times.
|
2990 | if (distance_(obj, callback, min_dist)) break; |
| 429 | } | ||
| 430 | |||
| 431 | 6 | this->enable_tested_set_ = false; | |
| 432 | 6 | this->tested_set.clear(); | |
| 433 | } | ||
| 434 | |||
| 435 | //============================================================================== | ||
| 436 | template <typename HashTable> | ||
| 437 | ✗ | void SpatialHashingCollisionManager<HashTable>::collide( | |
| 438 | BroadPhaseCollisionManager* other_manager_, | ||
| 439 | CollisionCallBackBase* callback) const { | ||
| 440 | ✗ | auto* other_manager = | |
| 441 | static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_); | ||
| 442 | |||
| 443 | ✗ | if ((size() == 0) || (other_manager->size() == 0)) return; | |
| 444 | |||
| 445 | ✗ | if (this == other_manager) { | |
| 446 | ✗ | collide(callback); | |
| 447 | ✗ | return; | |
| 448 | } | ||
| 449 | |||
| 450 | ✗ | if (this->size() < other_manager->size()) { | |
| 451 | ✗ | for (const auto& obj : objs) { | |
| 452 | ✗ | if (other_manager->collide_(obj, callback)) return; | |
| 453 | } | ||
| 454 | } else { | ||
| 455 | ✗ | for (const auto& obj : other_manager->objs) { | |
| 456 | ✗ | if (collide_(obj, callback)) return; | |
| 457 | } | ||
| 458 | } | ||
| 459 | } | ||
| 460 | |||
| 461 | //============================================================================== | ||
| 462 | template <typename HashTable> | ||
| 463 | ✗ | void SpatialHashingCollisionManager<HashTable>::distance( | |
| 464 | BroadPhaseCollisionManager* other_manager_, | ||
| 465 | DistanceCallBackBase* callback) const { | ||
| 466 | ✗ | auto* other_manager = | |
| 467 | static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_); | ||
| 468 | |||
| 469 | ✗ | if ((size() == 0) || (other_manager->size() == 0)) return; | |
| 470 | |||
| 471 | ✗ | if (this == other_manager) { | |
| 472 | ✗ | distance(callback); | |
| 473 | ✗ | return; | |
| 474 | } | ||
| 475 | |||
| 476 | ✗ | Scalar min_dist = (std::numeric_limits<Scalar>::max)(); | |
| 477 | |||
| 478 | ✗ | if (this->size() < other_manager->size()) { | |
| 479 | ✗ | for (const auto& obj : objs) | |
| 480 | ✗ | if (other_manager->distance_(obj, callback, min_dist)) return; | |
| 481 | } else { | ||
| 482 | ✗ | for (const auto& obj : other_manager->objs) | |
| 483 | ✗ | if (distance_(obj, callback, min_dist)) return; | |
| 484 | } | ||
| 485 | } | ||
| 486 | |||
| 487 | //============================================================================== | ||
| 488 | template <typename HashTable> | ||
| 489 | ✗ | bool SpatialHashingCollisionManager<HashTable>::empty() const { | |
| 490 | ✗ | return objs.empty(); | |
| 491 | } | ||
| 492 | |||
| 493 | //============================================================================== | ||
| 494 | template <typename HashTable> | ||
| 495 | 3945 | size_t SpatialHashingCollisionManager<HashTable>::size() const { | |
| 496 | 3945 | return objs.size(); | |
| 497 | } | ||
| 498 | |||
| 499 | //============================================================================== | ||
| 500 | template <typename HashTable> | ||
| 501 | 51 | void SpatialHashingCollisionManager<HashTable>::computeBound( | |
| 502 | std::vector<CollisionObject*>& objs, Vec3s& l, Vec3s& u) { | ||
| 503 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
51 | AABB bound; |
| 504 |
3/4✓ Branch 3 taken 12671 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12671 times.
✓ Branch 7 taken 51 times.
|
12722 | for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB(); |
| 505 | |||
| 506 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
51 | l = bound.min_; |
| 507 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
51 | u = bound.max_; |
| 508 | 51 | } | |
| 509 | |||
| 510 | //============================================================================== | ||
| 511 | template <typename HashTable> | ||
| 512 | template <typename Container> | ||
| 513 | 6493 | bool SpatialHashingCollisionManager<HashTable>::distanceObjectToObjects( | |
| 514 | CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback, | ||
| 515 | Scalar& min_dist) const { | ||
| 516 | 15463 | for (auto& obj2 : objs) { | |
| 517 | 8970 | if (obj == obj2) continue; | |
| 518 | |||
| 519 | 5942 | if (!this->enable_tested_set_) { | |
| 520 | 1470 | if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { | |
| 521 | 958 | if ((*callback)(obj, obj2, min_dist)) return true; | |
| 522 | } | ||
| 523 | } else { | ||
| 524 | 4472 | if (!this->inTestedSet(obj, obj2)) { | |
| 525 | 2236 | if (obj->getAABB().distance(obj2->getAABB()) < min_dist) { | |
| 526 | 242 | if ((*callback)(obj, obj2, min_dist)) return true; | |
| 527 | } | ||
| 528 | |||
| 529 | 2236 | this->insertTestedSet(obj, obj2); | |
| 530 | } | ||
| 531 | } | ||
| 532 | } | ||
| 533 | |||
| 534 | 6493 | return false; | |
| 535 | } | ||
| 536 | |||
| 537 | } // namespace coal | ||
| 538 | |||
| 539 | #endif | ||
| 540 |