C++ Physics engine in OpenGL