sascha@361: package de.intevation.gnv.math;
sascha@361: 
sascha@361: import com.vividsolutions.jts.geom.Coordinate;
sascha@361: import com.vividsolutions.jts.geom.Envelope;
sascha@361: 
sascha@361: import com.vividsolutions.jts.index.quadtree.Quadtree;
sascha@361: 
sascha@501: import gnu.trove.TDoubleArrayList;
sascha@501: 
sascha@501: import java.util.ArrayList;
sascha@501: import java.util.Collections;
sascha@501: import java.util.HashMap;
sascha@501: import java.util.List;
sascha@501: 
sascha@501: import org.apache.commons.math.stat.descriptive.moment.Mean;
sascha@501: import org.apache.commons.math.stat.descriptive.moment.StandardDeviation;
sascha@501: 
ingo@365: import org.apache.log4j.Logger;
ingo@365: 
sascha@361: /**
sascha@501:  *  @author Sascha L. Teichmann (sascha.teichmann@intevation.de)
sascha@361:  */
sascha@361: public final class Interpolation2D
sascha@361: {
ingo@365:     private static Logger log = Logger.getLogger(Interpolation2D.class);
ingo@365: 
sascha@361:     public interface Consumer {
ingo@416:         void interpolated(Coordinate point, boolean success);
sascha@361:     } // interface Consumer
sascha@361: 
sascha@361:     private Interpolation2D() {
sascha@361:     }
sascha@361: 
sascha@501:     private static final double dist(double a, double b) {
sascha@501:         a -= b;
sascha@501:         return Math.sqrt(a*a);
sascha@501:     }
sascha@501: 
sascha@501:     private static final double OUTLIER_EPS = 0.4d;
sascha@501: 
sascha@501:     public static final double [] calculateBufferRemoveOutlier(
sascha@501:         List <? extends Point2d> points
sascha@501:     ) {
sascha@501:         HashMap<Integer, ArrayList<Point2d>> iMap = 
sascha@501:             new HashMap<Integer, ArrayList<Point2d>>();
sascha@501: 
sascha@501:         HashMap<Integer, ArrayList<Point2d>> jMap = 
sascha@501:             new HashMap<Integer, ArrayList<Point2d>>();
sascha@501: 
sascha@501:         for (int k = points.size()-1; k >= 0; --k) {
sascha@501:             Point2d p = points.get(k);
sascha@501: 
sascha@501:             ArrayList<Point2d> jList = jMap.get(p.j);
sascha@501:             ArrayList<Point2d> iList = iMap.get(p.i);
sascha@501: 
sascha@501:             if (jList == null) {
sascha@501:                 jMap.put(p.j, jList = new ArrayList<Point2d>());
sascha@501:             }
sascha@501:             jList.add(p);
sascha@501: 
sascha@501:             if (iList == null) {
sascha@501:                 iMap.put(p.i, iList = new ArrayList<Point2d>());
sascha@501:             }
sascha@501:             iList.add(p);
sascha@501:         }
sascha@501: 
sascha@501:         TDoubleArrayList deltas = new TDoubleArrayList();
sascha@501: 
sascha@501:         Mean mean            = new Mean();
sascha@501:         StandardDeviation sd = new StandardDeviation();
sascha@501: 
sascha@501:         for (ArrayList<Point2d> v: jMap.values()) {
sascha@501:             Collections.sort(v, Point2d.Y_COMPARATOR);
sascha@501:             for (int i = 1, L = v.size(); i < L; ++i) {
sascha@501:                 double dy = Math.abs(v.get(i).y - v.get(i-1).y);
sascha@501:                 deltas.add(dy);
sascha@501:                 mean.increment(dy);
sascha@501:                 sd  .increment(dy);
sascha@501:             }
sascha@501:         }
sascha@501: 
sascha@501:         deltas.sort();
sascha@501: 
sascha@501:         double m   = mean.getResult();
sascha@501:         double s   = sd  .getResult();
sascha@501:         double eps = OUTLIER_EPS*s;
sascha@501: 
sascha@501:         int yi = deltas.size() - 1;
sascha@501:         while (yi > 0 && dist(deltas.get(yi), m) > eps) {
sascha@501:             --yi;
sascha@501:         }
sascha@501: 
sascha@501:         double dyMax = deltas.get(yi) + 1e-5d;
sascha@501: 
sascha@501:         if (log.isDebugEnabled()) {
sascha@501:             log.debug("mean  y: " + m);
sascha@501:             log.debug("sigma y: " + s);
sascha@501:         }
sascha@501: 
sascha@501:         deltas.reset();
sascha@501:         mean.clear();
sascha@501:         sd  .clear();
sascha@501: 
sascha@501:         for (ArrayList<Point2d> v: iMap.values()) {
sascha@501:             Collections.sort(v, Point2d.X_COMPARATOR);
sascha@501:             for (int i = 1, L = v.size(); i < L; ++i) {
sascha@501:                 double dx = Math.abs(v.get(i).x - v.get(i-1).x);
sascha@501:                 deltas.add(dx);
sascha@501:                 mean.increment(dx);
sascha@501:                 sd  .increment(dx);
sascha@501:             }
sascha@501:         }
sascha@501: 
sascha@501:         deltas.sort();
sascha@501: 
sascha@501:         m   = mean.getResult();
sascha@501:         s   = sd  .getResult();
sascha@501:         eps = OUTLIER_EPS*s;
sascha@501: 
sascha@501:         int xi = deltas.size() - 1;
sascha@501: 
sascha@501:         while (xi > 0 && dist(deltas.get(xi), m) > eps) {
sascha@501:             --xi;
sascha@501:         }
sascha@501: 
sascha@501:         double dxMax = deltas.get(xi) + 1e-5d;
sascha@501: 
sascha@501:         if (log.isDebugEnabled()) {
sascha@501:             log.debug("mean  y: " + m);
sascha@501:             log.debug("sigma y: " + s);
sascha@501:         }
sascha@501: 
sascha@501:         return new double [] { dxMax, dyMax };
sascha@501:     }
sascha@501: 
sascha@431:     public static final double [] calculateBuffer(
sascha@431:         List <? extends Point2d> points
sascha@431:     ) {
sascha@431:         HashMap<Integer, ArrayList<Point2d>> iMap = 
sascha@431:             new HashMap<Integer, ArrayList<Point2d>>();
sascha@431: 
sascha@431:         HashMap<Integer, ArrayList<Point2d>> jMap = 
sascha@431:             new HashMap<Integer, ArrayList<Point2d>>();
sascha@431: 
sascha@431:         for (int k = points.size()-1; k >= 0; --k) {
sascha@431:             Point2d p = points.get(k);
sascha@431: 
sascha@431:             ArrayList<Point2d> jList = jMap.get(p.j);
sascha@445:             ArrayList<Point2d> iList = iMap.get(p.i);
sascha@431: 
sascha@431:             if (jList == null) {
sascha@445:                 jMap.put(p.j, jList = new ArrayList<Point2d>());
sascha@431:             }
sascha@431:             jList.add(p);
sascha@431: 
sascha@431:             if (iList == null) {
sascha@445:                 iMap.put(p.i, iList = new ArrayList<Point2d>());
sascha@431:             }
sascha@431:             iList.add(p);
sascha@431:         }
sascha@431: 
sascha@431:         double dxMax = -Double.MAX_VALUE;
sascha@431:         double dyMax = -Double.MAX_VALUE;
sascha@431: 
sascha@431:         for (ArrayList<Point2d> v: jMap.values()) {
sascha@431:             Collections.sort(v, Point2d.Y_COMPARATOR);
sascha@431:             for (int i = 1, L = v.size(); i < L; ++i) {
sascha@445:                 double dy = Math.abs(v.get(i).y - v.get(i-1).y);
sascha@431:                 if (dy > dyMax) {
sascha@431:                     dyMax = dy;
sascha@431:                 }
sascha@431:             }
sascha@431:         }
sascha@431: 
sascha@431:         dyMax += 1e-5d;
sascha@431: 
sascha@431:         for (ArrayList<Point2d> v: iMap.values()) {
sascha@431:             Collections.sort(v, Point2d.X_COMPARATOR);
sascha@431:             for (int i = 1, L = v.size(); i < L; ++i) {
sascha@431:                 double dx = Math.abs(v.get(i).x - v.get(i-1).x);
sascha@431:                 if (dx > dxMax) {
sascha@431:                     dxMax = dx;
sascha@431:                 }
sascha@431:             }
sascha@431:         }
sascha@431: 
sascha@431:         dxMax += 1e-5d;
sascha@431: 
sascha@431:         return new double [] { dxMax, dyMax };
sascha@431:     }
sascha@431: 
sascha@361:     public static void interpolate(
sascha@361:         List<? extends Coordinate> path,
sascha@361:         List<? extends Point2d>    points,
sascha@361:         double                     from,
sascha@361:         double                     to,
sascha@361:         int                        steps,
sascha@361:         Metrics                    metrics,
sascha@361:         Consumer                   consumer
sascha@361:     ) {
sascha@434:         boolean debug = log.isDebugEnabled();
sascha@434: 
sascha@361:         int N = path.size();
sascha@361:         int M = points.size();
sascha@361: 
sascha@434:         if (debug) {
ingo@416:             log.debug("Size of path: " + N);
ingo@416:             log.debug("Size of points: " + M);
ingo@416:         }
ingo@365: 
sascha@361:         if (M < 1 || N < 2) { // nothing to do
sascha@361:             return;
sascha@361:         }
ingo@365: 
sascha@431:         double [] buffer = calculateBuffer(points);
sascha@431:         double dxMax = buffer[0];
sascha@431:         double dyMax = buffer[1];
ingo@365: 
sascha@434:         if (debug) {
ingo@416:             log.debug("buffer size: " + dxMax + " / " + dyMax);
ingo@416:         }
sascha@361: 
sascha@361:         // put into spatial index to speed up finding neighbors.
sascha@361:         Quadtree spatialIndex = new Quadtree();
sascha@361: 
sascha@361:         for (int i = 0; i < M; ++i) {
sascha@361:             Point2d p = points.get(i);
sascha@361:             spatialIndex.insert(p.envelope(), p);
sascha@361:         }
sascha@361: 
sascha@361:         LinearToMap linearToMap = new LinearToMap(
sascha@361:             path, from, to, metrics);
sascha@361: 
sascha@361:         double dP = (to - from)/steps;
sascha@361: 
sascha@361:         Coordinate center = new Coordinate();
sascha@361: 
sascha@361:         Envelope queryBuffer = new Envelope();
sascha@361: 
sascha@361:         Point2d [] neighbors = new Point2d[4];
sascha@361: 
ingo@365:         int missedInterpolations = 0;
ingo@365:         int interpolations = 0;
ingo@365: 
sascha@474:         L1Comparator invL1 = new L1Comparator(center);
sascha@474: 
ingo@365:         for (double p = from; p <= to; p += dP) {
sascha@361:             if (!linearToMap.locate(p, center)) {
sascha@361:                 continue;
sascha@361:             }
sascha@361:             queryBuffer.init(
sascha@361:                 center.x - dxMax, center.x + dxMax, 
sascha@361:                 center.y - dyMax, center.y + dyMax);
sascha@361: 
sascha@361:             List potential = spatialIndex.query(queryBuffer);
sascha@361: 
sascha@361:             Collections.sort(potential, invL1);
sascha@361: 
sascha@361:             neighbors[0] = neighbors[1] = 
sascha@361:             neighbors[2] = neighbors[3] = null;
sascha@361: 
sascha@361:             /* bit code of neighbors
sascha@361:                0---1
sascha@361:                | x |
sascha@361:                2---3
sascha@361:             */
sascha@361: 
sascha@361:             int mask = 0;
sascha@361:             // reversed order is essential here!
sascha@361:             for (int i = potential.size()-1; i >= 0; --i) {
sascha@361:                 Point2d n = (Point2d)potential.get(i);
sascha@361:                 int code = n.x > center.x ? 1 : 0;
sascha@361:                 if (n.y > center.y) code |= 2;
sascha@361:                 neighbors[code] = n;
sascha@361:                 mask |= 1 << code;
sascha@361:             }
sascha@361: 
sascha@361:             int numNeighbors = Integer.bitCount(mask);
sascha@361: 
sascha@361:             // only interpolate if we have all four neighbors
sascha@361:             // and we do not have any gaps.
sascha@361:             if (numNeighbors == 4
sascha@366:             && !neighbors[0].hasIGap(neighbors[1])
sascha@361:             && !neighbors[1].hasJGap(neighbors[3])
sascha@361:             && !neighbors[3].hasIGap(neighbors[2])
sascha@361:             && !neighbors[2].hasJGap(neighbors[0])
sascha@361:             ) {
sascha@361:                 double z1 = interpolate(
sascha@361:                     neighbors[0].x, neighbors[0].z,
sascha@361:                     neighbors[1].x, neighbors[1].z,
sascha@361:                     center.x);
sascha@361:                 double z2 = interpolate(
sascha@361:                     neighbors[2].x, neighbors[2].z,
sascha@361:                     neighbors[3].x, neighbors[3].z,
sascha@361:                     center.x);
sascha@361:                 double y1 = interpolate(
sascha@361:                     neighbors[0].x, neighbors[0].y,
sascha@361:                     neighbors[1].x, neighbors[1].y,
sascha@361:                     center.x);
sascha@361:                 double y2 = interpolate(
sascha@361:                     neighbors[2].x, neighbors[2].y,
sascha@361:                     neighbors[3].x, neighbors[3].y,
sascha@361:                     center.x);
sascha@361:                 center.z = interpolate(
sascha@361:                     y1, z1,
sascha@361:                     y2, z2,
sascha@361:                     center.y);
ingo@416:                 consumer.interpolated(center, true);
ingo@365:                 ++interpolations;
ingo@365:             }
ingo@365:             else {
ingo@416:                 consumer.interpolated(center, false);
ingo@365:                 ++missedInterpolations;
sascha@361:             }
sascha@361:         }
ingo@365: 
sascha@434:         if (debug) {
ingo@416:             log.debug("interpolations: " + 
ingo@416:                 interpolations + " / " + missedInterpolations);
ingo@416:         }
sascha@361:     }
sascha@361: 
sascha@361:     public static final double interpolate(
sascha@361:         double x1, double y1,
sascha@361:         double x2, double y2,
sascha@361:         double x
sascha@361:     ) {
sascha@361:         if (x2 == x1) {
sascha@361:             return (y1 + y2)*0.5d;
sascha@361:         }
sascha@361:         double m = (y2-y1)/(x2-x1);
sascha@361:         double b = y1 - m*x1;
sascha@361:         return m*x + b;
sascha@361:     }
sascha@361: }
sascha@361: // vim:set ts=4 sw=4 si et sta sts=4 fenc=utf8: