Рунге-Кутта Пример кода адвекции частиц 4-го порядка

Я пытался внедрить интеграцию RK4 в симуляцию, которую я делаю. Приведенная ниже функция — моя лучшая попытка использования RK4 для интегрирования по силовому полю в 3 измерениях на основе уравнений на стр. 12 в этот сайт.

В моем коде класс Particle по существу хранит списки скорости и положения и может вычислять силу для данной позиции (сила не зависит от скорости). Кроме того, я знаю, что моя функция длинная и вытянутая, и ее можно уменьшить с помощью цикла for, но я (в настоящее время) хочу соответствовать структуре, использованной в статье, которую я связал.

Когда я пытаюсь смоделировать Частицу, используя этот метод, ошибка значительно хуже, чем когда я использую метод интегрирования в чехарду. Поэтому я думаю, что что-то не так с моей реализацией RK4. Пожалуйста, дайте мне знать, если я неправильно понимаю, как работает RK4, когда использую его для решения связанных дифференциальных уравнений.

// 4th Order Runge-Kutta
void Update(Particle * p, double dt) {

double * v   = p->getVel();
double * pos = p->getPos();

double initPos[3] = {pos[0], pos[1], pos[2]};
double initVel[3] = {v[0], v[1], v[2]};
double mass = 0.01;

double k[4][3]; // related to dv
double l[4][3]; // related to dr

p->findForce();

k[0][0] = dt*p->force[0]/mass;
k[0][1] = dt*p->force[1]/mass;
k[0][2] = dt*p->force[2]/mass;

l[0][0] = dt*v[0];
l[0][1] = dt*v[1];
l[0][2] = dt*v[2];

// Set position to midpoint, using l[0]
pos[0] = initPos[0] + l[0][0]/2;
pos[1] = initPos[1] + l[0][1]/2;
pos[2] = initPos[2] + l[0][2]/2;

p->findForce();

k[1][0] = dt*p->force[0]/mass;
k[1][1] = dt*p->force[1]/mass;
k[1][2] = dt*p->force[2]/mass;

l[1][0] = dt*(v[0]+k[0][0]/2);
l[1][1] = dt*(v[1]+k[0][1]/2);
l[1][2] = dt*(v[2]+k[0][2]/2);

// Set position to midpoint, using l[1]
pos[0] = initPos[0] + l[1][0]/2;
pos[1] = initPos[1] + l[1][1]/2;
pos[2] = initPos[2] + l[1][2]/2;

p->findForce();

k[2][0] = dt*p->force[0]/mass;
k[2][1] = dt*p->force[1]/mass;
k[2][2] = dt*p->force[2]/mass;

l[2][0] = dt*(v[0]+k[1][0]/2);
l[2][1] = dt*(v[1]+k[1][1]/2);
l[2][2] = dt*(v[2]+k[1][2]/2);

// Set position to endpoint, using l[2]
pos[0] = initPos[0] + l[2][0];
pos[1] = initPos[1] + l[2][1];
pos[2] = initPos[2] + l[2][2];

p->findForce();

k[3][0] = dt*p->force[0]/mass;
k[3][1] = dt*p->force[1]/mass;
k[3][2] = dt*p->force[2]/mass;

l[3][0] = dt*(v[0]+k[2][0]);
l[3][1] = dt*(v[1]+k[2][1]);
l[3][2] = dt*(v[2]+k[2][2]);

// Finalize pos and v
pos[0] = initPos[0] + (l[0][0] + 2*l[1][0] + 2*l[2][0] + l[3][0])/6;
pos[1] = initPos[1] + (l[0][1] + 2*l[1][1] + 2*l[2][1] + l[3][1])/6;
pos[2] = initPos[2] + (l[0][2] + 2*l[1][2] + 2*l[2][2] + l[3][2])/6;

v[0]   = initVel[0] + (k[0][0] + 2*k[1][0] + 2*k[2][0] + k[3][0])/6;
v[1]   = initVel[1] + (k[0][1] + 2*k[1][1] + 2*k[2][1] + k[3][1])/6;
v[2]   = initVel[2] + (k[0][2] + 2*k[1][2] + 2*k[2][2] + k[3][2])/6;
}