balancing subtree at node destroys "sortedness" of DynamicAABBTree

[trsoluti]

Currently the balanceSubTreeAtNode() method in DynamicAABBTree determines which node to transfer from one branch to the other purely by the height of the node.

Where the nodes are of equal height it will take the right node. This can lead to "unsorted" AABB trees where the AABB of a node includes the area of its sibling.

To reproduce, execute the following code on a dynamic AABB tree tree:

    DynamicAABBTree tree(MemoryManager::getBaseAllocator());

    int object1Data = 34;
    AABB aabb1 = AABB(Vector3(7.5, 7.5, 7.5), Vector3(8.5, 8.5, 8.5));
    int object1Id = tree.addObject(aabb1, &object1Data);

    int object2Data = 21;
    AABB aabb2 = AABB(Vector3(-6, -6, -6), Vector3(-5, -5, -5));
    int object2Id = tree.addObject(aabb2, &object2Data);

    int object3Data = 31;
    AABB aabb3 = AABB(Vector3(5, 5, 5), Vector3(6, 6, 6));
    int object3Id = tree.addObject(aabb3, &object3Data);

    int object4Data = 22;
    AABB aabb4 = AABB(Vector3(-5.5, -5.5, -5.5), Vector3(-4.5, -4.5, -4.5));
    int object4Id = tree.addObject(aabb4, &object4Data);

    int object5Data = 32;
    AABB aabb5 = AABB(Vector3(5.5, 5.5, 5.5), Vector3(6.5, 6.5, 6.5));
    int object5Id = tree.addObject(aabb5, &object5Data);

    int object6Data = 33;
    AABB aabb6 = AABB(Vector3(7, 7, 7), Vector3(8, 8, 8));
    int object6Id = tree.addObject(aabb6, &object6Data);

    // Tree will now have one branch with height 2 (objects (32, 31), (33, 34))
    // and one with height 1 (objects 21 and 22)

    tree.removeObject(object5Id);

    // Now tree will have two branches: (21, (33, 34)) and (31, 32)
    // -- but 31 and 32 are inside the volume of 21-34!

To resolve, I suggest that the code at the appropriate lines in DynamicAABBTree.cpp (currently 466 and 536) also compute the volume of combining nodeF and nodeG respectively, and, if the heights are the same, chose the branch with the smaller total volume.

[DanielChappuis]

Thanks a lot for taking the time to report this. I will take a look at it when I have some time.

[DanielChappuis]

Hello. I took some time to look at your example. However I am not really sure I understand it very well.

 // Tree will now have one branch with height 2 (objects (32, 31), (33, 34))
 // and one with height 1 (objects 21 and 22)

Here, I agree with you. I observe the same result.

tree.removeObject(object5Id);

// Now tree will have two branches: (21, (33, 34)) and (31, 32)
// -- but 31 and 32 are inside the volume of 21-34!

But here, I don't observe this at all. I think there is something wrong with your example. Here you remove object with ID 'object5Id' from the tree wich contains data '32'. But if you remove data '32' from the tree, how could you have it in your result tree after the removal when you say 'Now tree will have two branches: (21, (33, 34)) and (31, 32)'?

Moreover, in your example, when you call:

tree.removeObject(object5Id);

no rotation is done during the tree balancing because in the balanceSubTreeAtNode() method, the balance factor is never < -1 or > 1. Therefore, I think there is something wrong with your example.

The result tree that I observe after the removal is a tree with two branches ((34, 33), 31) and (21, 22) which seems correct if we have removed data 32 from the tree.

Is you example correct ?

[trsoluti]

My apologies. The example should be removing object4Id (22), not object5Id. Then you will get the symptom.

[trsoluti]

On further review, balancing also has a problem if the two children of the pivot node have different heights. Consider the following test:

            // Now we are going to create a more unbalanced tree.
            object1Data = 21;
            object2Data = 41;
            object3Data = 31;
            object4Data = 22;
            object5Data = 42;
            aabb1 = AABB(Vector3(10, 10, 10), Vector3(11, 11, 11));
            aabb2 = AABB(Vector3(15, 15, 15), Vector3(16, 16, 16));
            aabb3 = AABB(Vector3(18, 18, 18), Vector3(19, 19, 19));
            aabb4 = AABB(Vector3(11, 11, 11), Vector3(12, 12, 12));
            aabb5 = AABB(Vector3(16, 16, 16), Vector3(17, 17, 17));
            object1Id = tree.addObject(aabb1, &object1Data);
            object2Id = tree.addObject(aabb2, &object2Data);
            object3Id = tree.addObject(aabb3, &object3Data);
            object4Id = tree.addObject(aabb4, &object4Data);
            object5Id = tree.addObject(aabb5, &object5Data);

            // The tree is now ((21,22),(31,(41,42))

            tree.removeObject(object4Id);

            // The tree is now (((21,31),(41,42))
            // but (41,42) is *inside* (21,31)!

