@Override
protected MoveProgress doFollowPath(MovingRoadUser object, Queue<Point> path,
TimeLapse time) {
final long startTimeConsumed = time.getTimeConsumed();
Point loc = objLocs.get(object);
final UnitConverter toInternalTimeConv = time.getTimeUnit().getConverterTo(
INTERNAL_TIME_UNIT);
final UnitConverter toExternalTimeConv = INTERNAL_TIME_UNIT
.getConverterTo(time.getTimeUnit());
double traveled = 0;
final double speed = min(toInternalSpeedConv.convert(object.getSpeed()),
maxSpeed);
if (speed == 0d) {
// FIXME add test for this case, also check GraphRoadModel
final Measure<Double, Length> dist = Measure.valueOf(0d,
externalDistanceUnit);
final Measure<Long, Duration> dur = Measure.valueOf(0L,
time.getTimeUnit());
return new MoveProgress(dist, dur, new ArrayList<Point>());
}
final List<Point> travelledNodes = new ArrayList<Point>();
while (time.hasTimeLeft() && path.size() > 0) {
checkArgument(isPointInBoundary(path.peek()),
"points in the path must be within the predefined boundary of the plane");
// distance in internal time unit that can be traveled with timeleft
final double travelDistance = speed
* toInternalTimeConv.convert(time.getTimeLeft());
final double stepLength = toInternalDistConv.convert(Point.distance(loc,
path.peek()));
if (travelDistance >= stepLength) {
loc = path.remove();
travelledNodes.add(loc);
final long timeSpent = DoubleMath.roundToLong(
toExternalTimeConv.convert(stepLength / speed),
RoundingMode.HALF_DOWN);
time.consume(timeSpent);
traveled += stepLength;
} else {
final Point diff = Point.diff(path.peek(), loc);
if (stepLength - travelDistance < DELTA) { // 0.00000001) {
loc = path.peek();
traveled += stepLength;
} else {
final double perc = travelDistance / stepLength;
loc = new Point(loc.x + perc * diff.x, loc.y + perc * diff.y);
traveled += travelDistance;
}
time.consumeAll();
}