Updated C++ version

This is the C++ version of the integrator, updated to use vectors.

UPDATE: Bug fixes. Funny how non-explicit single-argument constructors can hide errors in your code...

// RK++NET.cpp : main project file.

#include "stdafx.h"
#include <windows.h>

#include <map>
#include <vector>
#include <iostream>

const double gravity = -9.81;


template<typename T>
inline
T spring(const T& pos)
{
return -100.0 * pos;
}


template<typename T>
inline
T drag(double k, const T& pos)
{
return -k * pos;
}


template<typename A>
class EulerImplicit
{
public:
EulerImplicit(const A& accel):
accel(accel)
{
}

template<typename T>
inline
void operator()(T& pos, T& speed, const double delta) const
{
speed += delta * accel(pos, speed);
pos += delta * speed;
}

private:
const A& accel;
};


template<typename A>
class RK4
{
public:
RK4(const A& accel):
accel(accel)
{
}

template<typename T>
inline
void operator()(T& pos, T& speed, const double delta) const
{
const double half_delta = 0.5 * delta;

std::pair<T, T> k1 = accel(pos, speed);
std::pair<T, T> k2 = accel(pos + half_delta * k1.first, speed + half_delta * k1.second);
std::pair<T, T> k3 = accel(pos + half_delta * k2.first, speed + half_delta * k2.second);
std::pair<T, T> k4 = accel(pos + delta * k3.first, speed + delta * k3.second);

pos += delta / 6.0 * (k1.first + 2.0 * (k2.first + k3.first) + k4.first);
speed += delta / 6.0 * (k1.second + 2.0 * (k2.second + k3.second) + k4.second);
}

private:
const A& accel;
};


template<typename T>
class Force
{
public:
explicit Force(const T& up) : m_drag(1.0), m_gravity(gravity*up)
{
}

T operator()(const T& pos, const T& speed) const
{
return drag(m_drag, speed) + spring(pos) + m_gravity;
}

private:

const double m_drag;
T m_gravity;
};


template<typename A>
class Adapt
{
public:
Adapt(const A& accel):
accel(accel)
{
}

template<typename T>
inline std::pair<T, T> operator()(const T& pos, const T& speed) const
{
std::pair<T, T> ret;
ret.first = speed;
ret.second = accel(pos, speed);
return ret;
}

private:
const A& accel;
};


template<typename T, typename I>
inline
void simulate(const I& intg_func, T& pos, T& speed, double t0, const double t_fin, const double delta)
{
while(t0 < t_fin)
{
t0 += delta;
intg_func(pos, speed, delta);
}
}


template<int DIM>
class VecD
{
public:
VecD() {
for (int i=0; i<DIM; ++i)
vals[i] = 0.0;
}

explicit VecD(double v0, double v1=0.0, double v2=0.0)
{
if (DIM >= 1)
vals[0] = v0;

if (DIM >= 2)
vals[1] = v1;

if (DIM >= 3)
vals[2] = v2;

for (int i=3; i<DIM; ++i)
vals[i] = 0.0;
}

inline VecD<DIM> operator+=(const VecD<DIM>& other)
{
for (int i=0; i<DIM; ++i)
vals[i] += other.vals[i];
return *this;
}

inline VecD<DIM> operator+(const VecD<DIM>& other) const
{
VecD<DIM> tmp(*this);
return tmp += other;
}

public:
double vals[DIM];
};


template<int DIM>
inline
VecD<DIM>
operator*(double k, const VecD<DIM>& other)
{
VecD<DIM> ret(other);
for (int i=0; i<DIM; ++i)
ret.vals[i] *= k;
return ret;
}


typedef VecD<1> Vec1D;
typedef VecD<2> Vec2D;
typedef VecD<3> Vec3D;



void make_pos(int i, double& out) { out = (double)i; }
void make_pos(int i, Vec1D& out) { out = Vec1D((double) i); }
void make_pos(int i, Vec2D& out) { out = Vec2D(1.0, (double) i); }
void make_pos(int i, Vec3D& out) { out = Vec3D(1.0, (double) i, -1.0); }

void make_up(double &out) { out = 1.0; }
void make_up(Vec1D &out) { out = Vec1D(1.0); }
void make_up(Vec2D &out) { out = Vec2D(0.0, 1.0); }
void make_up(Vec3D &out) { out = Vec3D(0.0, 1.0, 0.0); }

template<typename Vec>
void run_sim(const char *descr)
{
typedef std::vector< std::pair<Vec, Vec> > Tuples;

std::cout << descr << std::endl;

const double t0 = 0.0;
const double t_fin = 1000.0;
const double delta = 0.005;
const int N_BODIES = 100;

Tuples initial_states;
initial_states.reserve(N_BODIES);
for (int i=0; i<N_BODIES; ++i)
{
Vec pos;
make_pos(i, pos);
initial_states.push_back( std::make_pair(pos, Vec(0.0)) );
}

double el = 0.0;
const int NRUNS=3;
for (int i=0; i<NRUNS; ++i)
{
Tuples states(initial_states);

Vec up;
make_up(up);
Force<Vec> accel(up);
EulerImplicit< Force<Vec> > euler(accel);
long int start0 = GetTickCount();
for (Tuples::iterator it = states.begin();
it != states.end();
++it)
{
simulate(euler, it->first, it->second, t0, t_fin, delta);
}
el += (GetTickCount() - start0) / 1000.0;
}
std::cout << "Time: " << el / NRUNS << "\n";
}

int main(int argc, char **argv)
{
run_sim<double>("double");
run_sim<Vec1D>("class 1D");
run_sim<Vec2D>("class 2D");
run_sim<Vec3D>("class 3D");

char c;
std::cin >> c;
return 0;
}