DoublePendulum C++ Simulation