Box2D Improvements

Cleaned up the source files, implemented more debug macros and properly
got some joint functions working.