CollisionDetection: some clean up and improvements