Improve debug draw and get box2d going propery