Collision Checking with Binary Search

[juelg]

collision_callback e.g. in sim/FR3.cpp should implemented using a binary search as the collisions are sorted. This would give us a runtime improvement from $\mathcal{O}(n)$ to $\mathcal{O}(\log(n))$.

[pkrack]

Say we have a list of $n$ collisions, and we want to check if any of $m$ geoms are part of a collision.

Using a binary search approach we get a runtime of:
$$\mathcal{O}(\log(n) \cdot m)$$
One binary search for each geom.

Using the current scanning approach we get a runtime of:
$$\mathcal{O}(n)$$
One scan through the entire list of collisions, check if the current collision contains a geom that is part of the set of $m$ geoms ($\mathcal{O}(1)$ check using a has table).

To me it is not obvious which approach is better. For large $n$ and small $m$ the binary search approach should be better. For smaller $n$ and large $m$ the current approach should be better. It depends on the scene.

[juelg]

Right, however, the current implementation is $\mathcal{O}(n\log(m))$ as plain C++ sets are used instead of unordered_set which have a Red Black Tree as implementation and thus a logarithmic look up time [1].

Currently we only search for collisions where 8 geoms are involved ($m=8$). Yes this could increase when we add more robots, but it is still kind of a constant compared to the $n$ which could involve thousands of geoms if we e.g. have other objects interacting with each other. Furthermore, by sorting our collision geoms by id (needs to be done only once in the beginning) we can (1) check in constant time if the geom ids that we are interested in are in the collision array number space and (2) implement a more efficient binary search which uses modified bounds from the search before and thus iteratively lowering $n$ quite significantly.

Consider the following examples:

Example 1:
M = [12, 13, 14]
N=[(2, 3), (4, 5)]

-> we can check in constant time that no collision occurred since 12 is greater than the biggest element in N.

Example 2:
M = [12, 13, 14]
N = [(2, 3), (4, 5), (12, 19), (13, 20), (14, 21), (20, 30), (21, 31)]

-> For the first collision geom 12 we will find collision (12, 19) with binary search. Thus, for the next search we can use
N' = [(13, 20), (14, 21), (20, 30), (21, 31)]
The same strategy works from above with 14, thus I would suggest alternating between first and last element to shorten the search space as much as possible for the remaining elements.

[1] https://en.cppreference.com/w/cpp/container/set