[geometry] Create Drake's own dynamic bounding volume hierarchy for *geometry*

[SeanCurtis-TRI]

Problem statement

Drake currently depends on FCL for its dynamic bounding volume hierarchy to do culling of geometries in various queries. This comes with a number of costs:

1. FCL is generalized across BV types we don't care about, but we're paying that price.
2. FCL's BVH update has some strange artifacts (reversing order of geometries when the hierarchy doesn't actually change). For Drake to guarantee that the results are always in a fixed order, Drake has to do extra work to re-order the geometries.
3. FCL doesn't support queries we'd desire in the future: ray casting.
4. We'd like to do spatial queries (ray casting) on geometries with other roles. That requires non-FCL Bvh.
5. Collision filters are applied after the fact; only after the full BVH traversal is made do we determine if a pair should be culled; our own Bvh could be built in such a way to better exploit the culling.
6. Attempts to improve performance are limited to being in FCL. By implementing our own, we can use arbitrary optimizations to improve performance.

[YongcanZhou]

Hi, I have some questions. The library which drake used to collision detected is different from the FCL(https://github.com/flexible-collision-library/fcl)? Is Drake collision detection based on FCL? Or where can I find the difference?

I also have some questions about [test_fcl_sphere_box.cpp](https://github.com/flexible-collision-library/fcl/blob/master/test/test_fcl_sphere_box.cpp#:~:text=//%20TODO(SeanCurtis%2DTRI,for%20an%20example. ). Is the TODO part not finished yet?

Can I use FCL to get the contact force between sphere and box? Or can Drake achieve it?

Thanks very much!

[sherm1]

@Michael-collide please direct Drake questions to Stackoverflow with tag "drake" and fcl questions to its GitHub site.

[YongcanZhou]

I'm sorry ~Thanks for your reminder!