C++ 2d rigid body experimentation