Improve collision cache heap allocation

[Levi-Armstrong]

This further improves memory usage and performance over pr #309.

• Nearly a 50% reduction in planning time
• Reduce memory usage by 15MB.

Before:

---------------------------------------------------------------------------------------------
Benchmark                                                   Time             CPU   Iterations
---------------------------------------------------------------------------------------------
BM_TRAJOPT_SIMPLE_COLLISION_SOLVE/real_time               355 us          355 us         1966
BM_TRAJOPT_PLANNING_SOLVE/min_time:6.000/real_time      39786 ms        39785 ms            1

After:

-----------------------------------------------------------------------------------------------
Benchmark                                                     Time             CPU   Iterations
-----------------------------------------------------------------------------------------------
BM_TRAJOPT_SIMPLE_COLLISION_SOLVE/real_time                 338 us          338 us         2086
BM_TRAJOPT_PLANNING_SOLVE/min_time:100.000/real_time      23936 ms        23935 ms            6

[Levi-Armstrong]