2D Physics Engine in C++