The ContactResultsMap results in a significant amount of heap allocations during motion planning.

[Levi-Armstrong]

This data structure should be updated to leverage Pool library to avoid this. Links to documentation of available options:

Boost Pool Library Boost Pool Library Examples

[Levi-Armstrong]

It looks like std has adopted some of these into c++17

https://www.rkaiser.de/wp-content/uploads/2021/03/embo2021-pmr-STL-for-Embedded-Applications-en.pdf

[Levi-Armstrong]

Here is PR #864 where I started leveraging std::pmr but not sure if it is being used correctly and needs further testing.

[Levi-Armstrong]

After watching a few cppcon presentation on this functionality it is more involved that I thought when using std::pmr if you are storing a custom class/struct. Each of the structs data members would need to use pmr types like string and std::array. On the final slide of one of the presentation they stated that usually an object pool or custom struct would be sufficient without the complexity of std::pmr. I agree and think a custom class can be created to store an internal map and control how things are cleared to reduce multiple allocations during collision checking.