interpolation.cc/.h: first working but buggy implementation of cubic Spline interpolation

git-svn-id: svn://localhost/ardour2/branches/3.0@5408 d708f5d6-7413-0410-9779-e7cbd77b26cf

2 files changed, 177 insertions, 43 deletions

diff --git a/libs/ardour/ardour/interpolation.h b/libs/ardour/ardour/interpolation.h
index 01ca994d7d..6ceb63e527 100644
--- a/libs/ardour/ardour/interpolation.h
+++ b/libs/ardour/ardour/interpolation.h
@@ -10,21 +10,31 @@ namespace ARDOUR {
 
 class Interpolation {
  protected:
-         double   _speed, _target_speed;
+     double   _speed, _target_speed;
+
+     // the idea is that when the speed is not 1.0, we have to 
+     // interpolate between samples and then we have to store where we thought we were. 
+     // rather than being at sample N or N+1, we were at N+0.8792922
+     std::vector<double> phase;
+
              
  public:
-         Interpolation () { _speed = 1.0; _target_speed = 1.0; }
+     Interpolation () { _speed = 1.0; _target_speed = 1.0; }
+ 
+     void set_speed (double new_speed)          { _speed = new_speed; _target_speed = new_speed; }
+     void set_target_speed (double new_speed)   { _target_speed = new_speed; }
+
+     double target_speed()          const { return _target_speed; }
+     double speed()                 const { return _speed; }
      
-         void set_speed (double new_speed)          { _speed = new_speed; _target_speed = new_speed; }
-         void set_target_speed (double new_speed)   { _target_speed = new_speed; }
+     void add_channel_to (int input_buffer_size, int output_buffer_size) { phase.push_back (0.0); }
+     void remove_channel_from () { phase.pop_back (); }
 
-         double target_speed()          const { return _target_speed; }
-         double speed()                 const { return _speed; }
-         
-	void add_channel_to (int /*input_buffer_size*/, int /*output_buffer_size*/) {}
-         void remove_channel_from () {}
-  
-         void reset () {}
+     void reset () {
+         for (size_t i = 0; i <= phase.size(); i++) {
+              phase[i] = 0.0;
+          }
+     }
 };
 
 // 40.24 fixpoint math
@@ -72,20 +82,80 @@ class FixedPointLinearInterpolation : public Interpolation {
         void reset ();
 };
 
- class LinearInterpolation : public Interpolation {
+class LinearInterpolation : public Interpolation {
  protected:
-    // the idea is that when the speed is not 1.0, we have to 
-    // interpolate between samples and then we have to store where we thought we were. 
-    // rather than being at sample N or N+1, we were at N+0.8792922
-    std::vector<double> phase;
     
  public:
-         void add_channel_to (int input_buffer_size, int output_buffer_size);
-         void remove_channel_from ();
-     
-         nframes_t interpolate (int channel, nframes_t nframes, Sample* input, Sample* output);
-         void reset ();
- };
+     nframes_t interpolate (int channel, nframes_t nframes, Sample* input, Sample* output);
+};
+ 
+
+#define MAX_PERIOD_SIZE 4096
+/**
+ * @class SplineInterpolation
+ * 
+ * @brief interpolates using cubic spline interpolation over an input period
+ * 
+ * Splines are piecewise cubic functions between each samples,
+ * where the cubic polynomials' values, first and second derivatives are equal
+ * on each sample point.
+ * 
+ * Those conditions are equivalent of solving the linear system of equations
+ * defined by the matrix equation (all indices are zero-based):
+ *  A * M = d
+ *
+ * where A has (n-2) rows and (n-2) columns
+ *
+ *  [ 4 1 0 0 ... 0 0 0 0 ]   [ M[1]   ]   [ 6*y[0] - 12*y[1] + 6*y[2] ]
+ *  [ 1 4 1 0 ... 0 0 0 0 ]   [ M[2]   ]   [ 6*y[1] - 12*y[2] + 6*y[3] ]
+ *  [ 0 1 4 1 ... 0 0 0 0 ]   [ M[3]   ]   [ 6*y[2] - 12*y[3] + 6*y[4] ]
+ *  [ 0 0 1 4 ... 0 0 0 0 ]   [ M[4]   ]   [ 6*y[3] - 12*y[4] + 6*y[5] ]
+ *            ...           *            =            ...            
+ *  [ 0 0 0 0 ... 4 1 0 0 ]   [ M[n-5] ]   [ 6*y[n-6]- 12*y[n-5] + 6*y[n-4] ]
+ *  [ 0 0 0 0 ... 1 4 1 0 ]   [ M[n-4] ]   [ 6*y[n-5]- 12*y[n-4] + 6*y[n-3] ]
+ *  [ 0 0 0 0 ... 0 1 4 1 ]   [ M[n-3] ]   [ 6*y[n-4]- 12*y[n-3] + 6*y[n-2] ]
+ *  [ 0 0 0 0 ... 0 0 1 4 ]   [ M[n-2] ]   [ 6*y[n-3]- 12*y[n-2] + 6*y[n-1] ]
+ *
+ *  For our purpose we use natural splines which means the boundary coefficients
+ *  M[0] = M[n-1] = 0
+ *
+ *  The interpolation polynomial in the i-th interval then has the form
+ *  p_i(x) = a3 (x - i)^3 + a2 (x - i)^2 + a1 (x - i) + a0
+ *         = ((a3 * (x - i) + a2) * (x - i) + a1) * (x - i) + a0
+ *     where
+ *  a3 = (M[i+1] - M[i]) / 6
+ *  a2 = M[i] / 2 
+ *  a1 = y[i+1] - y[i] - M[i+1]/6 - M[i]/3
+ *  a0 = y[i] 
+ *
+ *  We solve the system by LU-factoring the matrix A:
+ *  A = L * U:
+ *
+ *  [ 4 1 0 0 ... 0 0 0 0 ]   [ 1    0    0    0   ... 0      0      0      0 ]   [ m[0] 1    0    0   ... 0      0      0      ]
+ *  [ 1 4 1 0 ... 0 0 0 0 ]   [ l[0] 1    0    0   ... 0      0      0      0 ]   [ 0    m[1] 1    0   ... 0      0      0      ]
+ *  [ 0 1 4 1 ... 0 0 0 0 ]   [ 0    l[1] 1    0   ... 0      0      0      0 ]   [ 0    0    m[2] 1   ... 0      0      0      ]
+ *  [ 0 0 1 4 ... 0 0 0 0 ]   [ 0    0    l[2] 1   ... 0      0      0      0 ]                        ...                
+ *            ...           =                     ...                          *  [ 0    0    0    0   ... 0      0      0      ]
+ *  [ 0 0 0 0 ... 4 1 0 0 ]   [ 0    0    0    0   ... 1      0      0      0 ]   [ 0    0    0    0   ... 1      0      0      ]
+ *  [ 0 0 0 0 ... 1 4 1 0 ]   [ 0    0    0    0   ... l[n-6] 1      0      0 ]   [ 0    0    0    0   ... m[n-5] 1      0      ]
+ *  [ 0 0 0 0 ... 0 1 4 1 ]   [ 0    0    0    0   ... 0      l[n-5] 1      0 ]   [ 0    0    0    0   ... 0      m[n-4] 1      ]
+ *  [ 0 0 0 0 ... 0 0 1 4 ]   [ 0    0    0    0   ... 0      0      l[n-4] 1 ]   [ 0    0    0    0   ... 0      0      m[n-3] ]
+ *
+ *  where the l[i] and m[i] can be precomputed.
+ * 
+ *  Then we solve the system A * M = d by first solving the system
+ *    L * t = d 
+ *  and then
+ *    R * M = t
+ */
+class SplineInterpolation : public Interpolation {
+ protected:
+    double l[MAX_PERIOD_SIZE], m[MAX_PERIOD_SIZE];
+    
+ public:
+    SplineInterpolation();
+    nframes_t interpolate (int channel, nframes_t nframes, Sample* input, Sample* output);
+};
 
 class LibSamplerateInterpolation : public Interpolation {
  protected:
@@ -101,7 +171,7 @@ class LibSamplerateInterpolation : public Interpolation {
         ~LibSamplerateInterpolation ();
     
         void   set_speed (double new_speed);
-        void   set_target_speed (double /*new_speed*/) {}
+        void   set_target_speed (double new_speed)   {}
         double speed ()                        const { return _speed;      }
         
         void add_channel_to (int input_buffer_size, int output_buffer_size);
diff --git a/libs/ardour/interpolation.cc b/libs/ardour/interpolation.cc
index a5cdf1ce8b..8b4bb862ed 100644
--- a/libs/ardour/interpolation.cc
+++ b/libs/ardour/interpolation.cc
@@ -13,7 +13,7 @@ FixedPointLinearInterpolation::interpolate (int channel, nframes_t nframes, Samp
 	// rather than being at sample N or N+1, we were at N+0.8792922
 	// so the "phase" element, if you want to think about this way, 
 	// varies from 0 to 1, representing the "offset" between samples
-	uint64_t	phase = last_phase[channel];
+	uint64_t	the_phase = last_phase[channel];
 	
 	// acceleration
 	int64_t	 phi_delta;
@@ -29,8 +29,8 @@ FixedPointLinearInterpolation::interpolate (int channel, nframes_t nframes, Samp
 	nframes_t   i = 0;
 
 	for (nframes_t outsample = 0; outsample < nframes; ++outsample) {
-		i = phase >> 24;
-		Sample fractional_phase_part = (phase & fractional_part_mask) / binary_scaling_factor;
+		i = the_phase >> 24;
+		Sample fractional_phase_part = (the_phase & fractional_part_mask) / binary_scaling_factor;
 		
 		if (input && output) {
 			// Linearly interpolate into the output buffer
@@ -39,10 +39,10 @@ FixedPointLinearInterpolation::interpolate (int channel, nframes_t nframes, Samp
 				input[i+1] * fractional_phase_part;
 		}
 		
-		phase += phi + phi_delta;
+		the_phase += phi + phi_delta;
 	}
 
-	last_phase[channel] = (phase & fractional_part_mask);
+	last_phase[channel] = (the_phase & fractional_part_mask);
 	
 	// playback distance
 	return i;
@@ -116,25 +116,89 @@ LinearInterpolation::interpolate (int channel, nframes_t nframes, Sample *input,
 	return i;
 }
 
-void 
-LinearInterpolation::add_channel_to (int /*input_buffer_size*/, int /*output_buffer_size*/)
-{
-	phase.push_back (0.0);
-}
-
-void 
-LinearInterpolation::remove_channel_from ()
+SplineInterpolation::SplineInterpolation()
 {
-	phase.pop_back ();
+    // precompute LU-factorization of matrix A
+    // see "Teubner Taschenbuch der Mathematik", p. 1105
+    m[0] = 4.0;
+    for (int i = 0; i <= MAX_PERIOD_SIZE - 2; i++) {
+        l[i] = 1.0 / m[i];
+        m[i+1] = 4.0 - l[i];
+    }
 }
 
-
-void
-LinearInterpolation::reset() 
+nframes_t
+SplineInterpolation::interpolate (int channel, nframes_t nframes, Sample *input, Sample *output)
 {
-	for (size_t i = 0; i <= phase.size(); i++) {
-		phase[i] = 0.0;
-	}
+    // How many input samples we need
+    nframes_t n = ceil (double(nframes) * _speed) + 2;
+    //   |------------------------------------------^
+    // this won't be here in the debugged version.
+    
+    double M[n], t[n-2];
+    
+    // natural spline: boundary conditions
+    M[0]     = 0.0;
+    M[n - 1] = 0.0;
+    
+    // solve L * t = d
+    // see "Teubner Taschenbuch der Mathematik", p. 1105
+    t[0] = 6.0 * (input[0] - 2*input[1] + input[2]); 
+    for (nframes_t i = 1; i <= n - 3; i++) {
+        t[i] = 6.0 * (input[i] - 2*input[i+1] + input[i+2])
+               - l[i-1] * t[i-1];
+    }
+    
+    // solve R * M = t
+    // see "Teubner Taschenbuch der Mathematik", p. 1105
+    M[n-2] = -t[n-3] / m[n-3];
+    for (nframes_t i = n-4;; i--) {
+        M[i+1] = -(t[i] + M[i+2]) / m[i];
+        if ( i == 0 ) break;
+    }
+    
+    // now interpolate
+    // index in the input buffers
+    nframes_t   i = 0;
+    
+    double acceleration;
+    double distance = 0.0;
+    
+    if (_speed != _target_speed) {
+        acceleration = _target_speed - _speed;
+    } else {
+        acceleration = 0.0;
+    }
+    
+    distance = phase[channel];
+    for (nframes_t outsample = 0; outsample < nframes; outsample++) {
+        i = floor(distance);
+        
+        Sample x = distance - i;
+        
+        /* this would break the assertion below
+        if (x >= 1.0) {
+            x -= 1.0;
+            i++;
+        }
+        */
+        
+        if (input && output) {
+            assert (i <= n-1);
+            double a0 = input[i];
+            double a1 = input[i+1] - input[i] - M[i+1]/6.0 - M[i]/3.0;
+            double a2 = M[i] / 2.0;
+            double a3 = (M[i+1] - M[i]) / 6.0;
+            // interpolate into the output buffer
+            output[outsample] = ((a3*x +a2)*x +a1)*x + a0;
+        }
+        distance += _speed + acceleration;
+    }
+    
+    i = floor(distance);
+    phase[channel] = distance - floor(distance);
+    
+    return i;
 }
 
 LibSamplerateInterpolation::LibSamplerateInterpolation() : state (0)