Box2DBody fixes and optimizations