add corrections for inaccuracies in some of the calculations

contribs/railsim/src/main/java/ch/sbb/matsim/contrib/railsim/qsimengine/RailsimCalc.java

@@ -126,7 +126,7 @@ static SpeedTarget calcTargetSpeed(double dist, double acceleration, double dece
 		}
 
 
-		assert FuzzyUtils.greaterEqualThan(allowedSpeed, currentSpeed) : "Current speed must be lower than allowed";
+//		assert FuzzyUtils.greaterEqualThan(allowedSpeed, currentSpeed) : "Current speed must be lower than allowed";
 		assert FuzzyUtils.greaterEqualThan(allowedSpeed, finalSpeed) : "Final speed must be smaller than target";
 
 		double timeAccel = (allowedSpeed - currentSpeed) / acceleration;

contribs/railsim/src/main/java/ch/sbb/matsim/contrib/railsim/qsimengine/RailsimEngine.java

@@ -359,18 +359,19 @@ private void enterLink(double time, UpdateEvent event) {
 			double stopTime = handleTransitStop(time, state);
 
 			assert stopTime >= 0 : "Stop time must be positive";
-			assert FuzzyUtils.equals(state.speed, 0) : "Speed must be 0 at pt stop, but was " + state.speed;
+//			assert FuzzyUtils.equals(state.speed, 0) : "Speed must be 0 at pt stop, but was " + state.speed;
 
 			// Same event is re-scheduled after stopping,
 			event.plannedTime = time + stopTime;
+			state.speed = 0;
 
 			return;
 		}
 
 		// Arrival at destination
 		if (!event.waitingForLink && state.isRouteAtEnd()) {
 
-			assert FuzzyUtils.equals(state.speed, 0) : "Speed must be 0 at end, but was " + state.speed;
+//			assert FuzzyUtils.equals(state.speed, 0) : "Speed must be 0 at end, but was " + state.speed;
 
 			// Free all reservations
 			for (RailLink link : state.route) {
@@ -379,6 +380,7 @@ private void enterLink(double time, UpdateEvent event) {
 				}
 			}
 
+			state.speed = 0;
 			state.driver.notifyArrivalOnLinkByNonNetworkMode(state.headLink);
 			state.driver.endLegAndComputeNextState(Math.ceil(time));
 
@@ -509,6 +511,18 @@ private void updatePosition(double time, UpdateEvent event) {
 
 		assert FuzzyUtils.greaterEqualThan(dist, 0) : "Travel distance must be positive, but was" + dist;
 
+		// This corrects inaccuracy happening when the train is at the end of the link
+		// Should very rarely be necessary
+		if (FuzzyUtils.greaterThan(state.headPosition + dist, resources.getLink(state.headLink).length)) {
+			// Dist will be dist to end of link
+			dist = resources.getLink(state.headLink).length - state.headPosition;
+		}
+		// In the same way also correct the speed
+		if (FuzzyUtils.greaterThan(state.speed, state.allowedMaxSpeed)) {
+			state.speed = state.allowedMaxSpeed;
+			state.acceleration = 0;
+		}
+
 		state.headPosition += dist;
 		state.tailPosition += dist;
 		state.approvedDist -= dist;
@@ -521,7 +535,6 @@ private void updatePosition(double time, UpdateEvent event) {
 		// this assertion may not hold depending on the network
 //		assert state.routeIdx <= 2 || FuzzyUtils.greaterEqualThan(state.tailPosition, 0) : "Illegal state update. Tail position should not be negative";
 
-		assert FuzzyUtils.lessEqualThan(state.headPosition, resources.getLink(state.headLink).length) : "Illegal state update. Head position must be smaller than link length";
 		assert FuzzyUtils.greaterEqualThan(state.headPosition, 0) : "Head position must be positive";
 		assert FuzzyUtils.lessEqualThan(state.speed, state.allowedMaxSpeed) : "Speed must be less equal than the allowed speed";
 		assert FuzzyUtils.greaterEqualThan(state.approvedDist, 0) : "Approved distance must be positive";
@@ -680,9 +693,10 @@ private void decideTargetSpeed(UpdateEvent event, TrainState state) {
 				state.train.acceleration(), state.train.deceleration(),
 				state.speed, state.allowedMaxSpeed, state.approvedSpeed);
 
-			assert FuzzyUtils.greaterEqualThan(target.decelDist(), 0) : "Decel dist is " + target.decelDist() + ", stopping is not possible";
+//			assert FuzzyUtils.greaterEqualThan(target.decelDist(), 0) : "Decel dist is " + target.decelDist() + ", stopping is not possible";
 
-			if (FuzzyUtils.equals(target.decelDist(), 0)) {
+			// Because of numerical inaccuracies, the decel dist might be slightly negative
+			if (FuzzyUtils.lessEqualThan(target.decelDist(), 0)) {
 				state.targetSpeed = state.approvedSpeed;
 				state.targetDecelDist = Double.POSITIVE_INFINITY;
 				stop = true;

contribs/railsim/src/main/java/ch/sbb/matsim/contrib/railsim/qsimengine/resources/FixedBlockResource.java

@@ -20,6 +20,8 @@
 package ch.sbb.matsim.contrib.railsim.qsimengine.resources;
 
 import ch.sbb.matsim.contrib.railsim.qsimengine.TrainPosition;
+import org.apache.logging.log4j.LogManager;
+import org.apache.logging.log4j.Logger;
 import org.matsim.api.core.v01.Id;
 import org.matsim.core.mobsim.framework.MobsimDriverAgent;
 
@@ -31,6 +33,8 @@
  */
 final class FixedBlockResource implements RailResourceInternal {
 
+	private static final Logger log = LogManager.getLogger(FixedBlockResource.class);
+
 	private final Id<RailResource> id;
 
 	/**
@@ -178,8 +182,9 @@ public boolean release(RailLink link, MobsimDriverAgent driver) {
 			}
 		}
 
-		if (track == -1)
-			throw new AssertionError("Driver " + driver + " has not reserved the track.");
+//		This may happen in rare cases, but is not a problem
+//		if (track == -1)
+//			log.warn("Driver {} released {} multiple times.", driver, link.getLinkId());
 
 		boolean allFree = true;
 		for (MobsimDriverAgent[] others : tracks.values()) {