Надо розобраться с методом рунге-кутта 4-порядка точности - C (СИ)
Формулировка задачи:
Подскажите что я делаю не так ? Скорей всего всё , ну тем не менее подскажите .
Мне нужен метод рунге-кутта 4-порядка , для колебательных систем , типа джойнтов ,
гармонических осцилляторов , и,т,д?
Вот код , в котором я пытался добиться простого линейного перемещения с ускорением
методом рунге-кутта 4-порядка точности !
Ну короче временная функция которую мы интегрируем
методом рунге-кутта 4-порядка должна перемещать точку с ускорением?
P.S: код частично взят с википедии, и , не много переделаный!
//============================================================================
// 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;
}Решение задачи: «Надо розобраться с методом рунге-кутта 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;
}