void rk2(real* y, void (*deriv_func)(real, real*, int, real*, void*), real x, const int n, const real h, void* data) {
int i;
real k1[n], k2[n], ym[n];
real xm;
deriv_func(x,y,n,k1,data);
for(i=0;i<n;i++) {
ym[i]=y[i]+k1[i]*0.5*h;
}
xm=x+0.5*h;
deriv_func(xm,ym,n,k2,data);
for(i=0;i<n;i++) {
y[i]=y[i]+k2[i]*h;
}
}
void do_step( System system , state_type &x , time_type t , time_type dt )
{
typedef typename odeint::unwrap_reference< System >::type system_type;
typedef typename odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
typedef typename odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
system_type &sys = system;
deriv_func_type &deriv_func = sys.first;
jacobi_func_type &jacobi_func = sys.second;
m_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_impl<state_type> , detail::ref( *this ) , detail::_1 ) );
for( size_t i=0 ; i<x.size() ; ++i )
m_pm.m_v[i] = i;
t += dt;
// apply first Newton step
deriv_func( x , m_dxdt.m_v , t );
m_b.m_v = dt * m_dxdt.m_v;
jacobi_func( x , m_jacobi.m_v , t );
m_jacobi.m_v *= dt;
m_jacobi.m_v -= boost::numeric::ublas::identity_matrix< value_type >( x.size() );
solve( m_b.m_v , m_jacobi.m_v );
m_x.m_v = x - m_b.m_v;
// iterate Newton until some precision is reached
// ToDo: maybe we should apply only one Newton step -> linear implicit one-step scheme
while( boost::numeric::ublas::norm_2( m_b.m_v ) > m_epsilon )
{
deriv_func( m_x.m_v , m_dxdt.m_v , t );
m_b.m_v = x - m_x.m_v + dt*m_dxdt.m_v;
// simplified version, only the first Jacobian is used
// jacobi( m_x , m_jacobi , t );
// m_jacobi *= dt;
// m_jacobi -= boost::numeric::ublas::identity_matrix< value_type >( x.size() );
solve( m_b.m_v , m_jacobi.m_v );
m_x.m_v -= m_b.m_v;
}
x = m_x.m_v;
}
void do_step( System system , state_type& x , time_type t , time_type dt )
{
typedef typename odeint::unwrap_reference< System >::type system_type;
typedef typename odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
typedef typename odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
system_type &sys = system;
deriv_func_type &deriv_func = sys.first;
jacobi_func_type &jacobi_func = sys.second;
m_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_impl<state_type> , detail::ref( *this ) , detail::_1 ) );
state_type x1 = x;// x1 for storing x(k+1)
// std::cout << x1[0] << " " << x1[1] << "\n";
implicit_euler< double > imp_euler;
imp_euler.do_step(system, x1, t, dt);
//x(k+1) computed from above line for x(k) = 4*x(k+1) - 3*x(k+2) + 2*dt*x'(k+2)
// std::cout << x1[0] << " " << x1[1] << "\n";
for( size_t i=0 ; i<x.size() ; ++i )
m_pm.m_v[i] = i;
t +=dt;// t = t0 + 2*dt for x(k+2) and x'(k+2)
deriv_func( x1 , m_dxdt.m_v , t ); //m_dxdt.m_v is x'(k+2)
m_b.m_v = 2.0 * dt * m_dxdt.m_v;
jacobi_func( x1 , m_jacobi.m_v , t );
m_jacobi.m_v *= dt;
m_jacobi.m_v -= boost::numeric::ublas::identity_matrix< value_type >( x.size() );
solve( m_b.m_v , m_jacobi.m_v );
m_x.m_v = (4.0 *x1)- (3.0*x) + m_b.m_v;//x(k) after first Newton iteration
std::cout << "Before loop : " << m_x.m_v[0] << " " << m_x.m_v[1] << " " << m_b.m_v[0] << " " << m_b.m_v[1] << "\n";
while( boost::numeric::ublas::norm_2( m_b.m_v ) > m_epsilon )
{
deriv_func( m_x.m_v , m_dxdt.m_v , t );
m_b.m_v = -(4.0 *x1)+ (3.0*x) + m_x.m_v + 2.0 * dt * m_dxdt.m_v;
solve( m_b.m_v , m_jacobi.m_v );
m_x.m_v -= m_b.m_v;
std::cout << "Inside loop : " << m_x.m_v[0] << " " << m_x.m_v[1] << " " << m_b.m_v[0] << " " << m_b.m_v[1] << "\n";
}
x = m_x.m_v;
}
void rk4(real* y, void (*deriv_func)(real, real*, int, real*, void*), real t, const int n, const real h, void* data) {
int i;
real k1[n], k2[n], k3[n], k4[n], ym[n];
real tm, k5;
deriv_func(t,y,n,k1,data);
for(i=0;i<n;i++) {
ym[i]=y[i]+k1[i]*0.5*h;
}
tm=t+0.5*h;
deriv_func(tm,ym,n,k2,data);
for(i=0;i<n;i++) {
ym[i]=y[i]+k2[i]*0.5*h;
}
deriv_func(tm,ym,n,k3,data);
for(i=0;i<n;i++) {
ym[i]=y[i]+k3[i]*h;
}
tm=t+h;
deriv_func(tm,ym,n,k4,data);
for(i=0;i<n;i++) {
k5=(k1[i]+2.0*(k2[i]+k3[i])+k4[i])/6.0;
y[i]=y[i]+k5*h;
}
}
static void saveOut (double t, double *y) {
int j;
REAL(YOUT)[(it)*(ntot+1)] = t;
for (j = 0; j < n_eq; j++)
REAL(YOUT)[(it)*(ntot + 1) + j + 1] = y[j];
/* if ordinary output variables: call function again */
if (nout>0) {
if (isDll == 1) /* output function in DLL */
deriv_func (&n_eq, &t, y, xdytmp, out, ipar) ;
else
C_deriv_out_gb(&nout, &t, y, xdytmp, out);
for (j = 0; j < nout; j++)
REAL(YOUT)[(it)*(ntot + 1) + j + n_eq + 1] = out[j];
}
}
void rk8(real* y, void (*deriv_func)(real, real*, int, real*, void*), real t, const int n, const real h, void* data) {
int i,j,k;
int nrk=11;
real *ym = (real* )Alloc1DArray( sizeof(real), n );
real **kn = (real**)Alloc2DArray( sizeof(real), nrk*2, n );
memcpy( ym, y, sizeof(real)*n );
for(i=0;i<nrk;i++) {
deriv_func( t+h*c[i], ym, n, kn[i], data );
for(j=0;j<n;j++) {
ym[j] = y[j];
for(k=0;k<i+1;k++) {
ym[j] += a[i][k]*h*kn[k][j];
}
}
}
memcpy( y, ym, sizeof(real)*n );
Free1DArray( (void*)ym );
Free2DArray( (void**)kn, nrk);
}
void do_step( System system , state_type &x , time_type t , time_type dt )
{
typedef typename omplext_odeint::unwrap_reference< System >::type system_type;
typedef typename omplext_odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
typedef typename omplext_odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
system_type &sys = system;
deriv_func_type &deriv_func = sys.first;
jacobi_func_type &jacobi_func = sys.second;
m_resizer.adjust_size( x , detail::bind(
&stepper_type::template resize_impl< state_type > , detail::ref( *this ) , detail::_1 ) );
m_identity.m_v = 1;
t += dt;
m_x.m_v = x;
deriv_func( x , m_dxdt.m_v , t );
jacobi_func( x , m_jacobi.m_v , t );
m_dxdt.m_v *= -dt;
m_jacobi.m_v *= dt;
m_jacobi.m_v -= m_identity.m_v ;
// using ilu_0 preconditioning -incomplete LU factorisation
// itl::pc::diagonal<matrix_type,double> L(m_jacobi.m_v);
itl::pc::ilu_0<matrix_type> L( m_jacobi.m_v );
solve( m_jacobi.m_v , m_x.m_v , m_dxdt.m_v , L );
x+= m_x.m_v;
}
void do_step( System system , const state_type &x , time_type t , state_type &xout , time_type dt , state_type &xerr )
{
// get the systen and jacobi function
typedef typename odeint::unwrap_reference< System >::type system_type;
typedef typename odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
typedef typename odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
system_type &sys = system;
deriv_func_type &deriv_func = sys.first;
jacobi_func_type &jacobi_func = sys.second;
const size_t n = x.size();
m_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_impl<state_type> , detail::ref( *this ) , detail::_1 ) );
for( size_t i=0 ; i<n ; ++i )
m_pm.m_v( i ) = i;
deriv_func( x , m_dxdt.m_v , t );
jacobi_func( x , m_jac.m_v , t , m_dfdt.m_v );
m_jac.m_v *= -1.0;
m_jac.m_v += 1.0 / m_coef.gamma / dt * boost::numeric::ublas::identity_matrix< value_type >( n );
boost::numeric::ublas::lu_factorize( m_jac.m_v , m_pm.m_v );
for( size_t i=0 ; i<n ; ++i )
m_g1.m_v[i] = m_dxdt.m_v[i] + dt * m_coef.d1 * m_dfdt.m_v[i];
boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g1.m_v );
for( size_t i=0 ; i<n ; ++i )
m_xtmp.m_v[i] = x[i] + m_coef.a21 * m_g1.m_v[i];
deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + m_coef.c2 * dt );
for( size_t i=0 ; i<n ; ++i )
m_g2.m_v[i] = m_dxdtnew.m_v[i] + dt * m_coef.d2 * m_dfdt.m_v[i] + m_coef.c21 * m_g1.m_v[i] / dt;
boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g2.m_v );
for( size_t i=0 ; i<n ; ++i )
m_xtmp.m_v[i] = x[i] + m_coef.a31 * m_g1.m_v[i] + m_coef.a32 * m_g2.m_v[i];
deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + m_coef.c3 * dt );
for( size_t i=0 ; i<n ; ++i )
m_g3.m_v[i] = m_dxdtnew.m_v[i] + dt * m_coef.d3 * m_dfdt.m_v[i] + ( m_coef.c31 * m_g1.m_v[i] + m_coef.c32 * m_g2.m_v[i] ) / dt;
boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g3.m_v );
for( size_t i=0 ; i<n ; ++i )
m_xtmp.m_v[i] = x[i] + m_coef.a41 * m_g1.m_v[i] + m_coef.a42 * m_g2.m_v[i] + m_coef.a43 * m_g3.m_v[i];
deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + m_coef.c4 * dt );
for( size_t i=0 ; i<n ; ++i )
m_g4.m_v[i] = m_dxdtnew.m_v[i] + dt * m_coef.d4 * m_dfdt.m_v[i] + ( m_coef.c41 * m_g1.m_v[i] + m_coef.c42 * m_g2.m_v[i] + m_coef.c43 * m_g3.m_v[i] ) / dt;
boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g4.m_v );
for( size_t i=0 ; i<n ; ++i )
m_xtmp.m_v[i] = x[i] + m_coef.a51 * m_g1.m_v[i] + m_coef.a52 * m_g2.m_v[i] + m_coef.a53 * m_g3.m_v[i] + m_coef.a54 * m_g4.m_v[i];
deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + dt );
for( size_t i=0 ; i<n ; ++i )
m_g5.m_v[i] = m_dxdtnew.m_v[i] + ( m_coef.c51 * m_g1.m_v[i] + m_coef.c52 * m_g2.m_v[i] + m_coef.c53 * m_g3.m_v[i] + m_coef.c54 * m_g4.m_v[i] ) / dt;
boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g5.m_v );
for( size_t i=0 ; i<n ; ++i )
m_xtmp.m_v[i] += m_g5.m_v[i];
deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + dt );
for( size_t i=0 ; i<n ; ++i )
xerr[i] = m_dxdtnew.m_v[i] + ( m_coef.c61 * m_g1.m_v[i] + m_coef.c62 * m_g2.m_v[i] + m_coef.c63 * m_g3.m_v[i] + m_coef.c64 * m_g4.m_v[i] + m_coef.c65 * m_g5.m_v[i] ) / dt;
boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , xerr );
for( size_t i=0 ; i<n ; ++i )
xout[i] = m_xtmp.m_v[i] + xerr[i];
}