To fix this problem you'll have to pivot the child node of the pivot node as well, when the closer node is also the deeper node.

Here's an algorithm that will balance the tree properly:

1) Determine if rebalancing not needed — return immediately.

2) Assign the following node roles:

**Unbalanced Node**: the node we determined is out of balance
**Keep Node**: the child of the Unbalanced Node with the lesser height
**Pivot Node**: the child of the Unbalanced Node with the greater height
**Pivot Keep Node**: the child of the Pivot Node furthest from the Keep Node (i.e. highest combined AABB volume).
**Move Node**: the child of the Pivot Node closest to the Keep Node (lowest combined AABB volume),
    unless the Optional Sub-pivot Node is present, in which case it is the child of the Optional Sub-pivot Node closest to the Keep Node. 
**Optional Sub-pivot Node**: The child of the Pivot Node that is closest to the Keep Node,
    only if that node has a greater height than that of the Pivot Keep Node
**Optional Sub-pivot Keep Node**: if the Optional Sub-pivot Node is present, this is the child of the Sub-pivot Node furthest from the Keep Node

3) Re-organise the tree so that the following is true:

a) The parent of the Pivot Node is the former parent of the Unbalanced Node (or the tree Root Node is updated if the Unbalanced Node had no parent).
b) One child of the Pivot Node is the Unbalanced Node.
c) The other child of the Pivot Node is the Optional Sub-pivot Node, if present, otherwise it is the Pivot Keep Node
d) One child of the Unbalanced Node is the Keep Node.
e) The other child of the Unbalanced Node is the Move Node.
f) If the Optional Sub-pivot Node is present, one of its children is the Optional Sub-pivot Keep Node
g) If the Optional Sub-pivot Node is present, the other of its children is the Pivot Keep Node
h) All parent relationships are reciprocal to the child relationships
i) AABB’s and heights of all branch nodes are correct.

[DanielChappuis]

Thanks for the correction. Now I have the same result that you have when you say:

tree.removeObject(object4Id);

// Now tree will have two branches: (21, (33, 34)) and (31, 32)

I don't see anything wrong with this result but I don't really understand when you say:

    // -- but 31 and 32 are inside the volume of 21-34!

What do you mean exactly when you say this? For instance 31 has an AABB with (min=Vector3(5, 5, 5) and max=Vector3(6, 6, 6)) but this is not inside 21 for instance because 21 in your example has an AABB with (min=Vector3(-6, -6, -6) and max=Vector3(-5, -5, -5)).

[trsoluti]

I don't see anything wrong with this result but I don't really understand when you say:

// -- but 31 and 32 are inside the volume of 21-34!

What do you mean exactly when you say this?

What I meant was that the tree branch that has the two children (21) and (33, 34) will have a fat AABB of (min=(-6, -6, -6) and max=(8.5, 8.5, 8.5)). Its sibling branch, the one with the two children (31) and (32) will have a fat AABB of (min=(5, 5, 5,) and max=(6.5, 6.5, 6.5)), which is "inside" its sibling.

Hopefully this makes the issue a little clearer.

[DanielChappuis]

In this case, after the rotation (tree balancing), the AABB of one branch is inside the AABB of its sibling but this is not really an error, the tree is still valid. This should not cause any undesired behavior during the simulation (except a small waste of time during broad-phase). A Bounding Volume Hierarchy Tree is not 'sorted' (it's not a red black tree). The tree is valid as long as the AABB of each child is inside the AABB of its parent.

Of course it this case, the tree is not optimal but we do not need an optimal tree, we simply need a tree that is good enough in order to keep AABB collision test across the tree fast. It is always possible to make the algorithm more complex to find a better tree but we should do this only if we can show that the more complex algorithm gives a tree that is much faster and compensates the time lost in a more complex algorithm.

I basically use this technique for the BVH tree: https://box2d.org/files/ErinCatto_DynamicBVH_GDC2019.pdf

[trsoluti]

Thanks for the explanation, Daniel! I understand now that "close enough is good enough" for the tree. I'm going to prepare a few timing tests, and if my proposed changes don't provide significant time benefits then I'll be happy to close the Issue.

[trsoluti]

Just to let you know, my preliminary (not very optimised) implementation yields a 16.7% speed increase in adding objects, a 14.3% increase in updating objects, and a whopping 68.1% increase in determining collisions. It definitely looks work the complexity.

I'll update the code in my fork and send a pull request. I can't guarantee it's the most efficient implementation because I don't know the tricks of CPP.