implementation and performance testing of the SFMM. Stride information in...

implementation and performance testing of the SFMM. Stride information in pedestrianOSM will now be cleared to avoid large linked lists.

VadereModelTests/TestOSM/scenarios/basic_1_chicken_osm1.scenario

@@ -1194,6 +1194,78 @@
           } ]
         },
         "id" : -1
+      }, {
+        "shape" : {
+          "type" : "POLYGON",
+          "points" : [ {
+            "x" : -1.0E-4,
+            "y" : -1.0E-4
+          }, {
+            "x" : 35.0001,
+            "y" : -1.0E-4
+          }, {
+            "x" : 35.0001,
+            "y" : 0.5001
+          }, {
+            "x" : -1.0E-4,
+            "y" : 0.5001
+          } ]
+        },
+        "id" : -1
+      }, {
+        "shape" : {
+          "type" : "POLYGON",
+          "points" : [ {
+            "x" : 35.0001,
+            "y" : -1.0E-4
+          }, {
+            "x" : 35.0001,
+            "y" : 60.0001
+          }, {
+            "x" : 34.4999,
+            "y" : 60.0001
+          }, {
+            "x" : 34.4999,
+            "y" : -1.0E-4
+          } ]
+        },
+        "id" : -1
+      }, {
+        "shape" : {
+          "type" : "POLYGON",
+          "points" : [ {
+            "x" : 35.0001,
+            "y" : 60.0001
+          }, {
+            "x" : -1.0E-4,
+            "y" : 60.0001
+          }, {
+            "x" : -1.0E-4,
+            "y" : 59.4999
+          }, {
+            "x" : 35.0001,
+            "y" : 59.4999
+          } ]
+        },
+        "id" : -1
+      }, {
+        "shape" : {
+          "type" : "POLYGON",
+          "points" : [ {
+            "x" : -1.0E-4,
+            "y" : 60.0001
+          }, {
+            "x" : -1.0E-4,
+            "y" : -1.0E-4
+          }, {
+            "x" : 0.5001,
+            "y" : -1.0E-4
+          }, {
+            "x" : 0.5001,
+            "y" : 60.0001
+          } ]
+        },
+        "id" : -1
       } ],
       "stairs" : [ ],
       "targets" : [ {

VadereSimulator/src/org/vadere/simulator/models/osm/OptimalStepsModel.java

@@ -199,18 +199,19 @@ public class OptimalStepsModel implements MainModel, PotentialFieldModel {
 	@Override
 	public void update(final double simTimeInSec) {
 
+		final double timeStepInSec = simTimeInSec - this.lastSimTimeInSec;
 		// event driven update
 		if (attributesOSM.getUpdateType() == UpdateType.EVENT_DRIVEN
 				&& !pedestrianEventsQueue.isEmpty()) {
 			while (pedestrianEventsQueue.peek().getTimeOfNextStep() < simTimeInSec) {
 				PedestrianOSM ped = pedestrianEventsQueue.poll();
-				ped.update(-1, simTimeInSec, CallMethod.EVENT_DRIVEN);
+				ped.update(timeStepInSec, simTimeInSec, CallMethod.EVENT_DRIVEN);
 				pedestrianEventsQueue.add(ped);
 			}
 
 		} else {
 			// time step length
-			final double timeStepInSec = simTimeInSec - this.lastSimTimeInSec;
+
 
 			// parallel update
 			if (attributesOSM.getUpdateType() == UpdateType.PARALLEL) {

VadereSimulator/src/org/vadere/simulator/models/osm/PedestrianOSM.java

 package org.vadere.simulator.models.osm;
 
+import org.apache.commons.lang3.tuple.Pair;
 import org.vadere.simulator.models.SpeedAdjuster;
 import org.vadere.simulator.models.osm.optimization.StepCircleOptimizer;
 import org.vadere.simulator.models.osm.stairOptimization.StairStepOptimizer;
@@ -56,7 +57,7 @@ public class PedestrianOSM extends Pedestrian {
 	// calculated by (current position - last position)/(period of time).
 	private double speedByAbsoluteDistance;
 
-	private LinkedList<Double>[] strides;    // strides[0] = length strides[1] = time
+	private LinkedList<Pair<Double, Double>> strides; // left = length, right = time
 	private StairStepOptimizer stairStepOptimizer;
 
 	@SuppressWarnings("unchecked")
@@ -94,9 +95,7 @@ public class PedestrianOSM extends Pedestrian {
 			this.minStepLength = 0;
 		}
 
-		this.strides = (LinkedList<Double>[]) (new LinkedList<?>[2]);
-		this.strides[0] = new LinkedList<>();
-		this.strides[1] = new LinkedList<>();
+		this.strides = new LinkedList<>();
 	}
 
 	private static UpdateSchemeOSM createUpdateScheme(UpdateType updateType, PedestrianOSM pedestrian) {
@@ -121,9 +120,14 @@ public class PedestrianOSM extends Pedestrian {
 	}
 
 	public void update(double timeStepInSec, double currentTimeInSec, CallMethod callMethod) {
+		double lastSimTimeInSec = currentTimeInSec - timeStepInSec;
 
-		this.updateScheme.update(timeStepInSec, currentTimeInSec, callMethod);
+		// clear the old strides to avoid large linked lists
+		if(!strides.isEmpty() && lastSimTimeInSec < strides.peekFirst().getRight()) {
+			clearStrides();
+		}
 
+		this.updateScheme.update(timeStepInSec, currentTimeInSec, callMethod);
 	}
 
 	public void updateNextPosition() {
@@ -183,10 +187,9 @@ public class PedestrianOSM extends Pedestrian {
 			// compute velocity by forward difference
 			setVelocity(new Vector2D(nextPosition.x - currentPosition.x,
 					nextPosition.y - currentPosition.y).multiply(1.0 / stepTime));
-
 		}
-		strides[0].add(currentPosition.distance(nextPosition));
-		strides[1].add(this.getTimeOfNextStep());
+
+		strides.add(Pair.of(currentPosition.distance(nextPosition), getTimeOfNextStep()));
 	}
 
 	public double getStepSize() {
@@ -224,8 +227,7 @@ public class PedestrianOSM extends Pedestrian {
 	}
 
 	public void clearStrides() {
-		strides[0].clear();
-		strides[1].clear();
+		strides.clear();
 	}
 
 	// Getters...
@@ -302,7 +304,7 @@ public class PedestrianOSM extends Pedestrian {
 		return attributesOSM;
 	}
 
-	public LinkedList<Double>[] getStrides() {
+	public LinkedList<Pair<Double, Double>> getStrides() {
 		return strides;
 	}
 

VadereSimulator/src/org/vadere/simulator/models/potential/PotentialFieldObstacleCompactSoftshell.java

@@ -5,6 +5,8 @@ import java.util.LinkedList;
 import java.util.List;
 import java.util.Random;
 
+import org.apache.log4j.LogManager;
+import org.apache.log4j.Logger;
 import org.vadere.simulator.models.potential.fields.PotentialFieldObstacle;
 import org.vadere.state.attributes.Attributes;
 import org.vadere.state.attributes.models.AttributesPotentialCompactSoftshell;
@@ -19,7 +21,7 @@ import org.vadere.util.math.MathUtil;
 
 public class PotentialFieldObstacleCompactSoftshell implements PotentialFieldObstacle {
 
-
+	private static Logger log = LogManager.getLogger(PotentialFieldObstacleCompactSoftshell.class);
 	private AttributesPotentialCompactSoftshell attributes;
 	private Random random;
 	private double width;

VadereSimulator/src/org/vadere/simulator/models/potential/fields/ObstacleDistancePotential.java

@@ -17,6 +17,7 @@ import org.vadere.util.potential.calculators.EikonalSolver;
 import org.vadere.util.potential.calculators.EikonalSolverFIM;
 import org.vadere.util.potential.calculators.EikonalSolverFMM;
 import org.vadere.util.potential.calculators.EikonalSolverFSM;
+import org.vadere.util.potential.calculators.EikonalSolverSFMM;
 import org.vadere.util.potential.calculators.PotentialFieldCalculatorNone;
 import org.vadere.util.potential.timecost.UnitTimeCostFunction;
 
@@ -54,7 +55,7 @@ public class ObstacleDistancePotential implements IPotentialField {
 				eikonalSolver = new EikonalSolverFSM(cellGrid, obstacles, isHighAccuracyFM, new UnitTimeCostFunction());
 				break;
 			default:
-				eikonalSolver = new EikonalSolverFMM(cellGrid, obstacles, isHighAccuracyFM, new UnitTimeCostFunction());
+				eikonalSolver = new EikonalSolverSFMM(cellGrid, obstacles, isHighAccuracyFM, new UnitTimeCostFunction());
 		}
 
 		long ms = System.currentTimeMillis();

VadereSimulator/src/org/vadere/simulator/models/potential/fields/PotentialFieldAndInitializer.java

@@ -84,7 +84,7 @@ class PotentialFieldAndInitializer {
 				eikonalSolver = new EikonalSolverFSM(cellGrid, targetShapes, isHighAccuracyFM, timeCost);
 				break;
 			default:
-				eikonalSolver = new EikonalSolverFMM(cellGrid, targetShapes, isHighAccuracyFM, timeCost);
+				eikonalSolver = new EikonalSolverSFMM(cellGrid, targetShapes, isHighAccuracyFM, timeCost);
 		}
 
 		long ms = System.currentTimeMillis();

VadereSimulator/src/org/vadere/simulator/projects/dataprocessing/processor/PedestrianOSMStrideLengthProcessor.java

 package org.vadere.simulator.projects.dataprocessing.processor;
 
+import org.apache.commons.lang3.tuple.Pair;
 import org.vadere.simulator.control.SimulationState;
 import org.vadere.simulator.models.Model;
 import org.vadere.simulator.models.osm.OptimalStepsModel;
@@ -34,8 +35,8 @@ public class PedestrianOSMStrideLengthProcessor extends DataProcessor<TimestepPe
 			List<PedestrianOSM> osmPeds = peds.stream().map(p -> ((PedestrianOSM) p)).collect(Collectors.toList());
 			osmPeds.forEach(ped -> {
 
-				LinkedList<Double> strideLengths = ped.getStrides()[0];
-				double length = strideLengths.isEmpty() ? 0.0 : strideLengths.getLast();
+				LinkedList<Pair<Double, Double>> strideLengths = ped.getStrides();
+				double length = strideLengths.isEmpty() ? 0.0 : strideLengths.getLast().getLeft();
 
 				this.putValue(new TimestepPedestrianIdKey(state.getStep(), ped.getId()), length);
 			});

VadereSimulator/tests/org/vadere/simulator/projects/dataprocessing/processor/PedestrianOSMStrideLengthProcessorTestEnv.java

 package org.vadere.simulator.projects.dataprocessing.processor;
 
+import org.apache.commons.lang3.tuple.Pair;
 import org.mockito.Mockito;
 import org.vadere.simulator.models.MainModel;
 import org.vadere.simulator.models.osm.OptimalStepsModel;
@@ -82,10 +83,10 @@ public class PedestrianOSMStrideLengthProcessorTestEnv extends ProcessorTestEnv<
 	private PedestrianOSM getPedMock(int id, Double... strides) {
 		PedestrianOSM ped = mock(PedestrianOSM.class, Mockito.RETURNS_DEEP_STUBS);
 		when(ped.getId()).thenReturn(id);
-		LinkedList<Double> strideList = new LinkedList<>(Arrays.asList(strides));
-		LinkedList<Double>[] tmp = (LinkedList<Double>[]) (new LinkedList<?>[2]);
-		tmp[0] = strideList;
-		when(ped.getStrides()).thenReturn(tmp);
+		LinkedList<Pair<Double, Double>> strideList = new LinkedList<>();
+
+		Arrays.stream(strides).forEach(stide -> strideList.add(Pair.of(stide, 0.2)));
+		when(ped.getStrides()).thenReturn(strideList);
 		return ped;
 	}
 

VadereUtils/src/org/vadere/util/math/MathUtil.java

@@ -12,6 +12,9 @@ import org.vadere.util.geometry.shapes.VPoint;
  * 
  */
 public class MathUtil {
+
+	private final static List<Point> neumannNeighborhood = getNeumannNeighborhood(new Point(0, 0));
+
 	/**
 	 * The two-norm of a vector.
 	 * 
@@ -340,12 +343,10 @@ public class MathUtil {
 	 */
 	public static List<Point> getNeumannNeighborhood(final Point p) {
 		List<Point> neumannNeighborhood = new LinkedList<>();
-
 		neumannNeighborhood.add(new Point(p.x - 1, p.y));
 		neumannNeighborhood.add(new Point(p.x + 1, p.y));
 		neumannNeighborhood.add(new Point(p.x, p.y - 1));
 		neumannNeighborhood.add(new Point(p.x, p.y + 1));
-
 		return neumannNeighborhood;
 	}
 
@@ -354,7 +355,7 @@ public class MathUtil {
 	 * neighborhood.
 	 */
 	public static List<Point> getRelativeNeumannNeighborhood() {
-		return getNeumannNeighborhood(new Point(0, 0));
+		return neumannNeighborhood;
 	}
 
 	/**
@@ -387,6 +388,28 @@ public class MathUtil {
 		return result;
 	}
 
+	/**
+	 * Returns the max real solutions of the quadratic equation ax^2+bx+c=0 or Double.MAX_VALUE if there is no solution.
+	 */
+	public static double solveQuadraticMax(double a, double b, double c) {
+		if (a != 0) {
+			double discr = (b * b) - (4 * a * c);
+
+			// one solution
+			if (discr == 0) {
+				return -b / (2.0 * a);
+			} else if (discr > 0) {
+				return Math.max((-b + Math.sqrt(discr)) / (2.0 * a), (-b - Math.sqrt(discr)) / (2.0 * a));
+			}
+		} else if (c != 0) {
+			return -c / b;
+		} else {
+			throw new IllegalArgumentException("ax^2 + bx + c = 0 is not a valid quadratic equation for a=b=0.");
+		}
+
+		return Double.MAX_VALUE;
+	}
+
 	/**
 	 * Returns true if the point p lies on the left of the vector defined by ab.
 	 */

VadereUtils/src/org/vadere/util/potential/CellGrid.java

@@ -288,14 +288,17 @@ public class CellGrid {
 				.flatMap(stream -> stream);
 	}
 
-	public boolean isValidPoint(Point point) {
-		Point p = (point);
+	public boolean isValidPoint(final Point point) {
+		return isValidPoint(point.x, point.y);
+	}
+
+	public boolean isValidPoint(final int x, final int y) {
 
-		if ((p.x < 0) || (p.x >= numPointsX)) {
+		if ((x < 0) || (x >= numPointsX)) {
 			return false;
 		}
 
-		if ((p.y < 0) || (p.y >= numPointsY)) {
+		if ((y < 0) || (y >= numPointsY)) {
 			return false;
 		}
 		return true;

VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolver.java

@@ -160,9 +160,17 @@ public interface EikonalSolver {
 		return cellGrid.isValidPoint(point);
 	}
 
+	default boolean isValidPoint(final int x, final int y, final CellGrid cellGrid) {
+		return cellGrid.isValidPoint(x, y);
+	}
+
 	default double computeGodunovDifference(final Point point, final CellGrid cellGrid, final Direction direction) {
+		return computeGodunovDifference(point.x, point.y, cellGrid, direction);
+	}
+
+	default double computeGodunovDifference(final int x, int y, final CellGrid cellGrid, final Direction direction) {
 
-		VPoint position = cellGrid.pointToCoord(point.x, point.y);
+		VPoint position = cellGrid.pointToCoord(x, y);
 		double cost = getTimeCostFunction().costAt(new VPoint(position.x, position.y));
 		double speed = (1.0 / cellGrid.getResolution()) / cost; // = F/cost
 
@@ -180,50 +188,50 @@ public interface EikonalSolver {
 		Point yhPoint;
 		switch (direction) {
 			case UP_LEFT:
-				xPoint = new Point(point.x - 1, point.y);
-				yPoint = new Point(point.x, point.y + 1);
-				xhPoint = new Point(point.x - 2, point.y);
-				yhPoint = new Point(point.x, point.y + 2);
+				xPoint = new Point(x - 1, y);
+				yPoint = new Point(x, y + 1);
+				xhPoint = new Point(x - 2, y);
+				yhPoint = new Point(x, y + 2);
 				break;
 			case UP_RIGHT:
-				xPoint = new Point(point.x + 1, point.y);
-				yPoint = new Point(point.x, point.y + 1);
-				xhPoint = new Point(point.x + 2, point.y);
-				yhPoint = new Point(point.x, point.y + 2);
+				xPoint = new Point(x + 1, y);
+				yPoint = new Point(x, y + 1);
+				xhPoint = new Point(x + 2, y);
+				yhPoint = new Point(x, y + 2);
 				break;
 			case BOTTOM_LEFT:
-				xPoint = new Point(point.x - 1, point.y);
-				yPoint = new Point(point.x, point.y - 1);
-				xhPoint = new Point(point.x - 2, point.y);
-				yhPoint = new Point(point.x, point.y - 2);
+				xPoint = new Point(x - 1, y);
+				yPoint = new Point(x, y - 1);
+				xhPoint = new Point(x - 2, y);
+				yhPoint = new Point(x, y - 2);
 				break;
 			case BOTTOM_RIGHT:
-				xPoint = new Point(point.x + 1, point.y);
-				yPoint = new Point(point.x, point.y - 1);
-				xhPoint = new Point(point.x + 2, point.y);
-				yhPoint = new Point(point.x, point.y - 2);
+				xPoint = new Point(x + 1, y);
+				yPoint = new Point(x, y - 1);
+				xhPoint = new Point(x + 2, y);
+				yhPoint = new Point(x, y - 2);
 				break;
 			default: {
-				if (isValidPoint(new Point(point.x + 1, point.y), cellGrid) &&
-						(!isValidPoint(new Point(point.x - 1, point.y), cellGrid)
-								|| (cellGrid.getValue(new Point(point.x + 1, point.y)).potential < cellGrid
-										.getValue(new Point(point.x - 1, point.y)).potential))) {
-					xPoint = new Point(point.x + 1, point.y);
-					xhPoint = new Point(point.x + 2, point.y);
+				if (isValidPoint(x + 1, y, cellGrid) &&
+						(!isValidPoint(x - 1, y, cellGrid)
+								|| (cellGrid.getValue(x + 1, y).potential < cellGrid
+								.getValue(x - 1, y).potential))) {
+					xPoint = new Point(x + 1, y);
+					xhPoint = new Point(x + 2, y);
 				} else {
-					xPoint = new Point(point.x - 1, point.y);
-					xhPoint = new Point(point.x - 2, point.y);
+					xPoint = new Point(x - 1, y);
+					xhPoint = new Point(x - 2, y);
 				}
 
-				if (isValidPoint(new Point(point.x, point.y + 1), cellGrid) &&
-						(!isValidPoint(new Point(point.x, point.y - 1), cellGrid)
-								|| (cellGrid.getValue(new Point(point.x, point.y + 1)).potential < cellGrid
-										.getValue(new Point(point.x, point.y - 1)).potential))) {
-					yPoint = new Point(point.x, point.y + 1);
-					yhPoint = new Point(point.x, point.y + 2);
+				if (isValidPoint(x, y + 1, cellGrid) &&
+						(!isValidPoint(x, y - 1, cellGrid)
+								|| (cellGrid.getValue(x, y + 1).potential < cellGrid
+								.getValue(x, y - 1).potential))) {
+					yPoint = new Point(x, y + 1);
+					yhPoint = new Point(x, y + 2);
 				} else {
-					yPoint = new Point(point.x, point.y - 1);
-					yhPoint = new Point(point.x, point.y - 2);
+					yPoint = new Point(x, y - 1);
+					yhPoint = new Point(x, y - 2);
 				}
 			}
 		}
@@ -270,24 +278,15 @@ public interface EikonalSolver {
 					c += factor * Math.pow(tp, 2);
 				}
 			}
-			java.util.List<Double> solutions = MathUtil.solveQuadratic(a, b, c);
-			int numberOfSolutions = solutions.size();
-
-			if (numberOfSolutions == 2) {
-				result = Math.max(solutions.get(0), solutions.get(1));
-			} else if (numberOfSolutions == 1) {
-				result = solutions.get(0);
-			}
+			return MathUtil.solveQuadraticMax(a, b, c);
 		}
 
 		return result;
 	}
 
-	default double computeGodunovDifference(final Point point, final CellGrid cellGrid) {
-		double result = Double.MAX_VALUE;
-
+	default double computeGodunovDifference(final int x, final int y, final CellGrid cellGrid) {
 		// enables cost fields with cost != 1
-		VPoint position = cellGrid.pointToCoord(point.x, point.y);
+		VPoint position = cellGrid.pointToCoord(x, y);
 		double cost = getTimeCostFunction().costAt(new VPoint(position.x, position.y));
 
 		double speed = (1.0 / cellGrid.getResolution()) / cost; // = F/cost
@@ -304,11 +303,11 @@ public interface EikonalSolver {
 			double val2 = Double.MAX_VALUE;
 
 			for (int i = 0; i < 2; i++) {
-				Point pni = new Point(point.x + neighbors.get(2 * j + i).x,
-						point.y + neighbors.get(2 * j + i).y);
+				Point pni = new Point(x + neighbors.get(2 * j + i).x,
+						y + neighbors.get(2 * j + i).y);
 				Point pni2 = new Point(
-						point.x + neighbors.get(2 * j + i).x * 2, point.y
-								+ neighbors.get(2 * j + i).y * 2);
+						x + neighbors.get(2 * j + i).x * 2, y
+						+ neighbors.get(2 * j + i).y * 2);
 
 				if (isValidPoint(pni, cellGrid) && cellGrid.getValue(pni).tag.frozen) {
 					double val1n = cellGrid.getValue(pni).potential;
@@ -342,16 +341,10 @@ public interface EikonalSolver {
 			}
 		}
 
-		java.util.List<Double> solutions = MathUtil
-				.solveQuadratic(coeff2, coeff1, coeff0);
-		int numberOfSolutions = solutions.size();
-
-		if (numberOfSolutions == 2) {
-			result = Math.max(solutions.get(0), solutions.get(1));
-		} else if (numberOfSolutions == 1) {
-			result = solutions.get(0);
-		}
+		return MathUtil.solveQuadraticMax(coeff2, coeff1, coeff0);
+	}
 
-		return result;
+	default double computeGodunovDifference(final Point point, final CellGrid cellGrid) {
+		return computeGodunovDifference(point.x, point.y, cellGrid);
 	}
 }

VadereUtils/src/org/vadere/util/potential/calculators/EikonalSolverSFMM.java

+package org.vadere.util.potential.calculators;
+
+import java.awt.Point;
+import java.util.Collection;
+import java.util.Comparator;
+import java.util.List;
+import java.util.PriorityQueue;
+import java.util.stream.Collectors;
+
+import org.apache.commons.math3.util.Pair;
+import org.apache.log4j.LogManager;
+import org.apache.log4j.Logger;
+import org.vadere.util.geometry.shapes.VPoint;
+import org.vadere.util.geometry.shapes.VShape;
+import org.vadere.util.math.MathUtil;
+import org.vadere.util.potential.CellGrid;
+import org.vadere.util.potential.CellState;
+import org.vadere.util.potential.PathFindingTag;
+import org.vadere.util.potential.timecost.ITimeCostFunction;
+
+/**
+ * EikonalSolverSFMM is almost identical to EikonalSolverFMM avoiding
+ * the update of values inside the priority queue. Instead the queue contains
+ * duplicates.
+ *
+ * See: jones-2006 (3D distance fields: a survey of techniques and applications)
+ * See: gomez-2015 (Fast Methods for Eikonal Equations: an Experimental Survey)
+ *
+ */
+public class EikonalSolverSFMM implements EikonalSolver {
+	protected final PriorityQueue<Pair<Point, Double>> narrowBand;
+	protected final ITimeCostFunction timeCostFunction;
+
+	protected CellGrid cellGrid;
+	protected List<Point> targetPoints;
+	protected Collection<VShape> targetShapes;
+	boolean isHighAccuracy = false;
+
+	/** only for logging */
+	protected static Logger logger = LogManager.getLogger(EikonalSolverFMM.class);
+	protected long runtime = 0;
+
+	/**
+	 * Initializes the FM potential calculator with a time cost function F > 0.
+	 */
+	public EikonalSolverSFMM(
+							final CellGrid potentialField,
+	                        final Collection<VShape> targetShapes,