Major Box2D Improvements

Implemented proper debugging macros and added advanced functionality.