added Track.getInterpolatedValue, preparing for re-use across NMEA and Igtimi connector

Change-Id: Ia956d8ca0d9e40f275ade9f2c3764887e0de195a
This commit is contained in:
Axel Uhl committed 2017-04-21 17:18:41 +02:00
1 parent 1026fcf044
commit 82da2ccb51
5 files changed
+102 -9

No files matched your search

@@ -250,7 +250,7 @@ public class IgtimiWindReceiver implements BulkFixReceiver {
if (awaPair.getB() == null) {
awaFrom = awaPair.getA().getApparentWindAngle();
} else {
awaFrom = timeBasedAgerage(timePoint,
awaFrom = timeBasedAverage(timePoint,
new ScalableBearing(awaPair.getA().getApparentWindAngle()), awaPair.getA().getTimePoint(),
new ScalableBearing(awaPair.getB().getApparentWindAngle()), awaPair.getB().getTimePoint());
}
@@ -270,7 +270,7 @@ public class IgtimiWindReceiver implements BulkFixReceiver {
if (awsPair.getB() == null) {
aws = awsPair.getA().getApparentWindSpeed();
} else {
aws = timeBasedAgerage(timePoint,
aws = timeBasedAverage(timePoint,
new ScalableSpeed(awsPair.getA().getApparentWindSpeed()), awsPair.getA().getTimePoint(),
new ScalableSpeed(awsPair.getB().getApparentWindSpeed()), awsPair.getB().getTimePoint());
}
@@ -290,7 +290,7 @@ public class IgtimiWindReceiver implements BulkFixReceiver {
if (sogPair.getB() == null) {
sog = sogPair.getA().getSpeedOverGround();
} else {
sog = timeBasedAgerage(timePoint,
sog = timeBasedAverage(timePoint,
new ScalableSpeed(sogPair.getA().getSpeedOverGround()), sogPair.getA().getTimePoint(),
new ScalableSpeed(sogPair.getB().getSpeedOverGround()), sogPair.getB().getTimePoint());
}
@@ -310,7 +310,7 @@ public class IgtimiWindReceiver implements BulkFixReceiver {
if (cogPair.getB() == null) {
sog = cogPair.getA().getCourseOverGround();
} else {
sog = timeBasedAgerage(timePoint,
sog = timeBasedAverage(timePoint,
new ScalableBearing(cogPair.getA().getCourseOverGround()), cogPair.getA().getTimePoint(),
new ScalableBearing(cogPair.getB().getCourseOverGround()), cogPair.getB().getTimePoint());
}
@@ -330,7 +330,7 @@ public class IgtimiWindReceiver implements BulkFixReceiver {
if (gpsPair.getB() == null) {
pos = gpsPair.getA().getPosition();
} else {
pos = timeBasedAgerage(timePoint, new ScalablePosition(gpsPair.getA().getPosition()), gpsPair.getA()
pos = timeBasedAverage(timePoint, new ScalablePosition(gpsPair.getA().getPosition()), gpsPair.getA()
.getTimePoint(), new ScalablePosition(gpsPair.getB().getPosition()), gpsPair.getB()
.getTimePoint());
}
@@ -350,7 +350,7 @@ public class IgtimiWindReceiver implements BulkFixReceiver {
if (hdgPair.getB() == null) {
hdg = hdgPair.getA().getTrueHeading();
} else {
hdg = timeBasedAgerage(timePoint,
hdg = timeBasedAverage(timePoint,
new ScalableBearing(hdgPair.getA().getTrueHeading()), hdgPair.getA().getTimePoint(),
new ScalableBearing(hdgPair.getB().getTrueHeading()), hdgPair.getB().getTimePoint());
}
@@ -370,7 +370,7 @@ public class IgtimiWindReceiver implements BulkFixReceiver {
if (hdgmPair.getB() == null) {
hdgm = hdgmPair.getA().getMagnetigHeading();
} else {
hdgm = timeBasedAgerage(timePoint,
hdgm = timeBasedAverage(timePoint,
new ScalableBearing(hdgmPair.getA().getMagnetigHeading()), hdgmPair.getA().getTimePoint(),
new ScalableBearing(hdgmPair.getB().getMagnetigHeading()), hdgmPair.getB().getTimePoint());
}
@@ -405,7 +405,7 @@ public class IgtimiWindReceiver implements BulkFixReceiver {
return trueHeading;
}
private <V, T> T timeBasedAgerage(TimePoint timePoint, ScalableValue<V, T> value1, TimePoint timePoint1, ScalableValue<V, T> value2, TimePoint timePoint2) {
private <V, T> T timeBasedAverage(TimePoint timePoint, ScalableValue<V, T> value1, TimePoint timePoint1, ScalableValue<V, T> value2, TimePoint timePoint2) {
final T acc;
if (timePoint1.equals(timePoint2)) {
acc = value1.add(value2).divide(2);
@@ -17,6 +17,8 @@ import com.sap.sailing.domain.abstractlog.race.RaceLogEventVisitor;
import com.sap.sailing.domain.common.abstractlog.NotRevokableException;
import com.sap.sse.common.Duration;
import com.sap.sse.common.TimePoint;
import com.sap.sse.common.Util.Function;
import com.sap.sse.common.scalablevalue.ScalableValue;
/**
* Wrapper for a {@link RaceLog} which will ignore all calls trying to add an {@link RaceLogEvent}. All other
@@ -250,4 +252,10 @@ public class NoAddingRaceLogWrapper implements RaceLog {
public void revokeEvent(AbstractLogEventAuthor author, RaceLogEvent toRevoke) throws NotRevokableException {
innerRaceLog.revokeEvent(author, toRevoke);
}
@Override
public <InternalType, ValueType> ValueType getInterpolatedValue(TimePoint timePoint,
Function<RaceLogEvent, ScalableValue<InternalType, ValueType>> converter) {
return innerRaceLog.getInterpolatedValue(timePoint, converter);
}
}
@@ -4,9 +4,12 @@ import java.io.Serializable;
import java.util.Iterator;
import java.util.concurrent.locks.ReadWriteLock;
import com.sap.sailing.domain.common.tracking.GPSFixMoving;
import com.sap.sse.common.Duration;
import com.sap.sse.common.TimePoint;
import com.sap.sse.common.Timed;
import com.sap.sse.common.Util.Function;
import com.sap.sse.common.scalablevalue.ScalableValue;
/**
* A track records {@link Timed} items for an object of type <code>ItemType</code>. It allows clients to ask for a value
@@ -117,6 +120,27 @@ public interface Track<FixType extends Timed> extends Serializable {
*/
FixType getLastRawFix();
/**
* Interpolates an aspect of the fixes in this track for a given {@code timePoint}. If {@code timePoint} matches
* exactly a fix in this track, that fix is used. If this track is empty, {@code null} is returned. If the
* {@code timePoint} is after the last fix of this track, the last fix is used; if before the first fix, the first
* fix is used.
* <p>
*
* The fix(es) are converted to {@link ScalableValue}s using the {@code converter} which gives callers a choice
* which aspect of the fixes to project and interpolate. If more than one value results because two fixes (one
* before, one after) are used, linear interpolation based on the fixes' time points takes place.
* <p>
*
* Example: for a track of {@link GPSFixMoving} fixes the course over ground shall be determined for a given time
* point. The call would look like this:
* {@code getInterpolatedValue(timePoint, f->new ScalableBearing(f.getSpeed().getBearing()))}
*
* @return the projected interpolated value, typed by what the {@link ScalableValue#divide(double)} method returns.
*/
<InternalType, ValueType> ValueType getInterpolatedValue(TimePoint timePoint,
Function<FixType, ScalableValue<InternalType, ValueType>> converter);
/**
* Returns an iterator starting at the first fix after <code>startingAt</code> (or "at or after" in case
* <code>inclusive</code> is <code>true</code>). The fixes returned by the iterator are the smoothened fixes (see
@@ -184,4 +208,5 @@ public interface Track<FixType extends Timed> extends Serializable {
* more than one raw fix in the track
*/
Duration getAverageIntervalBetweenRawFixes();
}
@@ -10,6 +10,9 @@ import com.sap.sailing.domain.tracking.Track;
import com.sap.sse.common.Duration;
import com.sap.sse.common.TimePoint;
import com.sap.sse.common.Timed;
import com.sap.sse.common.Util.Function;
import com.sap.sse.common.Util.Pair;
import com.sap.sse.common.scalablevalue.ScalableValue;
import com.sap.sse.concurrent.LockUtil;
import com.sap.sse.concurrent.NamedReentrantReadWriteLock;
import com.sap.sse.util.impl.ArrayListNavigableSet;
@@ -251,6 +254,47 @@ public class TrackImpl<FixType extends Timed> implements Track<FixType> {
}
}
private Pair<FixType, FixType> getSurroundingFixes(TimePoint timePoint) {
FixType left = getLastFixAtOrBefore(timePoint);
FixType right = getFirstFixAtOrAfter(timePoint);
com.sap.sse.common.Util.Pair<FixType, FixType> result = new com.sap.sse.common.Util.Pair<>(left, right);
return result;
}
private <V, T> T timeBasedAverage(TimePoint timePoint, ScalableValue<V, T> value1, TimePoint timePoint1, ScalableValue<V, T> value2, TimePoint timePoint2) {
final T acc;
if (timePoint1.equals(timePoint2)) {
acc = value1.add(value2).divide(2);
} else {
long timeDiff1 = Math.abs(timePoint1.asMillis() - timePoint.asMillis());
long timeDiff2 = Math.abs(timePoint2.asMillis() - timePoint.asMillis());
acc = value1.multiply(timeDiff2).add(value2.multiply(timeDiff1)).divide(timeDiff1 + timeDiff2);
}
return acc;
}
@Override
public <InternalType, ValueType> ValueType getInterpolatedValue(TimePoint timePoint, Function<FixType, ScalableValue<InternalType, ValueType>> converter) {
final ValueType aws;
Pair<FixType, FixType> awsPair = getSurroundingFixes(timePoint);
if (awsPair.getA() == null) {
if (awsPair.getB() == null) {
aws = null;
} else {
aws = converter.get(awsPair.getB()).divide(1);
}
} else {
if (awsPair.getB() == null) {
aws = converter.get(awsPair.getA()).divide(1);
} else {
aws = timeBasedAverage(timePoint,
converter.get(awsPair.getA()), awsPair.getA().getTimePoint(),
converter.get(awsPair.getB()), awsPair.getB().getTimePoint());
}
}
return aws;
}
@Override
public Iterator<FixType> getFixesIterator(TimePoint startingAt, boolean inclusive) {
assertReadLock();
@@ -41,12 +41,14 @@ import com.sap.sailing.domain.common.impl.KnotSpeedImpl;
import com.sap.sailing.domain.common.impl.KnotSpeedWithBearingImpl;
import com.sap.sailing.domain.common.impl.MeterDistance;
import com.sap.sailing.domain.common.impl.MeterPerSecondSpeedWithDegreeBearingImpl;
import com.sap.sailing.domain.common.scalablevalue.impl.ScalableBearing;
import com.sap.sailing.domain.common.scalablevalue.impl.ScalableSpeed;
import com.sap.sailing.domain.common.tracking.GPSFix;
import com.sap.sailing.domain.common.tracking.GPSFixMoving;
import com.sap.sailing.domain.common.tracking.impl.VeryCompactGPSFixMovingImpl;
import com.sap.sailing.domain.common.tracking.impl.CompactionNotPossibleException;
import com.sap.sailing.domain.common.tracking.impl.GPSFixImpl;
import com.sap.sailing.domain.common.tracking.impl.GPSFixMovingImpl;
import com.sap.sailing.domain.common.tracking.impl.VeryCompactGPSFixMovingImpl;
import com.sap.sailing.domain.tracking.DynamicGPSFixTrack;
import com.sap.sailing.domain.tracking.GPSFixTrack;
import com.sap.sailing.domain.tracking.impl.DistanceCache;
@@ -109,6 +111,20 @@ public class TrackTest {
track.addGPSFix(gpsFix5);
}
@Test
public void bearingInterpolationTest() {
TimePoint betweenFirstAndSecond = gpsFix1.getTimePoint().plus(gpsFix1.getTimePoint().until(gpsFix2.getTimePoint()).divide(2));
Bearing bearingBetweenFirstAndSecond = track.getInterpolatedValue(betweenFirstAndSecond, f->new ScalableBearing(f.getSpeed().getBearing()));
assertEquals(bearingBetweenFirstAndSecond.getDegrees(), gpsFix1.getSpeed().getBearing().middle(gpsFix2.getSpeed().getBearing()).getDegrees(), 0.00001);
}
@Test
public void speedInterpolationTest() {
TimePoint betweenFirstAndSecond = gpsFix1.getTimePoint().plus(gpsFix1.getTimePoint().until(gpsFix2.getTimePoint()).divide(2));
Speed speedBetweenFirstAndSecond = track.getInterpolatedValue(betweenFirstAndSecond, f->new ScalableSpeed(f.getSpeed()));
assertEquals(speedBetweenFirstAndSecond.getKnots(), (gpsFix1.getSpeed().getKnots()+gpsFix2.getSpeed().getKnots())/2, 0.01);
}
/**
* Tests for a incorrect method of estimating Positions
*/