2 files changed, 59 insertions, 9 deletions

diff --git a/src/vZ/Adaptive.hpp b/src/vZ/Adaptive.hpp
index f0cfed1..a422bb4 100644
--- a/src/vZ/Adaptive.hpp
+++ b/src/vZ/Adaptive.hpp
@@ -50,10 +50,7 @@ namespace vZ
 
     GenericAdaptiveIntegrator(Function f, unsigned int order,
                               ACoefficients a, BCoefficients b,
-                              BCoefficients bStar)
-      : GenericRKIntegrator<Y>(f), m_order(order), m_rejections(0),
-        m_a(a), m_b(b), m_bStar(bStar)
-    { }
+                              BCoefficients bStar);
     virtual ~GenericAdaptiveIntegrator() { }
 
     void step();
@@ -64,6 +61,9 @@ namespace vZ
     unsigned int m_rejections;
     ACoefficients m_a;
     BCoefficients m_b, m_bStar;
+
+    bool m_fsal, m_k1Set;
+    Y m_k1;
   };
 
   // Type alias
@@ -72,18 +72,45 @@ namespace vZ
   // Implementations
 
   template <typename Y>
+  GenericAdaptiveIntegrator<Y>::GenericAdaptiveIntegrator(
+    Function f, unsigned int order,
+    ACoefficients a, BCoefficients b, BCoefficients bStar
+  )
+    : GenericRKIntegrator<Y>(f), m_order(order), m_rejections(0),
+      m_a(a), m_b(b), m_bStar(bStar), m_fsal(true), m_k1Set(false)
+  {
+    std::size_t i;
+    for (i = 0; i < m_a.back().size(); ++i) {
+      if (m_a.back()[i] != m_b.at(i)) {
+        m_fsal = false;
+        return;
+      }
+    }
+
+    if (m_b.at(i) != Scalar(0))
+      m_fsal = false;
+  }
+
+  template <typename Y>
   void
   GenericAdaptiveIntegrator<Y>::step()
   {
     static const Scalar S = Scalar(19)/Scalar(20); // Arbitrary saftey factor
     Scalar newH = this->h();
     Y y;
+    KVector k;
 
     // Attempt the integration step in a loop
     while (true) {
-      KVector k = calculateK(m_a);
-      y         = calculateY(k, m_b);
-      Y yStar   = calculateY(k, m_bStar);
+      if (m_k1Set) {
+        k = calculateK(m_k1, y, m_a);
+      } else if (m_fsal) {
+        k = calculateK(y, m_a);
+      } else {
+        k = calculateK(m_a);
+        y = calculateY(k, m_b);
+      }
+      Y yStar = calculateY(k, m_bStar);
 
       // Get an error estimate
 
@@ -111,6 +138,12 @@ namespace vZ
     this->y(y);
     this->x(this->x() + this->h());
 
+    // Handle FSAL optimization
+    if (m_fsal) {
+      m_k1Set = true;
+      m_k1    = k.back();
+    }
+
     // Adjust the stepsize for the next iteration
     this->h(newH);
   }
diff --git a/src/vZ/RK.hpp b/src/vZ/RK.hpp
index 97ab487..8964741 100644
--- a/src/vZ/RK.hpp
+++ b/src/vZ/RK.hpp
@@ -46,6 +46,8 @@ namespace vZ
 
     // Perform the stages of an RK integration
     KVector calculateK(const ACoefficients& a) const;
+    KVector calculateK(Y& y, const ACoefficients& a) const;
+    KVector calculateK(Y k1, Y& y, const ACoefficients& a) const;
     Y calculateY(const KVector& k, const BCoefficients& b) const;
   };
 
@@ -58,11 +60,26 @@ namespace vZ
   typename GenericRKIntegrator<Y>::KVector
   GenericRKIntegrator<Y>::calculateK(const ACoefficients& a) const
   {
+    Y ytemp; // Ignored
+    return calculateK(ytemp, a);
+  }
+
+  template <typename Y>
+  typename GenericRKIntegrator<Y>::KVector
+  GenericRKIntegrator<Y>::calculateK(Y& y, const ACoefficients& a) const
+  {
+    return calculateK(this->f()(this->x(), this->y()), y, a);
+  }
+
+  template <typename Y>
+  typename GenericRKIntegrator<Y>::KVector
+  GenericRKIntegrator<Y>::calculateK(Y k1, Y& y, const ACoefficients& a) const
+  {
     KVector k;
     k.reserve(a.size() + 1);
 
     // k1
-    k.push_back(this->f()(this->x(), this->y()));
+    k.push_back(k1);
 
     // k2..n
     for (typename ACoefficients::const_iterator i = a.begin();
@@ -70,7 +87,7 @@ namespace vZ
          ++i)
     {
       Scalar c(0);
-      Y y = this->y();
+      y = this->y();
       for (typename std::vector<Scalar>::size_type j = 0; j < i->size(); ++j) {
         Scalar aij = i->at(j);
         c += aij;