Directory: | ./ |
---|---|
File: | src/broadphase/broadphase_dynamic_AABB_tree.cpp |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 188 | 396 | 47.5% |
Branches: | 168 | 582 | 28.9% |
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 3 taken 86 times.
✗ Branch 4 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/4✓ Branch 1 taken 25342 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25342 times.
✗ Branch 5 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 |