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()); } }
* }
*/
}