Const and preallocation based optimizations and cleanup

[SJamieson]

These should be pretty safe to merge, but the changes do make quite a few functions const and makes some variables mutable --- let me know if you'd prefer I drop those changes and focus only on convexHullsOfCurves

[SJamieson]

I have confirmed that the default 10-agent simulation still runs fine with these changes