1 files changed, 60 insertions, 6 deletions

diff --git a/src/vZ/RK.hpp b/src/vZ/RK.hpp
index 3ba4b8a..df608ff 100644
--- a/src/vZ/RK.hpp
+++ b/src/vZ/RK.hpp
@@ -26,23 +26,77 @@
 namespace vZ
 {
   // Base class for Runge-Kutta type algorithms
-  template <typename T>
-  class GenericRKIntegrator : public GenericIntegrator<T>
+  template <typename Y>
+  class GenericRKIntegrator : public GenericIntegrator<Y>
   {
   public:
-    typedef typename GenericIntegrator<T>::Function Function;
+    typedef typename GenericIntegrator<Y>::Scalar   Scalar;
+    typedef typename GenericIntegrator<Y>::Function Function;
 
   protected:
     // Coefficients in the tableau representation of the RK algorithm
-    typedef std::vector<std::vector<T> > ACoefficients;
-    typedef std::vector<T>               BCoefficients;
+    typedef std::vector<std::vector<Scalar> > ACoefficients;
+    typedef std::vector<Scalar>               BCoefficients;
 
-    GenericRKIntegrator(Function f, T dt) : GenericIntegrator<T>(f, dt) { }
+    // Result vectors
+    typedef std::vector<Y> KVector;
+
+    GenericRKIntegrator(Function f) : GenericIntegrator<Y>(f) { }
     virtual ~GenericRKIntegrator() { }
+
+    // Perform the stages of an RK integration
+    KVector calculateK(const ACoefficients& a) const;
+    Y calculateY(const KVector& k, const BCoefficients& b) const;
   };
 
   // Type alias
   typedef GenericRKIntegrator<double> RKIntegrator;
+
+  // Implementation
+
+  template <typename Y>
+  typename GenericRKIntegrator<Y>::KVector
+  GenericRKIntegrator<Y>::calculateK(const ACoefficients& a) const
+  {
+    KVector k;
+    k.reserve(a.size() + 1);
+
+    // k1
+    k.push_back(this->f()(this->x(), this->y()));
+
+    // k2..n
+    for (typename ACoefficients::const_iterator i = a.begin();
+         i != a.end();
+         ++i)
+    {
+      Scalar c = 0;
+      Y y = this->y();
+      for (typename std::vector<Scalar>::size_type j = 0; j < i->size(); ++j) {
+        Scalar aij = i->at(j);
+        c += aij;
+
+        y += aij*this->h()*k.at(j);
+      }
+
+      k.push_back(this->f()(this->x() + c, y));
+    }
+
+    return k;
+  }
+
+  template <typename Y>
+  Y
+  GenericRKIntegrator<Y>::calculateY(const KVector& k, const BCoefficients& b)
+    const
+  {
+    Y y = this->y();
+
+    for (typename std::vector<Scalar>::size_type i = 0; i < k.size(); ++i) {
+      y += this->h()*b.at(i)*k.at(i);
+    }
+
+    return y;
+  }
 }
 
 #endif // VZ_RK_HPP