Move all vectors to box2D ones, hack box2d a bit to have more functions return this for nicer code.
Tiny simplification
Move the agents using box2d physics.