Fixed some bugs, added others to sweep and prune
Merge branch 'master' of https://github.com/JimGomez48/collision_sim