Fixed some bugs and added test for the collision system