@@ -294,6 +294,44 @@ bool leafCollide(CollisionObject* o1, CollisionObject* o2,
294
294
return false ;
295
295
}
296
296
297
+ // ==============================================================================
298
+ 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
+ if (node1->isLeaf ()) {
303
+ CollisionObject* o1 = static_cast <CollisionObject*>(node1->data );
304
+ if (o1->getNodeType () == GEOM_HALFSPACE ||
305
+ 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
+ if (node2->isLeaf ()) {
318
+ CollisionObject* o2 = static_cast <CollisionObject*>(node2->data );
319
+ if (o2->getNodeType () == GEOM_HALFSPACE ||
320
+ 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
+ return node1->bv .overlap (node2->bv );
333
+ }
334
+
297
335
// ==============================================================================
298
336
bool collisionRecurse (DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
299
337
DynamicAABBTreeCollisionManager::DynamicAABBNode* root2,
@@ -304,7 +342,9 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
304
342
return leafCollide (o1, o2, callback);
305
343
}
306
344
307
- if (!root1->bv .overlap (root2->bv )) return false ;
345
+ if (!nodeCollide (root1, root2)) {
346
+ return false ;
347
+ }
308
348
309
349
if (root2->isLeaf () ||
310
350
(!root1->isLeaf () && (root1->bv .size () > root2->bv .size ()))) {
@@ -325,7 +365,17 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root,
325
365
return leafCollide (leaf, query, callback);
326
366
}
327
367
328
- if (!root->bv .overlap (query->getAABB ())) return false ;
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
+ DynamicAABBTreeCollisionManager::DynamicAABBNode query_node;
372
+ query_node.data = query;
373
+ query_node.bv = query->getAABB ();
374
+ query_node.parent = nullptr ;
375
+ query_node.children [1 ] = nullptr ;
376
+ if (!nodeCollide (root, &query_node)) {
377
+ return false ;
378
+ }
329
379
330
380
size_t select_res =
331
381
select (query->getAABB (), *(root->children [0 ]), *(root->children [1 ]));
0 commit comments