add interp2d() for fast bilinear 2D interpolation

1 files changed, 47 insertions, 0 deletions

diff --git a/src/misc.c b/src/misc.c
index d20bc26..605f3ea 100644
--- a/src/misc.c
+++ b/src/misc.c
@@ -3,6 +3,7 @@
 #include <stdlib.h> /* for size_t */
 #include <gsl/gsl_sf_gamma.h>
 #include "vector.h"
+#include <stdio.h>
 
 static struct {
     int n;
@@ -338,6 +339,52 @@ double kahan_sum(double *x, size_t n)
     return sum;
 }
 
+double interp2d(double x, double y, double *xp, double *yp, double *zp, size_t n1, size_t n2)
+{
+    /* A fast bilinear interpolation routine which assumes that the values in
+     * `xp` and `yp` are evenly spaced.
+     *
+     * `zp` should be a 2D array indexed as zp[i,j] = zp[i*n2 + j].
+     *
+     * If x < xp[0], x > xp[n-1], y < yp[0], or y > yp[n-1] prints an error and
+     * exits.
+     *
+     * See https://en.wikipedia.org/wiki/Bilinear_interpolation. */
+    size_t i, j;
+    double q11, q12, q21, q22;
+
+    if (x <= xp[0]) {
+        fprintf(stderr, "x < xp[0]!\n");
+        exit(1);
+    }
+
+    if (y <= yp[0]) {
+        fprintf(stderr, "y < xp[0]!\n");
+        exit(1);
+    }
+
+    i = (x-xp[0])/(xp[1]-xp[0]);
+
+    if (i > n1-2) {
+        fprintf(stderr, "i > n1 - 2!\n");
+        exit(1);
+    }
+
+    j = (y-yp[0])/(yp[1]-yp[0]);
+
+    if (j > n2-2) {
+        fprintf(stderr, "j > n2 - 2!\n");
+        exit(1);
+    }
+
+    q11 = zp[i*n2+j];
+    q12 = zp[i*n2+j+1];
+    q21 = zp[(i+1)*n2+j];
+    q22 = zp[(i+1)*n2+j+1];
+
+    return (q11*(xp[i+1]-x)*(yp[j+1]-y) + q21*(x-xp[i])*(yp[j+1]-y) + q12*(xp[i+1]-x)*(y-yp[j]) + q22*(x-xp[i])*(y-yp[j]))/((xp[i+1]-xp[i])*(yp[i+1]-yp[i]));
+}
+
 double interp1d(double x, double *xp, double *yp, size_t n)
 {
     /* A fast interpolation routine which assumes that the values in `xp` are