Port to C++ from Verlet-js