| 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_dynamic_AABB_tree_array.h" | ||
| 39 | |||
| 40 | #ifdef COAL_HAVE_OCTOMAP | ||
| 41 | #include "coal/octree.h" | ||
| 42 | #endif | ||
| 43 | namespace coal { | ||
| 44 | namespace detail { | ||
| 45 | namespace dynamic_AABB_tree_array { | ||
| 46 | |||
| 47 | #if COAL_HAVE_OCTOMAP | ||
| 48 | |||
| 49 | //============================================================================== | ||
| 50 | ✗ | bool collisionRecurse_( | |
| 51 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, | ||
| 52 | size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, | ||
| 53 | const AABB& root2_bv, const Transform3s& tf2, | ||
| 54 | CollisionCallBackBase* callback) { | ||
| 55 | ✗ | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = | |
| 56 | ✗ | nodes1 + root1_id; | |
| 57 | ✗ | if (!root2) { | |
| 58 | ✗ | if (root1->isLeaf()) { | |
| 59 | ✗ | CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data); | |
| 60 | |||
| 61 | ✗ | if (!obj1->collisionGeometry()->isFree()) { | |
| 62 | ✗ | OBB obb1, obb2; | |
| 63 | ✗ | convertBV(root1->bv, Transform3s::Identity(), obb1); | |
| 64 | ✗ | convertBV(root2_bv, tf2, obb2); | |
| 65 | |||
| 66 | ✗ | if (obb1.overlap(obb2)) { | |
| 67 | ✗ | Box* box = new Box(); | |
| 68 | ✗ | Transform3s box_tf; | |
| 69 | ✗ | constructBox(root2_bv, tf2, *box, box_tf); | |
| 70 | |||
| 71 | ✗ | box->cost_density = tree2->getDefaultOccupancy(); | |
| 72 | |||
| 73 | ✗ | CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf); | |
| 74 | ✗ | return (*callback)(obj1, &obj2); | |
| 75 | ✗ | } | |
| 76 | } | ||
| 77 | } else { | ||
| 78 | ✗ | if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr, | |
| 79 | root2_bv, tf2, callback)) | ||
| 80 | ✗ | return true; | |
| 81 | ✗ | if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr, | |
| 82 | root2_bv, tf2, callback)) | ||
| 83 | ✗ | return true; | |
| 84 | } | ||
| 85 | |||
| 86 | ✗ | return false; | |
| 87 | ✗ | } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { | |
| 88 | ✗ | CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data); | |
| 89 | ✗ | if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) { | |
| 90 | ✗ | OBB obb1, obb2; | |
| 91 | ✗ | convertBV(root1->bv, Transform3s::Identity(), obb1); | |
| 92 | ✗ | convertBV(root2_bv, tf2, obb2); | |
| 93 | |||
| 94 | ✗ | if (obb1.overlap(obb2)) { | |
| 95 | ✗ | Box* box = new Box(); | |
| 96 | ✗ | Transform3s box_tf; | |
| 97 | ✗ | constructBox(root2_bv, tf2, *box, box_tf); | |
| 98 | |||
| 99 | ✗ | box->cost_density = Scalar(root2->getOccupancy()); | |
| 100 | ✗ | box->threshold_occupied = tree2->getOccupancyThres(); | |
| 101 | |||
| 102 | ✗ | CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf); | |
| 103 | ✗ | return (*callback)(obj1, &obj2); | |
| 104 | ✗ | } else | |
| 105 | ✗ | return false; | |
| 106 | } else | ||
| 107 | ✗ | return false; | |
| 108 | } | ||
| 109 | |||
| 110 | ✗ | OBB obb1, obb2; | |
| 111 | ✗ | convertBV(root1->bv, Transform3s::Identity(), obb1); | |
| 112 | ✗ | convertBV(root2_bv, tf2, obb2); | |
| 113 | |||
| 114 | ✗ | if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; | |
| 115 | |||
| 116 | ✗ | if (!tree2->nodeHasChildren(root2) || | |
| 117 | ✗ | (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { | |
| 118 | ✗ | if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, | |
| 119 | tf2, callback)) | ||
| 120 | ✗ | return true; | |
| 121 | ✗ | if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, | |
| 122 | tf2, callback)) | ||
| 123 | ✗ | return true; | |
| 124 | } else { | ||
| 125 | ✗ | for (unsigned int i = 0; i < 8; ++i) { | |
| 126 | ✗ | if (tree2->nodeChildExists(root2, i)) { | |
| 127 | ✗ | const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); | |
| 128 | ✗ | AABB child_bv; | |
| 129 | ✗ | computeChildBV(root2_bv, i, child_bv); | |
| 130 | |||
| 131 | ✗ | if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, | |
| 132 | callback)) | ||
| 133 | ✗ | return true; | |
| 134 | } else { | ||
| 135 | ✗ | AABB child_bv; | |
| 136 | ✗ | computeChildBV(root2_bv, i, child_bv); | |
| 137 | ✗ | if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, tf2, | |
| 138 | callback)) | ||
| 139 | ✗ | return true; | |
| 140 | } | ||
| 141 | } | ||
| 142 | } | ||
| 143 | |||
| 144 | ✗ | return false; | |
| 145 | } | ||
| 146 | |||
| 147 | //============================================================================== | ||
| 148 | ✗ | bool distanceRecurse_( | |
| 149 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, | ||
| 150 | size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, | ||
| 151 | const AABB& root2_bv, const Transform3s& tf2, | ||
| 152 | DistanceCallBackBase* callback, Scalar& min_dist) { | ||
| 153 | ✗ | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = | |
| 154 | ✗ | nodes1 + root1_id; | |
| 155 | ✗ | if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { | |
| 156 | ✗ | if (tree2->isNodeOccupied(root2)) { | |
| 157 | ✗ | Box* box = new Box(); | |
| 158 | ✗ | Transform3s box_tf; | |
| 159 | ✗ | constructBox(root2_bv, tf2, *box, box_tf); | |
| 160 | ✗ | CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf); | |
| 161 | ✗ | return (*callback)(static_cast<CollisionObject*>(root1->data), &obj, | |
| 162 | ✗ | min_dist); | |
| 163 | ✗ | } else | |
| 164 | ✗ | return false; | |
| 165 | } | ||
| 166 | |||
| 167 | ✗ | if (!tree2->isNodeOccupied(root2)) return false; | |
| 168 | |||
| 169 | ✗ | if (!tree2->nodeHasChildren(root2) || | |
| 170 | ✗ | (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { | |
| 171 | ✗ | AABB aabb2; | |
| 172 | ✗ | convertBV(root2_bv, tf2, aabb2); | |
| 173 | |||
| 174 | ✗ | Scalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv); | |
| 175 | ✗ | Scalar d2 = aabb2.distance((nodes1 + root1->children[1])->bv); | |
| 176 | |||
| 177 | ✗ | if (d2 < d1) { | |
| 178 | ✗ | if (d2 < min_dist) { | |
| 179 | ✗ | if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, | |
| 180 | tf2, callback, min_dist)) | ||
| 181 | ✗ | return true; | |
| 182 | } | ||
| 183 | |||
| 184 | ✗ | if (d1 < min_dist) { | |
| 185 | ✗ | if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, | |
| 186 | tf2, callback, min_dist)) | ||
| 187 | ✗ | return true; | |
| 188 | } | ||
| 189 | } else { | ||
| 190 | ✗ | if (d1 < min_dist) { | |
| 191 | ✗ | if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, | |
| 192 | tf2, callback, min_dist)) | ||
| 193 | ✗ | return true; | |
| 194 | } | ||
| 195 | |||
| 196 | ✗ | if (d2 < min_dist) { | |
| 197 | ✗ | if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, | |
| 198 | tf2, callback, min_dist)) | ||
| 199 | ✗ | return true; | |
| 200 | } | ||
| 201 | } | ||
| 202 | } else { | ||
| 203 | ✗ | for (unsigned int i = 0; i < 8; ++i) { | |
| 204 | ✗ | if (tree2->nodeChildExists(root2, i)) { | |
| 205 | ✗ | const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); | |
| 206 | ✗ | AABB child_bv; | |
| 207 | ✗ | computeChildBV(root2_bv, i, child_bv); | |
| 208 | |||
| 209 | ✗ | AABB aabb2; | |
| 210 | ✗ | convertBV(child_bv, tf2, aabb2); | |
| 211 | ✗ | Scalar d = root1->bv.distance(aabb2); | |
| 212 | |||
| 213 | ✗ | if (d < min_dist) { | |
| 214 | ✗ | if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, | |
| 215 | callback, min_dist)) | ||
| 216 | ✗ | return true; | |
| 217 | } | ||
| 218 | } | ||
| 219 | } | ||
| 220 | } | ||
| 221 | |||
| 222 | ✗ | return false; | |
| 223 | } | ||
| 224 | |||
| 225 | #endif | ||
| 226 | |||
| 227 | //============================================================================== | ||
| 228 | 6437 | bool collisionRecurse( | |
| 229 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, | ||
| 230 | size_t root1_id, | ||
| 231 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2, | ||
| 232 | size_t root2_id, CollisionCallBackBase* callback) { | ||
| 233 | 6437 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = | |
| 234 | 6437 | nodes1 + root1_id; | |
| 235 | 6437 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 = | |
| 236 | 6437 | nodes2 + root2_id; | |
| 237 |
6/6✓ Branch 1 taken 2369 times.
✓ Branch 2 taken 4068 times.
✓ Branch 4 taken 1676 times.
✓ Branch 5 taken 693 times.
✓ Branch 6 taken 1676 times.
✓ Branch 7 taken 4761 times.
|
6437 | if (root1->isLeaf() && root2->isLeaf()) { |
| 238 |
2/2✓ Branch 1 taken 1662 times.
✓ Branch 2 taken 14 times.
|
1676 | if (!root1->bv.overlap(root2->bv)) return false; |
| 239 | 28 | return (*callback)(static_cast<CollisionObject*>(root1->data), | |
| 240 | 14 | static_cast<CollisionObject*>(root2->data)); | |
| 241 | } | ||
| 242 | |||
| 243 |
2/2✓ Branch 1 taken 3537 times.
✓ Branch 2 taken 1224 times.
|
4761 | if (!root1->bv.overlap(root2->bv)) return false; |
| 244 | |||
| 245 |
4/4✓ Branch 1 taken 1190 times.
✓ Branch 2 taken 34 times.
✓ Branch 3 taken 595 times.
✓ Branch 4 taken 629 times.
|
2414 | if (root2->isLeaf() || |
| 246 |
4/4✓ Branch 1 taken 1167 times.
✓ Branch 2 taken 23 times.
✓ Branch 5 taken 561 times.
✓ Branch 6 taken 606 times.
|
1190 | (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { |
| 247 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 593 times.
|
595 | if (collisionRecurse(nodes1, root1->children[0], nodes2, root2_id, |
| 248 | callback)) | ||
| 249 | 2 | return true; | |
| 250 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 592 times.
|
593 | if (collisionRecurse(nodes1, root1->children[1], nodes2, root2_id, |
| 251 | callback)) | ||
| 252 | 1 | return true; | |
| 253 | } else { | ||
| 254 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 628 times.
|
629 | if (collisionRecurse(nodes1, root1_id, nodes2, root2->children[0], |
| 255 | callback)) | ||
| 256 | 1 | return true; | |
| 257 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 628 times.
|
628 | if (collisionRecurse(nodes1, root1_id, nodes2, root2->children[1], |
| 258 | callback)) | ||
| 259 | ✗ | return true; | |
| 260 | } | ||
| 261 | 1220 | return false; | |
| 262 | } | ||
| 263 | |||
| 264 | //============================================================================== | ||
| 265 | 70616 | inline COAL_DLLAPI bool collisionRecurse( | |
| 266 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, | ||
| 267 | size_t root_id, CollisionObject* query, CollisionCallBackBase* callback) { | ||
| 268 | 70616 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; | |
| 269 |
2/2✓ Branch 2 taken 39024 times.
✓ Branch 3 taken 31592 times.
|
70616 | if (!root->bv.overlap(query->getAABB())) return false; |
| 270 | |||
| 271 |
2/2✓ Branch 1 taken 38 times.
✓ Branch 2 taken 31554 times.
|
31592 | if (root->isLeaf()) { |
| 272 | 38 | return (*callback)(static_cast<CollisionObject*>(root->data), query); | |
| 273 | } | ||
| 274 | |||
| 275 | 31554 | size_t select_res = implementation_array::select( | |
| 276 | 31554 | query->getAABB(), root->children[0], root->children[1], nodes); | |
| 277 | |||
| 278 |
2/2✓ Branch 1 taken 16 times.
✓ Branch 2 taken 31538 times.
|
31554 | if (collisionRecurse(nodes, root->children[select_res], query, callback)) |
| 279 | 16 | return true; | |
| 280 | |||
| 281 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 31537 times.
|
31538 | if (collisionRecurse(nodes, root->children[1 - select_res], query, callback)) |
| 282 | 1 | return true; | |
| 283 | |||
| 284 | 31537 | return false; | |
| 285 | } | ||
| 286 | |||
| 287 | //============================================================================== | ||
| 288 | 8098 | bool selfCollisionRecurse( | |
| 289 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, | ||
| 290 | size_t root_id, CollisionCallBackBase* callback) { | ||
| 291 | 8098 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; | |
| 292 |
2/2✓ Branch 1 taken 4070 times.
✓ Branch 2 taken 4028 times.
|
8098 | if (root->isLeaf()) return false; |
| 293 | |||
| 294 |
2/2✓ Branch 1 taken 16 times.
✓ Branch 2 taken 4012 times.
|
4028 | if (selfCollisionRecurse(nodes, root->children[0], callback)) return true; |
| 295 | |||
| 296 |
2/2✓ Branch 1 taken 20 times.
✓ Branch 2 taken 3992 times.
|
4012 | if (selfCollisionRecurse(nodes, root->children[1], callback)) return true; |
| 297 | |||
| 298 |
2/2✓ Branch 1 taken 6 times.
✓ Branch 2 taken 3986 times.
|
3992 | if (collisionRecurse(nodes, root->children[0], nodes, root->children[1], |
| 299 | callback)) | ||
| 300 | 6 | return true; | |
| 301 | |||
| 302 | 3986 | return false; | |
| 303 | } | ||
| 304 | |||
| 305 | //============================================================================== | ||
| 306 | 18576 | bool distanceRecurse( | |
| 307 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, | ||
| 308 | size_t root1_id, | ||
| 309 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2, | ||
| 310 | size_t root2_id, DistanceCallBackBase* callback, Scalar& min_dist) { | ||
| 311 | 18576 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 = | |
| 312 | 18576 | nodes1 + root1_id; | |
| 313 | 18576 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 = | |
| 314 | 18576 | nodes2 + root2_id; | |
| 315 |
6/6✓ Branch 1 taken 4017 times.
✓ Branch 2 taken 14559 times.
✓ Branch 4 taken 2673 times.
✓ Branch 5 taken 1344 times.
✓ Branch 6 taken 2673 times.
✓ Branch 7 taken 15903 times.
|
18576 | if (root1->isLeaf() && root2->isLeaf()) { |
| 316 | 2673 | CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data); | |
| 317 | 2673 | CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data); | |
| 318 | 2673 | return (*callback)(root1_obj, root2_obj, min_dist); | |
| 319 | } | ||
| 320 | |||
| 321 |
4/4✓ Branch 1 taken 13584 times.
✓ Branch 2 taken 2319 times.
✓ Branch 3 taken 7790 times.
✓ Branch 4 taken 8113 times.
|
29487 | if (root2->isLeaf() || |
| 322 |
4/4✓ Branch 1 taken 12240 times.
✓ Branch 2 taken 1344 times.
✓ Branch 5 taken 5471 times.
✓ Branch 6 taken 6769 times.
|
13584 | (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { |
| 323 | 7790 | Scalar d1 = root2->bv.distance((nodes1 + root1->children[0])->bv); | |
| 324 | 7790 | Scalar d2 = root2->bv.distance((nodes1 + root1->children[1])->bv); | |
| 325 | |||
| 326 |
2/2✓ Branch 0 taken 5112 times.
✓ Branch 1 taken 2678 times.
|
7790 | if (d2 < d1) { |
| 327 |
2/2✓ Branch 0 taken 3034 times.
✓ Branch 1 taken 2078 times.
|
5112 | if (d2 < min_dist) { |
| 328 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3034 times.
|
3034 | if (distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, |
| 329 | callback, min_dist)) | ||
| 330 | ✗ | return true; | |
| 331 | } | ||
| 332 | |||
| 333 |
2/2✓ Branch 0 taken 724 times.
✓ Branch 1 taken 4388 times.
|
5112 | if (d1 < min_dist) { |
| 334 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 724 times.
|
724 | if (distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, |
| 335 | callback, min_dist)) | ||
| 336 | ✗ | return true; | |
| 337 | } | ||
| 338 | } else { | ||
| 339 |
2/2✓ Branch 0 taken 2066 times.
✓ Branch 1 taken 612 times.
|
2678 | if (d1 < min_dist) { |
| 340 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2066 times.
|
2066 | if (distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, |
| 341 | callback, min_dist)) | ||
| 342 | ✗ | return true; | |
| 343 | } | ||
| 344 | |||
| 345 |
2/2✓ Branch 0 taken 199 times.
✓ Branch 1 taken 2479 times.
|
2678 | if (d2 < min_dist) { |
| 346 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 199 times.
|
199 | if (distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, |
| 347 | callback, min_dist)) | ||
| 348 | ✗ | return true; | |
| 349 | } | ||
| 350 | } | ||
| 351 | } else { | ||
| 352 | 8113 | Scalar d1 = root1->bv.distance((nodes2 + root2->children[0])->bv); | |
| 353 | 8113 | Scalar d2 = root1->bv.distance((nodes2 + root2->children[1])->bv); | |
| 354 | |||
| 355 |
2/2✓ Branch 0 taken 3056 times.
✓ Branch 1 taken 5057 times.
|
8113 | if (d2 < d1) { |
| 356 |
2/2✓ Branch 0 taken 2585 times.
✓ Branch 1 taken 471 times.
|
3056 | if (d2 < min_dist) { |
| 357 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2585 times.
|
2585 | if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], |
| 358 | callback, min_dist)) | ||
| 359 | ✗ | return true; | |
| 360 | } | ||
| 361 | |||
| 362 |
2/2✓ Branch 0 taken 134 times.
✓ Branch 1 taken 2922 times.
|
3056 | if (d1 < min_dist) { |
| 363 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 134 times.
|
134 | if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], |
| 364 | callback, min_dist)) | ||
| 365 | ✗ | return true; | |
| 366 | } | ||
| 367 | } else { | ||
| 368 |
2/2✓ Branch 0 taken 3016 times.
✓ Branch 1 taken 2041 times.
|
5057 | if (d1 < min_dist) { |
| 369 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3016 times.
|
3016 | if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], |
| 370 | callback, min_dist)) | ||
| 371 | ✗ | return true; | |
| 372 | } | ||
| 373 | |||
| 374 |
2/2✓ Branch 0 taken 850 times.
✓ Branch 1 taken 4207 times.
|
5057 | if (d2 < min_dist) { |
| 375 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 850 times.
|
850 | if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], |
| 376 | callback, min_dist)) | ||
| 377 | ✗ | return true; | |
| 378 | } | ||
| 379 | } | ||
| 380 | } | ||
| 381 | |||
| 382 | 15903 | return false; | |
| 383 | } | ||
| 384 | |||
| 385 | //============================================================================== | ||
| 386 | 27019 | bool distanceRecurse( | |
| 387 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, | ||
| 388 | size_t root_id, CollisionObject* query, DistanceCallBackBase* callback, | ||
| 389 | Scalar& min_dist) { | ||
| 390 | 27019 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; | |
| 391 |
2/2✓ Branch 1 taken 9225 times.
✓ Branch 2 taken 17794 times.
|
27019 | if (root->isLeaf()) { |
| 392 | 9225 | CollisionObject* root_obj = static_cast<CollisionObject*>(root->data); | |
| 393 | 9225 | return (*callback)(root_obj, query, min_dist); | |
| 394 | } | ||
| 395 | |||
| 396 | 17794 | Scalar d1 = query->getAABB().distance((nodes + root->children[0])->bv); | |
| 397 | 17794 | Scalar d2 = query->getAABB().distance((nodes + root->children[1])->bv); | |
| 398 | |||
| 399 |
2/2✓ Branch 0 taken 8905 times.
✓ Branch 1 taken 8889 times.
|
17794 | if (d2 < d1) { |
| 400 |
2/2✓ Branch 0 taken 8601 times.
✓ Branch 1 taken 304 times.
|
8905 | if (d2 < min_dist) { |
| 401 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 8601 times.
|
8601 | if (distanceRecurse(nodes, root->children[1], query, callback, min_dist)) |
| 402 | ✗ | return true; | |
| 403 | } | ||
| 404 | |||
| 405 |
2/2✓ Branch 0 taken 4591 times.
✓ Branch 1 taken 4314 times.
|
8905 | if (d1 < min_dist) { |
| 406 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 4591 times.
|
4591 | if (distanceRecurse(nodes, root->children[0], query, callback, min_dist)) |
| 407 | ✗ | return true; | |
| 408 | } | ||
| 409 | } else { | ||
| 410 |
2/2✓ Branch 0 taken 8611 times.
✓ Branch 1 taken 278 times.
|
8889 | if (d1 < min_dist) { |
| 411 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 8611 times.
|
8611 | if (distanceRecurse(nodes, root->children[0], query, callback, min_dist)) |
| 412 | ✗ | return true; | |
| 413 | } | ||
| 414 | |||
| 415 |
2/2✓ Branch 0 taken 5056 times.
✓ Branch 1 taken 3833 times.
|
8889 | if (d2 < min_dist) { |
| 416 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 5056 times.
|
5056 | if (distanceRecurse(nodes, root->children[1], query, callback, min_dist)) |
| 417 | ✗ | return true; | |
| 418 | } | ||
| 419 | } | ||
| 420 | |||
| 421 | 17794 | return false; | |
| 422 | } | ||
| 423 | |||
| 424 | //============================================================================== | ||
| 425 | 11948 | bool selfDistanceRecurse( | |
| 426 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes, | ||
| 427 | size_t root_id, DistanceCallBackBase* callback, Scalar& min_dist) { | ||
| 428 | 11948 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id; | |
| 429 |
2/2✓ Branch 1 taken 5980 times.
✓ Branch 2 taken 5968 times.
|
11948 | if (root->isLeaf()) return false; |
| 430 | |||
| 431 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 5968 times.
|
5968 | if (selfDistanceRecurse(nodes, root->children[0], callback, min_dist)) |
| 432 | ✗ | return true; | |
| 433 | |||
| 434 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 5968 times.
|
5968 | if (selfDistanceRecurse(nodes, root->children[1], callback, min_dist)) |
| 435 | ✗ | return true; | |
| 436 | |||
| 437 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 5968 times.
|
5968 | if (distanceRecurse(nodes, root->children[0], nodes, root->children[1], |
| 438 | callback, min_dist)) | ||
| 439 | ✗ | return true; | |
| 440 | |||
| 441 | 5968 | return false; | |
| 442 | } | ||
| 443 | |||
| 444 | #if COAL_HAVE_OCTOMAP | ||
| 445 | |||
| 446 | //============================================================================== | ||
| 447 | ✗ | bool collisionRecurse( | |
| 448 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, | ||
| 449 | size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, | ||
| 450 | const AABB& root2_bv, const Transform3s& tf2, | ||
| 451 | CollisionCallBackBase* callback) { | ||
| 452 | ✗ | if (tf2.rotation().isIdentity()) | |
| 453 | ✗ | return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, | |
| 454 | ✗ | tf2.translation(), callback); | |
| 455 | else | ||
| 456 | ✗ | return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, | |
| 457 | ✗ | callback); | |
| 458 | } | ||
| 459 | |||
| 460 | //============================================================================== | ||
| 461 | ✗ | bool distanceRecurse( | |
| 462 | DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1, | ||
| 463 | size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, | ||
| 464 | const AABB& root2_bv, const Transform3s& tf2, | ||
| 465 | DistanceCallBackBase* callback, Scalar& min_dist) { | ||
| 466 | ✗ | if (tf2.rotation().isIdentity()) | |
| 467 | ✗ | return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, | |
| 468 | ✗ | tf2.translation(), callback, min_dist); | |
| 469 | else | ||
| 470 | ✗ | return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, | |
| 471 | ✗ | callback, min_dist); | |
| 472 | } | ||
| 473 | |||
| 474 | #endif | ||
| 475 | |||
| 476 | } // namespace dynamic_AABB_tree_array | ||
| 477 | |||
| 478 | } // namespace detail | ||
| 479 | |||
| 480 | //============================================================================== | ||
| 481 |
1/2✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
|
102 | DynamicAABBTreeArrayCollisionManager::DynamicAABBTreeArrayCollisionManager() { |
| 482 | 102 | tree_topdown_balance_threshold = &dtree.bu_threshold; | |
| 483 | 102 | tree_topdown_level = &dtree.topdown_level; | |
| 484 | 102 | max_tree_nonbalanced_level = 10; | |
| 485 | 102 | tree_incremental_balance_pass = 10; | |
| 486 | 102 | *tree_topdown_balance_threshold = 2; | |
| 487 | 102 | *tree_topdown_level = 0; | |
| 488 | 102 | tree_init_level = 0; | |
| 489 | 102 | setup_ = false; | |
| 490 | |||
| 491 | // from experiment, this is the optimal setting | ||
| 492 | 102 | octree_as_geometry_collide = true; | |
| 493 | 102 | octree_as_geometry_distance = false; | |
| 494 | 102 | } | |
| 495 | |||
| 496 | //============================================================================== | ||
| 497 | 102 | void DynamicAABBTreeArrayCollisionManager::registerObjects( | |
| 498 | const std::vector<CollisionObject*>& other_objs) { | ||
| 499 |
2/2✓ Branch 1 taken 16 times.
✓ Branch 2 taken 86 times.
|
102 | if (other_objs.empty()) return; |
| 500 | |||
| 501 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 86 times.
|
86 | if (size() > 0) { |
| 502 | ✗ | BroadPhaseCollisionManager::registerObjects(other_objs); | |
| 503 | } else { | ||
| 504 |
4/8✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 25342 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 25342 times.
✓ Branch 9 taken 86 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
25428 | DynamicAABBNode* leaves = new DynamicAABBNode[other_objs.size()]; |
| 505 | 86 | table.rehash(other_objs.size()); | |
| 506 |
2/2✓ Branch 1 taken 25342 times.
✓ Branch 2 taken 86 times.
|
25428 | for (size_t i = 0, size = other_objs.size(); i < size; ++i) { |
| 507 | 25342 | leaves[i].bv = other_objs[i]->getAABB(); | |
| 508 | 25342 | leaves[i].parent = dtree.NULL_NODE; | |
| 509 | 25342 | leaves[i].children[1] = dtree.NULL_NODE; | |
| 510 | 25342 | leaves[i].data = other_objs[i]; | |
| 511 | 25342 | table[other_objs[i]] = i; | |
| 512 | } | ||
| 513 | |||
| 514 | 86 | int n_leaves = (int)other_objs.size(); | |
| 515 | |||
| 516 | 86 | dtree.init(leaves, n_leaves, tree_init_level); | |
| 517 | |||
| 518 | 86 | setup_ = true; | |
| 519 | } | ||
| 520 | } | ||
| 521 | |||
| 522 | //============================================================================== | ||
| 523 | ✗ | void DynamicAABBTreeArrayCollisionManager::registerObject( | |
| 524 | CollisionObject* obj) { | ||
| 525 | ✗ | size_t node = dtree.insert(obj->getAABB(), obj); | |
| 526 | ✗ | table[obj] = node; | |
| 527 | ✗ | } | |
| 528 | |||
| 529 | //============================================================================== | ||
| 530 | ✗ | void DynamicAABBTreeArrayCollisionManager::unregisterObject( | |
| 531 | CollisionObject* obj) { | ||
| 532 | ✗ | size_t node = table[obj]; | |
| 533 | ✗ | table.erase(obj); | |
| 534 | ✗ | dtree.remove(node); | |
| 535 | ✗ | } | |
| 536 | |||
| 537 | //============================================================================== | ||
| 538 | 128 | void DynamicAABBTreeArrayCollisionManager::setup() { | |
| 539 |
2/2✓ Branch 0 taken 42 times.
✓ Branch 1 taken 86 times.
|
128 | if (!setup_) { |
| 540 | 42 | int num = (int)dtree.size(); | |
| 541 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 26 times.
|
42 | if (num == 0) { |
| 542 | 16 | setup_ = true; | |
| 543 | 16 | return; | |
| 544 | } | ||
| 545 | |||
| 546 | 26 | int height = (int)dtree.getMaxHeight(); | |
| 547 | |||
| 548 | 26 | if ((Scalar)height - std::log((Scalar)num) / std::log(2.0) < | |
| 549 |
1/2✓ Branch 0 taken 26 times.
✗ Branch 1 not taken.
|
26 | max_tree_nonbalanced_level) |
| 550 | 26 | dtree.balanceIncremental(tree_incremental_balance_pass); | |
| 551 | else | ||
| 552 | ✗ | dtree.balanceTopdown(); | |
| 553 | |||
| 554 | 26 | setup_ = true; | |
| 555 | } | ||
| 556 | } | ||
| 557 | |||
| 558 | //============================================================================== | ||
| 559 | 26 | void DynamicAABBTreeArrayCollisionManager::update() { | |
| 560 |
2/2✓ Branch 4 taken 2688 times.
✓ Branch 5 taken 26 times.
|
2714 | for (auto it = table.cbegin(), end = table.cend(); it != end; ++it) { |
| 561 | 2688 | const CollisionObject* obj = it->first; | |
| 562 | 2688 | size_t node = it->second; | |
| 563 |
1/2✓ Branch 3 taken 2688 times.
✗ Branch 4 not taken.
|
2688 | dtree.getNodes()[node].bv = obj->getAABB(); |
| 564 | } | ||
| 565 | |||
| 566 | 26 | dtree.refit(); | |
| 567 | 26 | setup_ = false; | |
| 568 | |||
| 569 | 26 | setup(); | |
| 570 | 26 | } | |
| 571 | |||
| 572 | //============================================================================== | ||
| 573 | ✗ | void DynamicAABBTreeArrayCollisionManager::update_( | |
| 574 | CollisionObject* updated_obj) { | ||
| 575 | ✗ | const auto it = table.find(updated_obj); | |
| 576 | ✗ | if (it != table.end()) { | |
| 577 | ✗ | size_t node = it->second; | |
| 578 | ✗ | if (!(dtree.getNodes()[node].bv == updated_obj->getAABB())) | |
| 579 | ✗ | dtree.update(node, updated_obj->getAABB()); | |
| 580 | } | ||
| 581 | ✗ | setup_ = false; | |
| 582 | ✗ | } | |
| 583 | |||
| 584 | //============================================================================== | ||
| 585 | ✗ | void DynamicAABBTreeArrayCollisionManager::update( | |
| 586 | CollisionObject* updated_obj) { | ||
| 587 | ✗ | update_(updated_obj); | |
| 588 | ✗ | setup(); | |
| 589 | ✗ | } | |
| 590 | |||
| 591 | //============================================================================== | ||
| 592 | ✗ | void DynamicAABBTreeArrayCollisionManager::update( | |
| 593 | const std::vector<CollisionObject*>& updated_objs) { | ||
| 594 | ✗ | for (size_t i = 0, size = updated_objs.size(); i < size; ++i) | |
| 595 | ✗ | update_(updated_objs[i]); | |
| 596 | ✗ | setup(); | |
| 597 | ✗ | } | |
| 598 | |||
| 599 | //============================================================================== | ||
| 600 | ✗ | void DynamicAABBTreeArrayCollisionManager::clear() { | |
| 601 | ✗ | dtree.clear(); | |
| 602 | ✗ | table.clear(); | |
| 603 | ✗ | } | |
| 604 | |||
| 605 | //============================================================================== | ||
| 606 | ✗ | void DynamicAABBTreeArrayCollisionManager::getObjects( | |
| 607 | std::vector<CollisionObject*>& objs) const { | ||
| 608 | ✗ | objs.resize(this->size()); | |
| 609 | ✗ | std::transform( | |
| 610 | table.begin(), table.end(), objs.begin(), | ||
| 611 | ✗ | std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); | |
| 612 | ✗ | } | |
| 613 | |||
| 614 | //============================================================================== | ||
| 615 | 7644 | void DynamicAABBTreeArrayCollisionManager::collide( | |
| 616 | CollisionObject* obj, CollisionCallBackBase* callback) const { | ||
| 617 | 7644 | callback->init(); | |
| 618 |
2/2✓ Branch 1 taken 120 times.
✓ Branch 2 taken 7524 times.
|
7644 | if (size() == 0) return; |
| 619 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 7524 times.
|
7524 | switch (obj->collisionGeometry()->getNodeType()) { |
| 620 | #if COAL_HAVE_OCTOMAP | ||
| 621 | ✗ | case GEOM_OCTREE: { | |
| 622 | ✗ | if (!octree_as_geometry_collide) { | |
| 623 | const OcTree* octree = | ||
| 624 | ✗ | static_cast<const OcTree*>(obj->collisionGeometryPtr()); | |
| 625 | ✗ | detail::dynamic_AABB_tree_array::collisionRecurse( | |
| 626 | ✗ | dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), | |
| 627 | ✗ | octree->getRootBV(), obj->getTransform(), callback); | |
| 628 | } else | ||
| 629 | ✗ | detail::dynamic_AABB_tree_array::collisionRecurse( | |
| 630 | dtree.getNodes(), dtree.getRoot(), obj, callback); | ||
| 631 | ✗ | } break; | |
| 632 | #endif | ||
| 633 | 7524 | default: | |
| 634 | 7524 | detail::dynamic_AABB_tree_array::collisionRecurse( | |
| 635 | dtree.getNodes(), dtree.getRoot(), obj, callback); | ||
| 636 | } | ||
| 637 | } | ||
| 638 | |||
| 639 | //============================================================================== | ||
| 640 | 160 | void DynamicAABBTreeArrayCollisionManager::distance( | |
| 641 | CollisionObject* obj, DistanceCallBackBase* callback) const { | ||
| 642 |
1/2✓ Branch 1 taken 160 times.
✗ Branch 2 not taken.
|
160 | callback->init(); |
| 643 |
2/4✓ Branch 1 taken 160 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 160 times.
|
160 | if (size() == 0) return; |
| 644 | 160 | Scalar min_dist = (std::numeric_limits<Scalar>::max)(); | |
| 645 |
2/4✓ Branch 3 taken 160 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 160 times.
|
160 | switch (obj->collisionGeometry()->getNodeType()) { |
| 646 | #if COAL_HAVE_OCTOMAP | ||
| 647 | ✗ | case GEOM_OCTREE: { | |
| 648 | ✗ | if (!octree_as_geometry_distance) { | |
| 649 | const OcTree* octree = | ||
| 650 | ✗ | static_cast<const OcTree*>(obj->collisionGeometryPtr()); | |
| 651 | ✗ | detail::dynamic_AABB_tree_array::distanceRecurse( | |
| 652 | ✗ | dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), | |
| 653 | ✗ | octree->getRootBV(), obj->getTransform(), callback, min_dist); | |
| 654 | } else | ||
| 655 | ✗ | detail::dynamic_AABB_tree_array::distanceRecurse( | |
| 656 | dtree.getNodes(), dtree.getRoot(), obj, callback, min_dist); | ||
| 657 | ✗ | } break; | |
| 658 | #endif | ||
| 659 | 160 | default: | |
| 660 |
1/2✓ Branch 3 taken 160 times.
✗ Branch 4 not taken.
|
160 | detail::dynamic_AABB_tree_array::distanceRecurse( |
| 661 | dtree.getNodes(), dtree.getRoot(), obj, callback, min_dist); | ||
| 662 | } | ||
| 663 | } | ||
| 664 | |||
| 665 | //============================================================================== | ||
| 666 | 74 | void DynamicAABBTreeArrayCollisionManager::collide( | |
| 667 | CollisionCallBackBase* callback) const { | ||
| 668 | 74 | callback->init(); | |
| 669 |
2/2✓ Branch 1 taken 16 times.
✓ Branch 2 taken 58 times.
|
74 | if (size() == 0) return; |
| 670 | 58 | detail::dynamic_AABB_tree_array::selfCollisionRecurse( | |
| 671 | dtree.getNodes(), dtree.getRoot(), callback); | ||
| 672 | } | ||
| 673 | |||
| 674 | //============================================================================== | ||
| 675 | 12 | void DynamicAABBTreeArrayCollisionManager::distance( | |
| 676 | DistanceCallBackBase* callback) const { | ||
| 677 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | callback->init(); |
| 678 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 12 times.
|
12 | if (size() == 0) return; |
| 679 | 12 | Scalar min_dist = (std::numeric_limits<Scalar>::max)(); | |
| 680 |
1/2✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
|
12 | detail::dynamic_AABB_tree_array::selfDistanceRecurse( |
| 681 | dtree.getNodes(), dtree.getRoot(), callback, min_dist); | ||
| 682 | } | ||
| 683 | |||
| 684 | //============================================================================== | ||
| 685 | ✗ | void DynamicAABBTreeArrayCollisionManager::collide( | |
| 686 | BroadPhaseCollisionManager* other_manager_, | ||
| 687 | CollisionCallBackBase* callback) const { | ||
| 688 | ✗ | callback->init(); | |
| 689 | ✗ | DynamicAABBTreeArrayCollisionManager* other_manager = | |
| 690 | static_cast<DynamicAABBTreeArrayCollisionManager*>(other_manager_); | ||
| 691 | ✗ | if ((size() == 0) || (other_manager->size() == 0)) return; | |
| 692 | ✗ | detail::dynamic_AABB_tree_array::collisionRecurse( | |
| 693 | dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), | ||
| 694 | other_manager->dtree.getRoot(), callback); | ||
| 695 | } | ||
| 696 | |||
| 697 | //============================================================================== | ||
| 698 | ✗ | void DynamicAABBTreeArrayCollisionManager::distance( | |
| 699 | BroadPhaseCollisionManager* other_manager_, | ||
| 700 | DistanceCallBackBase* callback) const { | ||
| 701 | ✗ | callback->init(); | |
| 702 | ✗ | DynamicAABBTreeArrayCollisionManager* other_manager = | |
| 703 | static_cast<DynamicAABBTreeArrayCollisionManager*>(other_manager_); | ||
| 704 | ✗ | if ((size() == 0) || (other_manager->size() == 0)) return; | |
| 705 | ✗ | Scalar min_dist = (std::numeric_limits<Scalar>::max)(); | |
| 706 | ✗ | detail::dynamic_AABB_tree_array::distanceRecurse( | |
| 707 | dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), | ||
| 708 | other_manager->dtree.getRoot(), callback, min_dist); | ||
| 709 | } | ||
| 710 | |||
| 711 | //============================================================================== | ||
| 712 | ✗ | bool DynamicAABBTreeArrayCollisionManager::empty() const { | |
| 713 | ✗ | return dtree.empty(); | |
| 714 | } | ||
| 715 | |||
| 716 | //============================================================================== | ||
| 717 | 7976 | size_t DynamicAABBTreeArrayCollisionManager::size() const { | |
| 718 | 7976 | return dtree.size(); | |
| 719 | } | ||
| 720 | |||
| 721 | //============================================================================== | ||
| 722 | const detail::implementation_array::HierarchyTree<AABB>& | ||
| 723 | ✗ | DynamicAABBTreeArrayCollisionManager::getTree() const { | |
| 724 | ✗ | return dtree; | |
| 725 | } | ||
| 726 | |||
| 727 | } // namespace coal | ||
| 728 |