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