--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gnv-artifacts/src/main/java/de/intevation/gnv/math/Interpolation2D.java	Fri Sep 28 12:13:47 2012 +0200
@@ -0,0 +1,214 @@
+package de.intevation.gnv.math;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.HashMap;
+import java.util.Collections;
+
+import com.vividsolutions.jts.geom.Coordinate;
+import com.vividsolutions.jts.geom.Envelope;
+
+import com.vividsolutions.jts.index.quadtree.Quadtree;
+
+import org.apache.log4j.Logger;
+
+/**
+ *  @author Sascha L. Teichmann
+ */
+public final class Interpolation2D
+{
+    private static Logger log = Logger.getLogger(Interpolation2D.class);
+
+    public interface Consumer {
+        void interpolated(Coordinate point);
+    } // interface Consumer
+
+    private Interpolation2D() {
+    }
+
+    public static void interpolate(
+        List<? extends Coordinate> path,
+        List<? extends Point2d>    points,
+        double                     from,
+        double                     to,
+        int                        steps,
+        Metrics                    metrics,
+        Consumer                   consumer
+    ) {
+        int N = path.size();
+        int M = points.size();
+
+        log.debug("Size of path: " + N);
+        log.debug("Size of points: " + M);
+
+        if (M < 1 || N < 2) { // nothing to do
+            return;
+        }
+
+        HashMap<Integer, ArrayList<Point2d>> map = new HashMap<Integer, ArrayList<Point2d>>();
+
+        for (int k = M-1; k >= 0; --k) {
+            Point2d p = points.get(k);
+
+            ArrayList<Point2d> list = map.get(p.j);
+
+            if (list == null) {
+                map.put(p.j, list = new ArrayList<Point2d>());
+            }
+            list.add(p);
+        }
+
+        double dxMax = -Double.MAX_VALUE;
+
+        for (ArrayList<Point2d> v: map.values()) {
+            Collections.sort(v, Point2d.X_COMPARATOR);
+            for (int i = 1, L = v.size(); i < L; ++i) {
+                double dx = Math.abs(v.get(i).x - v.get(i-1).x);
+                if (dx > dxMax) {
+                    dxMax = dx;
+                }
+            }
+        }
+
+        dxMax = dxMax + 1e-5d;
+
+        map.clear();
+
+        for (int k = M-1; k >= 0; --k) {
+            Point2d p = points.get(k);
+
+            ArrayList<Point2d> list = map.get(p.i);
+
+            if (list == null) {
+                map.put(p.i, list = new ArrayList<Point2d>());
+            }
+            list.add(p);
+        }
+
+        double dyMax = -Double.MAX_VALUE;
+
+        for (ArrayList<Point2d> v: map.values()) {
+            Collections.sort(v, Point2d.Y_COMPARATOR);
+            for (int i = 1, L = v.size(); i < L; ++i) {
+                double dy = Math.abs(v.get(i).y - v.get(i-1).y);
+                if (dy > dyMax) {
+                    dyMax = dy;
+                }
+            }
+        }
+
+        dyMax = dyMax + 1e-5d;
+
+        map = null;
+
+        log.debug("buffer size: " + dxMax + " / " + dyMax);
+
+        // put into spatial index to speed up finding neighbors.
+        Quadtree spatialIndex = new Quadtree();
+
+        for (int i = 0; i < M; ++i) {
+            Point2d p = points.get(i);
+            spatialIndex.insert(p.envelope(), p);
+        }
+
+        LinearToMap linearToMap = new LinearToMap(
+            path, from, to, metrics);
+
+        double dP = (to - from)/steps;
+
+        Coordinate center = new Coordinate();
+
+        Envelope queryBuffer = new Envelope();
+
+        Point2d [] neighbors = new Point2d[4];
+
+        int missedInterpolations = 0;
+        int interpolations = 0;
+
+        for (double p = from; p <= to; p += dP) {
+            if (!linearToMap.locate(p, center)) {
+                continue;
+            }
+            queryBuffer.init(
+                center.x - dxMax, center.x + dxMax, 
+                center.y - dyMax, center.y + dyMax);
+
+            List potential = spatialIndex.query(queryBuffer);
+
+            L1Comparator invL1 = new L1Comparator(center);
+            Collections.sort(potential, invL1);
+
+            neighbors[0] = neighbors[1] = 
+            neighbors[2] = neighbors[3] = null;
+
+            /* bit code of neighbors
+               0---1
+               | x |
+               2---3
+            */
+
+            int mask = 0;
+            // reversed order is essential here!
+            for (int i = potential.size()-1; i >= 0; --i) {
+                Point2d n = (Point2d)potential.get(i);
+                int code = n.x > center.x ? 1 : 0;
+                if (n.y > center.y) code |= 2;
+                neighbors[code] = n;
+                mask |= 1 << code;
+            }
+
+            int numNeighbors = Integer.bitCount(mask);
+
+            // only interpolate if we have all four neighbors
+            // and we do not have any gaps.
+            if (numNeighbors == 4
+            && !neighbors[0].hasIGap(neighbors[1])
+            && !neighbors[1].hasJGap(neighbors[3])
+            && !neighbors[3].hasIGap(neighbors[2])
+            && !neighbors[2].hasJGap(neighbors[0])
+            ) {
+                double z1 = interpolate(
+                    neighbors[0].x, neighbors[0].z,
+                    neighbors[1].x, neighbors[1].z,
+                    center.x);
+                double z2 = interpolate(
+                    neighbors[2].x, neighbors[2].z,
+                    neighbors[3].x, neighbors[3].z,
+                    center.x);
+                double y1 = interpolate(
+                    neighbors[0].x, neighbors[0].y,
+                    neighbors[1].x, neighbors[1].y,
+                    center.x);
+                double y2 = interpolate(
+                    neighbors[2].x, neighbors[2].y,
+                    neighbors[3].x, neighbors[3].y,
+                    center.x);
+                center.z = interpolate(
+                    y1, z1,
+                    y2, z2,
+                    center.y);
+                consumer.interpolated(center);
+                ++interpolations;
+            }
+            else {
+                ++missedInterpolations;
+            }
+        }
+
+        log.debug("interpolations: " + interpolations + " / " + missedInterpolations);
+    }
+
+    public static final double interpolate(
+        double x1, double y1,
+        double x2, double y2,
+        double x
+    ) {
+        if (x2 == x1) {
+            return (y1 + y2)*0.5d;
+        }
+        double m = (y2-y1)/(x2-x1);
+        double b = y1 - m*x1;
+        return m*x + b;
+    }
+}
+// vim:set ts=4 sw=4 si et sta sts=4 fenc=utf8: