replace obstacle distance computation by fast marching method.

VadereSimulator/src/org/vadere/simulator/control/Simulation.java

@@ -6,19 +6,26 @@ import org.vadere.simulator.models.DynamicElementFactory;
 import org.vadere.simulator.models.MainModel;
 import org.vadere.simulator.models.Model;
 import org.vadere.simulator.models.potential.PotentialFieldModel;
+import org.vadere.simulator.models.potential.fields.IPotentialField;
 import org.vadere.simulator.models.potential.fields.IPotentialFieldTarget;
+import org.vadere.simulator.models.potential.fields.ObstacleDistancePotential;
 import org.vadere.simulator.projects.ScenarioStore;
 import org.vadere.simulator.projects.dataprocessing.ProcessorManager;
 import org.vadere.state.attributes.AttributesSimulation;
+import org.vadere.state.attributes.models.AttributesFloorField;
 import org.vadere.state.attributes.scenario.AttributesAgent;
 import org.vadere.state.scenario.Source;
 import org.vadere.state.scenario.Target;
 import org.vadere.state.scenario.Topography;
+import org.vadere.util.geometry.shapes.VPoint;
+import org.vadere.util.geometry.shapes.VRectangle;
 
 import java.util.Collection;
 import java.util.LinkedList;
 import java.util.List;
 import java.util.Random;
