Package net.sourceforge.gpstools

Source Code of net.sourceforge.gpstools.TrackInterpolator

package net.sourceforge.gpstools;

/* gpsdings
* Copyright (C) 2006 Moritz Ringler
* $Id: TrackInterpolator.java 442 2011-01-07 11:24:44Z ringler $
*
*  This program is free software: you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation, either version 3 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  You should have received a copy of the GNU General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

import java.io.PrintStream;
import java.math.BigDecimal;
import java.text.DateFormat;
import java.text.SimpleDateFormat;
import java.util.*;

import net.sourceforge.gpstools.gpx.GpxType;
import net.sourceforge.gpstools.gpx.Trk;
import net.sourceforge.gpstools.gpx.TrkType;
import net.sourceforge.gpstools.gpx.Trkpt;
import net.sourceforge.gpstools.gpx.Trkseg;
import net.sourceforge.gpstools.gpx.TrksegType;
import net.sourceforge.gpstools.gpx.Wpt;
import net.sourceforge.gpstools.gpx.WptType;
import org.apache.commons.math.MathException;
import org.apache.commons.math.FunctionEvaluationException;
import org.apache.commons.math.analysis.UnivariateRealInterpolator;
import net.sourceforge.gpstools.math.*;
import net.sourceforge.gpstools.math.PieceWiseFunction.SegmentFunction;

public class TrackInterpolator {
    final UnivariateRealInterpolator interpolator = new LinearInterpolator();
    final PieceWiseFunction fLongi = new PieceWiseFunction();
    final PieceWiseFunction fLati = new PieceWiseFunction();
    final PieceWiseFunction fEle = new PieceWiseFunction();

    private static final DateFormat ISO_UTC = new SimpleDateFormat(
            "yyyy-MM-dd'T'HH:mm:ss'Z'");
    static {
        ISO_UTC.setTimeZone(TimeZone.getTimeZone("UTC"));
    }

    private static final Comparator<WptType> wptSorter = new Comparator<WptType>() {
        @Override
        public int compare(WptType pt1, WptType pt2) {
            return pt1.getTime().compareTo(pt2.getTime());
        }
    };

    public static int compareByDateTime(WptType pt1, WptType pt2) {
        return wptSorter.compare(pt1, pt2);
    }

    public TrackInterpolator() {
        // explicit default constructor.
    }

    public int addAllTracks(GpxType gpx) {
        final Enumeration<? extends Trk> trks = gpx.enumerateTrk();
        TrkType trk;
        int count = 0;
        while (trks.hasMoreElements()) {
            trk = trks.nextElement();
            addTrack(trk);
            count++;
        }
        return count;
    }

    public void addTrack(TrkType trk) {
        final Enumeration<? extends Trkseg> segs = trk.enumerateTrkseg();
        for (TrksegType trkseg; segs.hasMoreElements();) {
            trkseg = segs.nextElement();
            addTrackSegment(trkseg);
        }
    }

    public synchronized void addTrackSegment(TrksegType seg) {
        final Enumeration<? extends Trkpt> pts = seg.enumerateTrkpt();
        Trkpt trkpt;
        final SortedSet<WptType> set = new TreeSet<WptType>(wptSorter);
        while (pts.hasMoreElements()) {
            trkpt = pts.nextElement();
            addWpt(set, trkpt);
        }
        if (set.size() < 3) {
            System.err
                    .println("Warning: dropping track segment because it has less than three trackpoints with different timestamps.");
        } else {
            interpolateSegment(set);
        }
    }

    public void addTrackSegment(WptType[] pts) {
        final SortedSet<WptType> set = new TreeSet<WptType>(wptSorter);
        for (WptType trkpt : pts) {
            addWpt(set, trkpt);
        }
        interpolateSegment(set);
    }

    private void addWpt(Set<WptType> seg, WptType pt) {
        if (pt.getTime() != null) {
            seg.add(pt);
        }
    }

    protected synchronized void interpolateSegment(SortedSet<WptType> points) {
        final int n = points.size();
        if (n == 0) {
            return;
        }
        final double[] time = new double[n];

        final double[] longi = new double[n];
        final double[] lati = new double[n];
        final SortedMap<Double, BigDecimal> elev = new TreeMap<Double, BigDecimal>();

        BigDecimal ele;
        int i = 0;
        double lontmp;
        double lattmp;
        for (WptType wpt : points) {
            lontmp = wpt.getLon().doubleValue();
            lattmp = wpt.getLat().doubleValue();
            if (Math.abs(lattmp) > 90 || Math.abs(lontmp) > 180) {
                System.err
                        .printf("Skipping trkpt with invalid coordinates lat=%5.3f lon=%5.3f\n",
                                lattmp, lontmp);
                continue;
            }
            longi[i] = lontmp;
            lati[i] = lattmp;
            time[i] = wpt.getTime().getTime();
            ele = wpt.getEle();
            if (ele != null) {
                elev.put(time[i], ele);
            }
            i++;
        }
        ele = null;
        final double tmin = time[0];
        final double tmax = time[n - 1];

        try {
            fLongi.addSegmentFunction(new SegmentFunction(tmin, tmax,
                    interpolator.interpolate(time, longi)));
            fLati.addSegmentFunction(new SegmentFunction(tmin, tmax,
                    interpolator.interpolate(time, lati)));
            addEle(tmin, tmax, elev);
        } catch (MathException ex) {
            throw new IllegalArgumentException(ex);
        }
    }

    private void addEle(double tmin, double tmax,
            SortedMap<Double, BigDecimal> elev) throws MathException {
        final int ne = elev.size();
        if (ne == 0) {
            // no elevation info
            fEle.addSegmentFunction(new SegmentFunction(tmin, tmax,
                    new ConstantFunction(BigDecimal.ZERO)));
            return;
        }
        final double ti0 = elev.firstKey().doubleValue();
        final double tin = elev.lastKey().doubleValue();
        if (elev.get(tmin) == null) {
            // elevation data missing at start
            fEle.addSegmentFunction(new SegmentFunction(tmin, ti0,
                    new ConstantFunction(elev.get(ti0))));
        }
        if (elev.get(tmax) == null) {
            // elevation data missing at end
            fEle.addSegmentFunction(new SegmentFunction(tin, tmax,
                    new ConstantFunction(elev.get(tin))));
        }
        switch (ne) {
        case 1:
            // only one point with elevation data
            // nothing more to do
            break;
        case 2:
            // two points with elevation data
            fEle.addSegmentFunction(ti0, tin,
                    new LinearFunction(ti0, elev.get(ti0).doubleValue(), tin,
                            elev.get(tin).doubleValue()));
            break;
        default:
            final double[] eleTime = new double[ne];
            final double[] eleVal = new double[ne];
            int i = 0;
            for (Double d : elev.keySet()) {
                eleTime[i] = d;
                eleVal[i] = elev.get(d).doubleValue();
                i++;
            }
            fEle.addSegmentFunction(new SegmentFunction(ti0, tin, interpolator
                    .interpolate(eleTime, eleVal)));
        }

    }

    public synchronized Wpt getWpt(Date date)
            throws FunctionEvaluationException {
        final double millis = date.getTime();
        final Wpt wpt = new Wpt();
        wpt.setTime(date);
        wpt.setLat(new BigDecimal(fLati.value(millis)));
        wpt.setLon(new BigDecimal(fLongi.value(millis)));
        wpt.setEle(new BigDecimal(fEle.value(millis)));
        return wpt;
    }

    public void printWpt(PrintStream ps, Wpt wpt) {
        if (wpt.getName() == null) {
            ps.print("WAYPOINT");
        } else {
            ps.print(wpt.getName());
        }
        ps.print(" ");
        if (wpt.getTime() == null) {
            ps.print("-");
        } else {
            synchronized (ISO_UTC) {
                ps.print(ISO_UTC.format(wpt.getTime()) + " ");
            }
        }
        ps.printf(Locale.US, "%2.6f", wpt.getLat());
        ps.print(" ");
        ps.printf(Locale.US, "%2.6f", wpt.getLon());
        ps.print(" ");
        BigDecimal ele = wpt.getEle();
        if (ele == null) {
            ps.print("-");
        } else {
            ps.printf(Locale.US, "%1.0f", wpt.getEle());
        }
        ps.println();
        ps.flush();
    }

    /*
     * public static void main(String[] argv) throws Exception{ Reader input =
     * new BufferedReader(new FileReader(argv[0])); GpxType gpx =
     * Gpx.unmarshal(input); TrackInterpolator interp = new TrackInterpolator();
     * final DateFormat EXIF_DATE_FORMAT = new
     * SimpleDateFormat("yyyy:MM:dd' 'HH:mm:ss"); interp.addAllTracks(gpx);
     * for(int i=1; i<argv.length; i++){ Wpt wpt; System.out.print(argv[i]);
     * System.out.print(" "); try{ wpt =
     * interp.getWpt(EXIF_DATE_FORMAT.parse(argv[i]));
     * interp.printWpt(System.out, wpt); } catch (Exception ex){
     * System.out.println(ex.getClass().getName() + ": " + ex.getMessage()); } }
     * }
     */

}
TOP

Related Classes of net.sourceforge.gpstools.TrackInterpolator

TOP
Copyright © 2018 www.massapi.com. All rights reserved.
All source code are property of their respective owners. Java is a trademark of Sun Microsystems, Inc and owned by ORACLE Inc. Contact coftware#gmail.com.