Надо розобраться с методом рунге-кутта 4-порядка точности - C (СИ)

Формулировка задачи:

Подскажите что я делаю не так ? Скорей всего всё , ну тем не менее подскажите . Мне нужен метод рунге-кутта 4-порядка , для колебательных систем , типа джойнтов , гармонических осцилляторов , и,т,д? Вот код , в котором я пытался добиться простого линейного перемещения с ускорением методом рунге-кутта 4-порядка точности ! Ну короче временная функция которую мы интегрируем методом рунге-кутта 4-порядка должна перемещать точку с ускорением?

//============================================================================
// Name        : RK4.cpp
// Author      : werasaimon
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================
 
#include <math.h>
#include <stdio.h>

using namespace std;
 
struct State
{
  State( float _pos = 0 , float _vel = 0 )
  : pos(_pos) , vel(_vel){}
 
  float pos;
  float vel;
};

//float accel(float x, float v, float dt)
//{
//    float stiffness = 1;
//    float damping = -0.005;
//    return -stiffness*x - damping*v;
//}

float acceleration = 0.2;
 
float accel( float pos , float vel , float _dt )
{
  return /*acceleration*/  vel * _dt;/*pos + vel * _dt*/;
}

typedef float (* vFunctionCall)(float x, float v, float dt);
 
State  RK4(float x, float v, float dt , vFunctionCall accel)
{
    float x1 = x;
    float v1 = v;
    float a1 = accel(x1, v1, 0);
 
    float x2 = x + 0.5*v1*dt;
    float v2 = v + 0.5*a1*dt;
    float a2 = accel(x2, v2, dt/2.0);
 
    float x3 = x + 0.5*v2*dt;
    float v3 = v + 0.5*a2*dt;
    float a3 = accel(x3, v3, dt/2.0);
 
    float x4 = x + v3*dt;
    float v4 = v + a3*dt;
    float a4 = accel(x4, v4, dt);
 
    float xf = x + (dt/6.0)*(v1 + 2*v2 + 2*v3 + v4);
    float vf = v + (dt/6.0)*(a1 + 2*a2 + 2*a3 + a4);
 
    return State( xf, vf );
}

int main()
{

  float t = 0;
  float dt = 1.0/60;// # Timestep of 1/60 second
  State state(10, 5);// # Position, velocity
  State euler(10, 5);// # For comparison with Euler integration

  printf( "Initial position: %f, velocity: %f \n" , state.pos , state.vel );
 
  while( t < 10 )
  {
    t += dt;
    state = RK4( state.pos, state.vel, dt , accel);
    euler = State( euler.pos + euler.vel*dt , euler.vel + accel(euler.pos,euler.vel,dt)*dt );
 
    printf( "Final RK4   position: %f, velocity: %f \n" , state.pos , state.vel );
    printf( "Final Euler position: %f, velocity: %f \n\n\n" , euler.pos , euler.vel );
  }
 
  return 0;
}

P.S: код частично взят с википедии, и , не много переделаный!

Решение задачи: «Надо розобраться с методом рунге-кутта 4-порядка точности»

textual
                            Листинг программы 
                        

                            TPhysicsGridSimulate( const Vector3& _gravity )
    : mGravity(_gravity)
    {
 
    }
 
 
    void Solver( int const& Step )
    {
 
        const float TensionConstant = 600.f;
        const float TensionDampingConstant = 2.0f;
 
 
        // Add gravity, store velocity
        for( unsigned int i = 0; i < mParticles.size(); ++i )
        {
            if( mParticles[i]->mType == DYNAMIC ) mParticles[i]->mForces[Step] = mGravity;
            if( mParticles[i]->mType == DYNAMIC ) mParticles[i]->mVels[Step]   = mParticles[i]->mSimVelocity;
        }
 
 
        /**/
        // Then further in Forces [Step] it is necessary to throw all the forces acting on particles.
        // Below, for example, the forces of springs are added.
 
        // Add spring forces
        for( unsigned int i = 0; i < mJoints.size(); ++i )
        {
            TParticle *PParticle1 = mJoints[i]->Particle1;
            TParticle *PParticle2 = mJoints[i]->Particle2;
 
            Vector3 PosDelta = PParticle1->mSimPosition - PParticle2->mSimPosition;
            Vector3 VelDelta = PParticle1->mSimVelocity - PParticle2->mSimVelocity;
 
            float  Dist = PosDelta.length();
 
            if( fabs(Dist) > EPSILON )
            {
                float F = -(TensionConstant * (Dist - mJoints[i]->mIdealDist) + TensionDampingConstant * ((PosDelta.dot(VelDelta)) / Dist));
                Vector3 Force = PosDelta / Dist * F;
 
                //mJoints[i]->mAccumulated[Step] += Force;
 
                if( PParticle1->mType == DYNAMIC ) PParticle1->mForces[Step] = PParticle1->mForces[Step] + Force * PParticle1->mMassInverse;
                if( PParticle2->mType == DYNAMIC ) PParticle2->mForces[Step] = PParticle2->mForces[Step] - Force * PParticle2->mMassInverse;
 
                float relax = 1.0 - mJoints[i]->mRelaxation;
 
                PParticle1->mVels[Step] =  PParticle1->mVels[Step] * relax;
                PParticle2->mVels[Step] =  PParticle2->mVels[Step] * relax;
 
            }
        }
        /**/
 
 
    }
 
 
    void UpdateRungeKutta4(float TimeStep)
    {
        int iter_rk4 = 0;
        for( unsigned int i = 0; i < mParticles.size(); ++i )
        {
            TParticle *Part = mParticles[i];
            Part->mSimPosition = Part->mPosition;
            Part->mSimVelocity = Part->mVelocity;
        }
 
        Solver(iter_rk4);
        iter_rk4++;
 
        for( unsigned int i = 0; i < mParticles.size(); ++i )
        {
            TParticle *Part = mParticles[i];
            Part->mSimPosition = Part->mPosition + (Part->mVels[0]   * TimeStep/2);
            Part->mSimVelocity = Part->mVelocity + (Part->mForces[0] * TimeStep/2);
        }
 
        Solver(iter_rk4);
        iter_rk4++;
 
        for( unsigned int i = 0; i < mParticles.size(); ++i )
        {
            TParticle *Part = mParticles[i];
            Part->mSimPosition = Part->mPosition + (Part->mVels[1] * TimeStep/2);
            Part->mSimVelocity = Part->mVelocity + (Part->mForces[1] * TimeStep/2);
        }
 
        Solver(iter_rk4);
        iter_rk4++;
 
        for( unsigned int i = 0; i < mParticles.size(); ++i )
        {
            TParticle *Part = mParticles[i];
            Part->mSimPosition = Part->mPosition + (Part->mVels[2] * TimeStep);
            Part->mSimVelocity = Part->mVelocity + (Part->mForces[2] * TimeStep);
        }
 
        Solver(iter_rk4);
 
 
        for( unsigned int i = 0; i < mParticles.size(); ++i )
        {
            TParticle *Part = mParticles[i];
            if(Part->mType == DYNAMIC)
            {
                Part->mPosition = RungeKuttaSum(Part->mPosition, Part->mVels[0], Part->mVels[1], Part->mVels[2], Part->mVels[3], TimeStep);
                Part->mVelocity = RungeKuttaSum(Part->mVelocity, Part->mForces[0], Part->mForces[1], Part->mForces[2], Part->mForces[3], TimeStep);
            }
 
        }
 
    }
 
    static Vector3 RungeKuttaSum(const Vector3& Pos, const Vector3& K1,const Vector3&  K2, const Vector3&  K3, const Vector3&  K4 , float const& TimeStep)
    {
        return  Pos + (K1 + K2 * 2.0 + K3 * 2.0 + K4) * TimeStep / 6.f;
    }
                        