+import java.util.function.Function;
+import java.util.stream.Collectors;
 
 public class Simulation {
 
@@ -72,6 +79,13 @@ public class Simulation {
 		this.targetControllers = new LinkedList<>();
 		this.topography = scenarioStore.topography;
 
+		IPotentialField distanceField = new ObstacleDistancePotential(
+				topography.getObstacles().stream().map(obs -> obs.getShape()).collect(Collectors.toList()),
+				new VRectangle(topography.getBounds()),
+				new AttributesFloorField());
+		Function<VPoint, Double> obstacleDistance = p -> distanceField.getPotential(p, null);
+		this.topography.setObstacleDistanceFunction(obstacleDistance);
+
 		this.runTimeInSec = attributesSimulation.getFinishTime();
 		this.startTimeInSec = startTimeInSec;
 		this.simTimeInSec = startTimeInSec;

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

@@ -24,25 +24,27 @@ public class PotentialFieldObstacleCompactSoftshell implements PotentialFieldObs
 	private double width;
 	private double height;
 	private Collection<Obstacle> obstacles;
+	private Topography topography;
 
 	public PotentialFieldObstacleCompactSoftshell(AttributesPotentialCompactSoftshell attributesPotential,
-			Collection<Obstacle> obstacles, Random random) {
+			Topography topography, Random random) {
 		this.attributes = attributesPotential;
 		this.random = random;
 
 		this.width = attributesPotential.getObstPotentialWidth();
 		this.height = attributesPotential.getObstPotentialHeight();
-
-		this.obstacles = obstacles;
+		this.topography = topography;
+		this.obstacles = topography.getObstacles();
 	}
 
 	@Override
 	public double getObstaclePotential(VPoint pos, Agent pedestrian) {
 
 		double potential = 0;
-		for (Obstacle obstacle : obstacles) {
+		//for (Obstacle obstacle : obstacles) {
 
-			double distance = obstacle.getShape().distance(pos);
+			//double distance = obstacle.getShape().distance(pos);
+			double distance = topography.distanceToObstacle(pos);
 
 			double radius = pedestrian.getRadius();
 			double currentPotential = 0;
@@ -56,7 +58,7 @@ public class PotentialFieldObstacleCompactSoftshell implements PotentialFieldObs
 
 			if (potential < currentPotential)
 				potential = currentPotential;
-		}
+		//}
 
 		return potential;
 	}
@@ -69,7 +71,7 @@ public class PotentialFieldObstacleCompactSoftshell implements PotentialFieldObs
 
 	@Override
 	public PotentialFieldObstacle copy() {
-		return new PotentialFieldObstacleCompactSoftshell(attributes, new LinkedList<>(obstacles), random);
+		return new PotentialFieldObstacleCompactSoftshell(attributes, topography, random);
 	}
 
 	@Override

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

@@ -47,25 +47,29 @@ public class PotentialFieldPedestrianCompactSoftshell implements PotentialFieldA
 	@Override
 	public double getAgentPotential(VPoint pos, Agent pedestrian,
 			Agent otherPedestrian) {
-		double distance = otherPedestrian.getPosition().distance(pos);
-
-		int intPower = this.attributes.getIntimateSpacePower();
-		int perPower = this.attributes.getPersonalSpacePower();
-		double factor = this.attributes.getIntimateSpaceFactor();
 
+		double radii = pedestrian.getRadius() + otherPedestrian.getRadius();
 		double potential = 0;
+		double distnaceSq = otherPedestrian.getPosition().distanceSq(pos);
+		double maxDistanceSq = (Math.max(personalWidth, intimateWidth)  + radii) * (Math.max(personalWidth, intimateWidth)  + radii);
 
-		double radii = pedestrian.getRadius() + otherPedestrian.getRadius();
+		if(distnaceSq < maxDistanceSq) {
+			double distance = otherPedestrian.getPosition().distance(pos);
 
-		if (distance < personalWidth + radii) {
-			potential += this.height * Math.exp(4 / (Math.pow(distance / (personalWidth + radii), (2 * perPower)) - 1));
-		}
-		if (distance < this.intimateWidth + radii) {
-			potential += this.height / factor
-					* Math.exp(4 / (Math.pow(distance / (this.intimateWidth + radii), (2 * intPower)) - 1));
-		}
-		if (distance < radii) {
-			potential += 1000 * Math.exp(1 / (Math.pow(distance / radii, 4) - 1));
+			int intPower = this.attributes.getIntimateSpacePower();
+			int perPower = this.attributes.getPersonalSpacePower();
+			double factor = this.attributes.getIntimateSpaceFactor();
+
+			if (distance < personalWidth + radii) {
+				potential += this.height * Math.exp(4 / (Math.pow(distance / (personalWidth + radii), (2 * perPower)) - 1));
+			}
+			if (distance < this.intimateWidth + radii) {
+				potential += this.height / factor
+						* Math.exp(4 / (Math.pow(distance / (this.intimateWidth + radii), (2 * intPower)) - 1));
+			}
+			if (distance < radii) {
+				potential += 1000 * Math.exp(1 / (Math.pow(distance / radii, 4) - 1));
+			}
 		}
 		return potential;
 

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

@@ -62,16 +62,18 @@ public abstract class AbstractPotentialFieldTarget implements IPotentialFieldTar
 		int targetId = agent.getNextTargetId();
 
 		// the agent has reached his current target
-		if (topography.getTarget(targetId).getShape().contains(pos)) {
+		// TODO: expensive operation
+		/*if (topography.getTarget(targetId).getShape().contains(pos)) {
 			return 0.0;
-		}
+		}*/
 
 		// the agent is inside an obstacle
-		for (ScenarioElement b : topography.getObstacles()) {
+		// TODO: expensive operation
+		/*for (ScenarioElement b : topography.getObstacles()) {
 			if (b.getShape().contains(pos)) {
 				return Double.MAX_VALUE;
 			}
-		}
+		}*/
 
 		/* Find minimal potential of given targets. */
 		Optional<PotentialFieldAndInitializer> optionalPotentialFieldAndAnalyser = getPotentialFieldAndInitializer(targetId);

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

+package org.vadere.simulator.models.potential.fields;
+
+import org.apache.log4j.LogManager;
+import org.apache.log4j.Logger;
+import org.jetbrains.annotations.NotNull;
+import org.jetbrains.annotations.Nullable;
+import org.vadere.state.attributes.models.AttributesFloorField;
+import org.vadere.state.scenario.Agent;
+import org.vadere.util.geometry.shapes.VPoint;
+import org.vadere.util.geometry.shapes.VRectangle;
+import org.vadere.util.geometry.shapes.VShape;
+import org.vadere.util.potential.CellGrid;
+import org.vadere.util.potential.CellState;
+import org.vadere.util.potential.FloorDiscretizer;
+import org.vadere.util.potential.PathFindingTag;
+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.PotentialFieldCalculatorNone;
+import org.vadere.util.potential.timecost.UnitTimeCostFunction;
+
+import java.util.Collection;
+
+/**
+ * @author Benedikt Zoennchen
+ */
+public class ObstacleDistancePotential implements IPotentialField {
+
+	private static Logger logger = LogManager.getLogger(ObstacleDistancePotential.class);
+	private final EikonalSolver eikonalSolver;
+
+	public ObstacleDistancePotential(@NotNull final Collection<VShape> obstacles,
+	                                 @NotNull final VRectangle bounds,
+	                                 @NotNull final AttributesFloorField attributesFloorField) {
+		CellGrid cellGrid = new CellGrid(bounds.getWidth(), bounds.getHeight(), attributesFloorField.getPotentialFieldResolution(), new CellState());
+
+		for (VShape shape : obstacles) {
+			FloorDiscretizer.setGridValuesForShapeCentered(cellGrid, shape,
+					new CellState(0.0, PathFindingTag.Target));
+		}
+
+		boolean isHighAccuracyFM = attributesFloorField.getCreateMethod().isHighAccuracy();
+
+		/* copy the static grid */
+		switch (attributesFloorField.getCreateMethod()) {
+			case NONE:
+				eikonalSolver = new PotentialFieldCalculatorNone();
+				break;
+			case FAST_ITERATIVE_METHOD:
+				eikonalSolver = new EikonalSolverFIM(cellGrid, obstacles, isHighAccuracyFM, new UnitTimeCostFunction());
+				break;
+			case FAST_SWEEPING_METHOD:
+				eikonalSolver = new EikonalSolverFSM(cellGrid, obstacles, isHighAccuracyFM, new UnitTimeCostFunction());
+				break;
+			default:
+				eikonalSolver = new EikonalSolverFMM(cellGrid, obstacles, isHighAccuracyFM, new UnitTimeCostFunction());
+		}
+
+		long ms = System.currentTimeMillis();
+		eikonalSolver.initialize();
+		logger.info("floor field initialization time:" + (System.currentTimeMillis() - ms + "[ms]"));
+	}
+
+	@Override
+	public double getPotential(@NotNull VPoint pos, @Nullable Agent agent) {
+		return eikonalSolver.getPotential(pos, 0.0, 1.0);
+	}
+}

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

@@ -77,7 +77,7 @@ public interface PotentialFieldObstacle extends Model {
 			AttributesPotentialCompactSoftshell attributesPotentialCompactSoftshell =
 					Model.findAttributes(modelAttributesList, AttributesPotentialCompactSoftshell.class);
 			result = new PotentialFieldObstacleCompactSoftshell(attributesPotentialCompactSoftshell,
-					topography.getObstacles(), random);
+					topography, random);
 		} else {
 			throw new VadereClassNotFoundException();
 		}

VadereState/src/org/vadere/state/scenario/Topography.java

@@ -10,10 +10,12 @@ import java.util.LinkedList;
 import java.util.List;
 import java.util.Map;
 import java.util.Set;
+import java.util.function.Function;
 import java.util.function.Predicate;
 import java.util.stream.Collectors;
 
 import org.apache.log4j.Logger;
+import org.jetbrains.annotations.NotNull;
 import org.vadere.state.attributes.Attributes;
 import org.vadere.state.attributes.scenario.AttributesAgent;
 import org.vadere.state.attributes.scenario.AttributesCar;
@@ -23,11 +25,13 @@ import org.vadere.util.geometry.LinkedCellsGrid;
 import org.vadere.util.geometry.shapes.VPoint;
 import org.vadere.util.geometry.shapes.VShape;
 
-@JsonIgnoreProperties(value = {"allOtherAttributes"})
+@JsonIgnoreProperties(value = {"allOtherAttributes", "obstacleDistanceFunction"})
 public class Topography {
 
 	/** Transient to prevent JSON serialization. */
 	private static Logger logger = Logger.getLogger(Topography.class);
+
+	private Function<VPoint, Double> obstacleDistanceFunction;
 	
 	// TODO [priority=low] [task=feature] magic number, use attributes / parameter?
 	/**
@@ -74,7 +78,9 @@ public class Topography {
 	/** Used to store links to all attributes that are not part of scenario elements. */
 	private Set<Attributes> allOtherAttributes = new HashSet<>(); // will be filled in the constructor
 
-	public Topography(AttributesTopography attributes, AttributesAgent attributesPedestrian,
+	public Topography(
+			AttributesTopography attributes,
+			AttributesAgent attributesPedestrian,
 			AttributesCar attributesCar) {
 
 		this.attributes = attributes;
@@ -106,6 +112,8 @@ public class Topography {
 
 		this.pedestrians = new DynamicElementContainer<>(bounds, CELL_SIZE);
 		this.cars = new DynamicElementContainer<>(bounds, CELL_SIZE);
+
+		this.obstacleDistanceFunction = p -> obstacles.stream().map(obs -> obs.getShape()).map(shape -> shape.distance(p)).min(Double::compareTo).orElse(Double.MAX_VALUE);
 		
 	}
 
@@ -136,6 +144,14 @@ public class Topography {
 		return null;
 	}
 
+	public double distanceToObstacle(@NotNull VPoint point) {
+		return this.obstacleDistanceFunction.apply(point);
+	}
+
+	public void setObstacleDistanceFunction(@NotNull Function<VPoint, Double> obstacleDistanceFunction) {
+		this.obstacleDistanceFunction = obstacleDistanceFunction;
+	}
+
 	public boolean containsTarget(final Predicate<Target> targetPredicate) {
 		return getTargets().stream().anyMatch(targetPredicate);
 	}

VadereUtils/src/org/vadere/util/geometry/shapes/VPoint.java

@@ -39,6 +39,10 @@ public class VPoint implements Cloneable {
 		return Point2D.distance(x, y, other.x, other.y);
 	}
 
+	public double distanceSq(final VPoint other) {
+		return Point2D.distanceSq(x, y, other.x, other.y);
+	}
+
 	public double distance(Point2D other) {
 		return Point2D.distance(x, y, other.getX(), other.getY());
 	}

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

@@ -80,7 +80,7 @@ public interface EikonalSolver {
         Point gridPoint = potentialField.getNearestPointTowardsOrigin(pos);
         VPoint gridPointCoord = potentialField.pointToCoord(gridPoint);
         int incX = 1, incY = 1;
-        double gridPotentials[];
+        double gridPotentials[] = new double[4];
 
         if (pos.x >= potentialField.getWidth()) {
             incX = 0;
@@ -90,12 +90,11 @@ public interface EikonalSolver {
             incY = 0;
         }
 
-        java.util.List<Point> points = new LinkedList<>();
-        points.add(gridPoint);
-        points.add(new Point(gridPoint.x + incX, gridPoint.y));
-        points.add(new Point(gridPoint.x + incX, gridPoint.y + incY));
-        points.add(new Point(gridPoint.x, gridPoint.y + incY));
-        gridPotentials = getGridPotentials(points, potentialField);
+	    gridPotentials[0] = potentialField.getValue(gridPoint).potential;
+	    gridPotentials[1] = potentialField.getValue(gridPoint.x + incX, gridPoint.y).potential;
+	    gridPotentials[2] = potentialField.getValue(gridPoint.x + incX, gridPoint.y + incY).potential;
+	    gridPotentials[3] = potentialField.getValue(gridPoint.x, gridPoint.y + incY).potential;
+
 
 		/* Interpolate the known (potential < Double.MAX_VALUE) values. */
         Pair<Double, Double> result = InterpolationUtil.bilinearInterpolationWithUnkown(

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

@@ -28,7 +28,7 @@ import java.util.stream.Collectors;
 public class EikonalSolverFIM implements EikonalSolver {
 
 	private CellGrid cellGrid;
-	private List<VShape> targetShapes;
+	private Collection<VShape> targetShapes;
 	private List<Point> targetPoints;
 	private static Logger logger = LogManager.getLogger(EikonalSolverFIM.class);
 	private ITimeCostFunction timeCostFunction;
@@ -40,7 +40,7 @@ public class EikonalSolverFIM implements EikonalSolver {
 	private LinkedList<Point> activeList;
 
 	public EikonalSolverFIM(final CellGrid cellGrid,
-			final List<VShape> targetShapes,
+			final Collection<VShape> targetShapes,
 			final boolean isHighAccuracy,
 			final ITimeCostFunction timeCostFunction) {
 		this.timeCostFunction = timeCostFunction;

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

@@ -2,6 +2,7 @@ package org.vadere.util.potential.calculators;
 
 import java.awt.Point;
 import java.util.ArrayList;
+import java.util.Collection;
 import java.util.List;
 import java.util.PriorityQueue;
 import java.util.stream.Collectors;
@@ -29,7 +30,7 @@ public class EikonalSolverFMM implements EikonalSolver {
 
 	protected CellGrid cellGrid;
 	protected List<Point> targetPoints;
-	protected List<VShape> targetShapes;
+	protected Collection<VShape> targetShapes;
 	boolean isHighAccuracy = false;
 
 	/** only for logging */
@@ -40,7 +41,7 @@ public class EikonalSolverFMM implements EikonalSolver {
 	 * Initializes the FM potential calculator with a time cost function F > 0.
 	 */
 	public EikonalSolverFMM(CellGrid potentialField,
-			List<VShape> targetShapes,
+	                        Collection<VShape> targetShapes,
 			boolean isHighAccuracy,
 			ITimeCostFunction timeCostFunction) {
 		this.cellGrid = potentialField;

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

@@ -13,6 +13,7 @@ import java.io.BufferedWriter;
 import java.io.File;
 import java.io.FileWriter;
 import java.io.IOException;
+import java.util.Collection;
 import java.util.List;
 import java.util.stream.Collectors;
 
@@ -36,12 +37,12 @@ public class EikonalSolverFSM implements EikonalSolver {
 	private ITimeCostFunction timeCostFunction;
 	private boolean isHighAccuracy;
 	private List<Point> targetPoints;
-	private List<VShape> targetShapes;
+	private Collection<VShape> targetShapes;
 
 	private static final double EPSILON = 0.001;
 
 	public EikonalSolverFSM(final CellGrid cellGrid,
-			final List<VShape> targetShapes,
+			final Collection<VShape> targetShapes,
 			final boolean isHighAccuracy,
 			final ITimeCostFunction timeCostFunction) {
 		this.timeCostFunction = timeCostFunction;