C++ implementation of simple 2D physics engine (Penalty method + Euler's method for integration)