Commit 55079e75 authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files

refactoring of the eikonalSolver code

parent 69a6b798
......@@ -2,7 +2,6 @@ package org.vadere.simulator.models.potential.fields;
import org.apache.log4j.LogManager;
import org.apache.log4j.Logger;
import org.vadere.state.attributes.models.AttributesFloorField;
import org.vadere.state.scenario.Agent;
import org.vadere.state.scenario.ScenarioElement;
import org.vadere.state.scenario.Target;
......@@ -10,32 +9,30 @@ import org.vadere.state.scenario.TargetPedestrian;
import org.vadere.state.scenario.Topography;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.VShape;
import org.vadere.util.math.InterpolationUtil;
import org.vadere.util.potential.CellGrid;
import org.vadere.util.potential.calculators.AbstractGridEikonalSolver;
import java.awt.*;
import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import java.util.Optional;
public abstract class AbstractPotentialFieldTarget implements IPotentialTargetGrid {
public abstract class AbstractGridPotentialFieldTarget implements IPotentialTargetGrid {
protected static double EPSILON_SIM_TIME = 1e-100; // TODO [priority=medium] [task=fix] 1e-100 comparisons with values that are O(1e-8) are dangerous. Better use 1e-8 here.
protected double lastUpdateTimestamp;
private Topography topography;
private boolean wasUpdated;
private static Logger logger = LogManager.getLogger(AbstractPotentialFieldTarget.class);
private static Logger logger = LogManager.getLogger(AbstractGridPotentialFieldTarget.class);
/**
* Stores all potential fields which represent the observation area. The key
* of the outer map equals the id of the floor, the key of the inner map
* equals the id of the target (there is a floor field per target).
*/
protected final HashMap<Integer, PotentialFieldAndInitializer> targetPotentialFields;
protected final HashMap<Integer, PotentialFieldAndInitializer<? extends AbstractGridEikonalSolver>> targetPotentialFields;
public AbstractPotentialFieldTarget(final Topography topography) {
public AbstractGridPotentialFieldTarget(final Topography topography) {
this.topography = topography;
this.wasUpdated = false;
this.targetPotentialFields = new HashMap<>();
......@@ -77,7 +74,7 @@ public abstract class AbstractPotentialFieldTarget implements IPotentialTargetGr
}
/* Find minimal potential of given targets. */
Optional<PotentialFieldAndInitializer> optionalPotentialFieldAndAnalyser =
Optional<PotentialFieldAndInitializer<? extends AbstractGridEikonalSolver>> optionalPotentialFieldAndAnalyser =
getPotentialFieldAndInitializer(targetId);
// no target exist
......@@ -85,69 +82,7 @@ public abstract class AbstractPotentialFieldTarget implements IPotentialTargetGr
return 0;
}
PotentialFieldAndInitializer potentialFieldAndAnalyser = optionalPotentialFieldAndAnalyser.get();
AttributesFloorField attributesFloorField = potentialFieldAndAnalyser.attributesFloorField;
potentialField = potentialFieldAndAnalyser.eikonalSolver.getPotentialField();
Point gridPoint = potentialField.getNearestPointTowardsOrigin(pos);
VPoint gridPointCoord = potentialField.pointToCoord(gridPoint);
int incX = 1, incY = 1;
double gridPotentials[];
double weightOfKnown[] = new double[1];
double tmpPotential;
if (pos.x >= potentialField.getWidth()) {
incX = 0;
}
if (pos.y >= potentialField.getHeight()) {
incY = 0;
}
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 = potentialFieldAndAnalyser.getGridPotentials(points);
/* Interpolate the known (potential < Double.MAX_VALUE) values. */
tmpPotential = InterpolationUtil
.bilinearInterpolationWithUnkown(
gridPotentials,
(pos.x - gridPointCoord.x)
/ potentialField.getResolution(),
(pos.y - gridPointCoord.y)
/ potentialField.getResolution(),
weightOfKnown);
/*
* If at least one node is known, a specialized version of
* interpolation is used: If the divisor weightOfKnown[ 0 ] would
* not be part of the equation, it would be a general bilinear
* interpolation using obstacleGridPenalty for the unknown. However,
* as soon as the interpolated value is not on the line of known
* values (weightOfKnown < 1) the potential is increased, like an
* additional penalty. The more the interpolated value moves into
* direction of the unknown, the higher the penalty becomes.
*/
if (weightOfKnown[0] > 0.00001) {
tmpPotential = tmpPotential / weightOfKnown[0]
+ (1 - weightOfKnown[0])
* attributesFloorField.getObstacleGridPenalty();
} else /* If all values are maximal, set potential to maximum. */
{
tmpPotential = Double.MAX_VALUE;
}
tmpPotential *= attributesFloorField.getTargetAttractionStrength();
if (tmpPotential < targetPotential) {
targetPotential = tmpPotential;
}
return targetPotential;
return optionalPotentialFieldAndAnalyser.get().eikonalSolver.getValue(pos);
}
/**
......@@ -189,7 +124,7 @@ public abstract class AbstractPotentialFieldTarget implements IPotentialTargetGr
return wasUpdated;
}
protected Map<Integer, PotentialFieldAndInitializer> getPotentialFieldMap() {
protected Map<Integer, PotentialFieldAndInitializer<? extends AbstractGridEikonalSolver>> getPotentialFieldMap() {
return targetPotentialFields;
}
......@@ -222,7 +157,7 @@ public abstract class AbstractPotentialFieldTarget implements IPotentialTargetGr
HashMap<Integer, CellGrid> map = new HashMap<>();
for (Map.Entry<Integer, PotentialFieldAndInitializer> entry2 : targetPotentialFields
for (Map.Entry<Integer, PotentialFieldAndInitializer<? extends AbstractGridEikonalSolver>> entry2 : targetPotentialFields
.entrySet()) {
map.put(entry2.getKey(), entry2.getValue().eikonalSolver.getPotentialField());
}
......@@ -230,7 +165,7 @@ public abstract class AbstractPotentialFieldTarget implements IPotentialTargetGr
return map;
}
protected Optional<PotentialFieldAndInitializer> getPotentialFieldAndInitializer(final int targetId) {
protected Optional<PotentialFieldAndInitializer<? extends AbstractGridEikonalSolver>> getPotentialFieldAndInitializer(final int targetId) {
if (targetPotentialFields.containsKey(targetId)) {
return Optional.of(targetPotentialFields.get(targetId));
} else {
......
......@@ -16,9 +16,9 @@ import org.vadere.util.reflection.DynamicClassInstantiator;
import org.vadere.util.reflection.VadereClassNotFoundException;
public interface IPotentialTargetGrid extends PotentialFieldTarget {
public HashMap<Integer, CellGrid> getCellGrids();
HashMap<Integer, CellGrid> getCellGrids();
public static IPotentialTargetGrid createPotentialField(List<Attributes> modelAttributesList,
static IPotentialTargetGrid createPotentialField(List<Attributes> modelAttributesList,
Topography topography, AttributesAgent attributesPedestrian, String className) {
DynamicClassInstantiator<IPotentialTargetGrid> instantiator = new DynamicClassInstantiator<>();
......
......@@ -26,13 +26,13 @@ import java.util.List;
* contains the factory method to create a PotentialFieldAndInitializer object.
*
*/
class PotentialFieldAndInitializer {
protected final EikonalSolver eikonalSolver;
class PotentialFieldAndInitializer<E extends EikonalSolver> {
protected final E eikonalSolver;
protected final AttributesFloorField attributesFloorField;
private static Logger logger = LogManager.getLogger(PotentialFieldAndInitializer.class);
protected PotentialFieldAndInitializer(
final EikonalSolver eikonalSolver,
final E eikonalSolver,
final AttributesFloorField attributesFloorField) {
this.eikonalSolver = eikonalSolver;
this.attributesFloorField = attributesFloorField;
......@@ -78,29 +78,19 @@ class PotentialFieldAndInitializer {
eikonalSolver = new PotentialFieldCalculatorNone();
break;
case FAST_ITERATIVE_METHOD:
eikonalSolver = new EikonalSolverFIM(cellGrid, targetShapes, isHighAccuracyFM, timeCost);
eikonalSolver = new EikonalSolverFIM(cellGrid, targetShapes, isHighAccuracyFM, timeCost, attributesPotential.getObstacleGridPenalty());
break;
case FAST_SWEEPING_METHOD:
eikonalSolver = new EikonalSolverFSM(cellGrid, targetShapes, isHighAccuracyFM, timeCost);
eikonalSolver = new EikonalSolverFSM(cellGrid, targetShapes, isHighAccuracyFM, timeCost, attributesPotential.getObstacleGridPenalty());
break;
default:
eikonalSolver = new EikonalSolverFMM(cellGrid, targetShapes, isHighAccuracyFM, timeCost);
eikonalSolver = new EikonalSolverFMM(cellGrid, targetShapes, isHighAccuracyFM, timeCost, attributesPotential.getObstacleGridPenalty());
}
long ms = System.currentTimeMillis();
eikonalSolver.initialize();
logger.info("floor field initialization time:" + (System.currentTimeMillis() - ms + "[ms]"));
return new PotentialFieldAndInitializer(eikonalSolver, attributesPotential);
}
public double[] getGridPotentials(List<Point> points) {
double[] result = new double[points.size()];
for (int i = 0; i < points.size(); i++) {
result[i] = this.eikonalSolver.getPotentialField().getValue(points.get(i)).potential;
}
return result;
return new PotentialFieldAndInitializer<>(eikonalSolver, attributesPotential);
}
}
......@@ -21,7 +21,7 @@ import org.vadere.util.geometry.shapes.VShape;
*
* Not finiehsed!
*/
public class PotentialFieldMultiTargetGrid<T extends Agent> extends AbstractPotentialFieldTarget {
public class PotentialFieldMultiTargetGrid<T extends Agent> extends AbstractGridPotentialFieldTarget {
private static double EPSILON_SIM_TIME = 1e-100;
private final Map<Integer, AttributesFloorField> attributesByTarget;
......
......@@ -19,7 +19,7 @@ import org.vadere.util.geometry.shapes.VShape;
* based on the AttributesFloorField.
*
*/
public class PotentialFieldSingleTargetGrid extends AbstractPotentialFieldTarget {
public class PotentialFieldSingleTargetGrid extends AbstractGridPotentialFieldTarget {
private Topography topography;
private AttributesFloorField attributesFloorField;
......
......@@ -15,7 +15,7 @@ import org.vadere.util.geometry.shapes.VShape;
* based on the AttributesFloorField.
*
*/
public class PotentialFieldTargetGrid<T extends Agent> extends AbstractPotentialFieldTarget {
public class PotentialFieldTargetGrid<T extends Agent> extends AbstractGridPotentialFieldTarget {
// private HashMap<Integer, PotentialFieldAndInitializer> staticPotentialFields;
/** The topography the floor fields are generated for. */
......
......@@ -80,7 +80,7 @@ public class PotentialFieldTargetQueuingGrid implements IPotentialTargetGrid, Dy
topography.getTargets().stream().map(t -> t.getShape()).collect(Collectors.toList());
this.detector = new QueueDetector(cellGrid, targetShapes, true, new UnitTimeCostFunction(),
attributesPedestrian, topography);
attributesPedestrian, topography, new AttributesFloorField().getObstacleGridPenalty());
this.queues = topography.getTargets().stream().map(t -> t.getId()).distinct()
.map(targetId -> new Queue(topography, targetId, detector)).collect(Collectors.toList());
}
......
......@@ -75,14 +75,11 @@ public class Queue implements DynamicElementRemoveListener<Pedestrian>, DynamicE
}
}
public double getValue(int x, int y) {
return detector.getPotentialField().getValue(new Point(x, y)).potential;
}
public double getValue(double x, double y) {
return detector.getPotentialField().getValue(new Point((int) Math.round(x / detector.getResolution()),
/*public double getValue(double x, double y) {
return detector.get
getPotentialField().getValue(new Point((int) Math.round(x / detector.getResolution()),
(int) Math.round(y / detector.getResolution()))).potential;
}
}*/
public void update() {
detector.setPolytope(null);
......
......@@ -41,9 +41,15 @@ public class QueueDetector extends EikonalSolverFMM {
* @param isHighAccuracy
* @param timeCostFunction
*/
public QueueDetector(CellGrid potentialField, List<VShape> targetShapes, boolean isHighAccuracy,
ITimeCostFunction timeCostFunction, AttributesAgent attributesPedestrian, Topography topography) {
super(potentialField, targetShapes, isHighAccuracy, timeCostFunction);
public QueueDetector(
final CellGrid potentialField,
final List<VShape> targetShapes,
final boolean isHighAccuracy,
final ITimeCostFunction timeCostFunction,
final AttributesAgent attributesPedestrian,
final Topography topography,
final double unkownPenalty) {
super(potentialField, targetShapes, isHighAccuracy, timeCostFunction, unkownPenalty);
this.orderedPoints = new LinkedList<>();
this.attributesPedestrian = attributesPedestrian;
this.topography = topography;
......
......@@ -52,13 +52,13 @@ public class InterpolationUtil {
* Computes bilinear interpolation while nodes may be undefined
* (=Double.MAX_VALUE). See bilinearInterpolation for further information
* about the basic method. In contrast to the original
* bilinearInterpolation(), bilinearInterpolationWithUnkown() just
* bilinearInterpolation(), bilinearInterpolationWithUnknown() just
* accumulates the nodes whose values are known multiplied by its weight.
* The accumulated weight of the known values are stored in the parameter
* weightOfKnown.
*/
public static double bilinearInterpolationWithUnkown(double z[], double t,
double u, double weightOfKnown[]) {
public static double bilinearInterpolationWithUnknown(double z[], double t,
double u, double weightOfKnown[]) {
double result = 0;
double weight[] = {(1 - t) * (1 - u), t * (1 - u), t * u, (1 - t) * u};
......
package org.vadere.util.potential.calculators;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.math.InterpolationUtil;
import org.vadere.util.potential.CellGrid;
import java.awt.*;
import java.util.*;
import java.util.List;
public abstract class AbstractGridEikonalSolver implements EikonalSolver {
protected CellGrid potentialField;
private double unknownPenalty;
public AbstractGridEikonalSolver(final CellGrid potentialField, final double unknownPenalty) {
this.potentialField = potentialField;
this.unknownPenalty = unknownPenalty;
}
@Override
public double getValue(final double x, final double y) {
Point gridPoint = potentialField.getNearestPointTowardsOrigin(x, y);
VPoint gridPointCoord = potentialField.pointToCoord(gridPoint);
int incX = 1, incY = 1;
double gridPotentials[];
double weightOfKnown[] = new double[1];
if (x >= potentialField.getWidth()) {
incX = 0;
}
if (y >= potentialField.getHeight()) {
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);
/* Interpolate the known (potential < Double.MAX_VALUE) values. */
double tmpPotential = InterpolationUtil.bilinearInterpolationWithUnknown(gridPotentials,
(x - gridPointCoord.x)
/ potentialField.getResolution(),
(y - gridPointCoord.y)
/ potentialField.getResolution(),
weightOfKnown);
/*
* If at least one node is known, a specialized version of
* interpolation is used: If the divisor weightOfKnown[ 0 ] would
* not be part of the equation, it would be a general bilinear
* interpolation using obstacleGridPenalty for the unknown. However,
* as soon as the interpolated value is not on the line of known
* values (weightOfKnown < 1) the potential is increased, like an
* additional penalty. The more the interpolated value moves into
* direction of the unknown, the higher the penalty becomes.
*/
if (weightOfKnown[0] > 0.00001) {
tmpPotential = tmpPotential / weightOfKnown[0]
+ (1 - weightOfKnown[0])
* unknownPenalty;
} else /* If all values are maximal, set potential to maximum. */
{
tmpPotential = Double.MAX_VALUE;
}
return tmpPotential;
}
public CellGrid getPotentialField() {
return potentialField;
}
private double[] getGridPotentials(final List<Point> points) {
double[] result = new double[points.size()];
for (int i = 0; i < points.size(); i++) {
result[i] = potentialField.getValue(points.get(i)).potential;
}
return result;
}
}
......@@ -2,6 +2,7 @@ package org.vadere.util.potential.calculators;
import org.apache.log4j.LogManager;
import org.apache.log4j.Logger;
import org.vadere.util.geometry.shapes.IPoint;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.math.MathUtil;
import org.vadere.util.potential.CellGrid;
......@@ -45,11 +46,17 @@ public interface EikonalSolver {
* @param point
* @return
*/
default double getValue(final Point point, final CellGrid cellGrid) {
/*default double getValue(final Point point, final CellGrid cellGrid) {
return cellGrid.getValue(point).potential;
}
}*/
//CellGrid getPotentialField();
CellGrid getPotentialField();
double getValue(final double x, final double y);
default double getValue(final IPoint position) {
return getValue(position.getX(), position.getY());
}
default boolean isHighAccuracy() {
return true;
......@@ -59,9 +66,7 @@ public interface EikonalSolver {
return new UnitTimeCostFunction();
}
default boolean isValidPoint(final Point point, final CellGrid cellGrid) {
return cellGrid.isValidPoint(point);
}
boolean isValidPoint(final Point point);
default double computeGodunovDifference(final Point point, final CellGrid cellGrid, final Direction direction) {
......@@ -107,8 +112,8 @@ public interface EikonalSolver {
yhPoint = new Point(point.x, point.y - 2);
break;
default: {
if (isValidPoint(new Point(point.x + 1, point.y), cellGrid) &&
(!isValidPoint(new Point(point.x - 1, point.y), cellGrid)
if (isValidPoint(new Point(point.x + 1, point.y)) &&
(!isValidPoint(new Point(point.x - 1, point.y))
|| (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);
......@@ -118,8 +123,8 @@ public interface EikonalSolver {
xhPoint = new Point(point.x - 2, point.y);
}
if (isValidPoint(new Point(point.x, point.y + 1), cellGrid) &&
(!isValidPoint(new Point(point.x, point.y - 1), cellGrid)
if (isValidPoint(new Point(point.x, point.y + 1)) &&
(!isValidPoint(new Point(point.x, point.y - 1))
|| (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);
......@@ -132,7 +137,7 @@ public interface EikonalSolver {
}
double xVal = Double.MAX_VALUE;
if (isValidPoint(xPoint, cellGrid)) {
if (isValidPoint(xPoint)) {
xVal = cellGrid.getValue(xPoint).potential;
if (xVal != Double.MAX_VALUE) {
a += 1.0;
......@@ -142,7 +147,7 @@ public interface EikonalSolver {
}
double yVal = Double.MAX_VALUE;
if (isValidPoint(yPoint, cellGrid)) {
if (isValidPoint(yPoint)) {
yVal = cellGrid.getValue(yPoint).potential;
if (yVal != Double.MAX_VALUE) {
a += 1.0;
......@@ -157,7 +162,7 @@ public interface EikonalSolver {
// logger.warn("no solution possible");
} else {
if (isHighAccuracy()) {
if (isValidPoint(xhPoint, cellGrid) && cellGrid.getValue(xhPoint).potential < xVal) {
if (isValidPoint(xhPoint) && cellGrid.getValue(xhPoint).potential < xVal) {
double tp = (1.0 / 3.0) * (4.0 * xVal - cellGrid.getValue(xhPoint).potential);
double factor = 9.0 / 4.0;
a += factor;
......@@ -165,7 +170,7 @@ public interface EikonalSolver {
c += factor * Math.pow(tp, 2);
}
if (isValidPoint(yhPoint, cellGrid) && cellGrid.getValue(yhPoint).potential < yVal) {
if (isValidPoint(yhPoint) && cellGrid.getValue(yhPoint).potential < yVal) {
double tp = (1.0 / 3.0) * (4.0 * yVal - cellGrid.getValue(yhPoint).potential);
double factor = 9.0 / 4.0;
a += factor;
......@@ -213,13 +218,13 @@ public interface EikonalSolver {
point.x + neighbors.get(2 * j + i).x * 2, point.y
+ neighbors.get(2 * j + i).y * 2);
if (isValidPoint(pni, cellGrid) && cellGrid.getValue(pni).tag.frozen) {
if (isValidPoint(pni) && cellGrid.getValue(pni).tag.frozen) {
double val1n = cellGrid.getValue(pni).potential;
if (val1n < val1) {
val1 = val1n;
if (isValidPoint(pni2, cellGrid)) {
if (isValidPoint(pni2)) {
double val2n = cellGrid.getValue(pni2).potential;
if (cellGrid.getValue(pni2).tag.frozen
&& val2n <= val1n) {
......
......@@ -9,7 +9,6 @@ import org.vadere.util.potential.PathFindingTag;
import org.vadere.util.potential.timecost.ITimeCostFunction;
import java.awt.*;
import java.io.*;
import java.util.*;
import java.util.List;
import java.util.stream.Collectors;
......@@ -25,7 +24,7 @@ import java.util.stream.Collectors;
*
*
*/
public class EikonalSolverFIM implements EikonalSolver {
public class EikonalSolverFIM extends AbstractGridEikonalSolver {
private CellGrid cellGrid;
private List<VShape> targetShapes;
......@@ -39,10 +38,13 @@ public class EikonalSolverFIM implements EikonalSolver {
private LinkedList<Point> activeList;
public EikonalSolverFIM(final CellGrid cellGrid,
public EikonalSolverFIM(
final CellGrid cellGrid,
final List<VShape> targetShapes,
final boolean isHighAccuracy,
final ITimeCostFunction timeCostFunction) {
final ITimeCostFunction timeCostFunction,
final double unknownPenalty) {
super(cellGrid, unknownPenalty);
this.timeCostFunction = timeCostFunction;
this.isHighAccuracy = isHighAccuracy;
this.targetShapes = targetShapes;
......@@ -149,13 +151,13 @@ public class EikonalSolverFIM implements EikonalSolver {
}
@Override
public CellGrid getPotentialField() {
return cellGrid;
public ITimeCostFunction getTimeCostFunction() {
return timeCostFunction;
}
@Override
public ITimeCostFunction getTimeCostFunction() {
return timeCostFunction;
public boolean isValidPoint(Point point) {
return cellGrid.isValidPoint(point);
}
@Override
......