Commit e4472e93 authored by Benedikt Zoennchen's avatar Benedikt Zoennchen

replace obstacle distance computation by fast marching method.

parent 9bcf02f7
......@@ -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;
......
......@@ -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
......
......@@ -47,16 +47,19 @@ public class PotentialFieldPedestrianCompactSoftshell implements PotentialFieldA
@Override
public double getAgentPotential(VPoint pos, Agent pedestrian,
Agent otherPedestrian) {
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);
if(distnaceSq < maxDistanceSq) {
double distance = otherPedestrian.getPosition().distance(pos);
int intPower = this.attributes.getIntimateSpacePower();
int perPower = this.attributes.getPersonalSpacePower();
double factor = this.attributes.getIntimateSpaceFactor();
double potential = 0;
double radii = pedestrian.getRadius() + otherPedestrian.getRadius();
if (distance < personalWidth + radii) {
potential += this.height * Math.exp(4 / (Math.pow(distance / (personalWidth + radii), (2 * perPower)) - 1));
}
......@@ -67,6 +70,7 @@ public class PotentialFieldPedestrianCompactSoftshell implements PotentialFieldA
if (distance < radii) {
potential += 1000 * Math.exp(1 / (Math.pow(distance / radii, 4) - 1));
}
}
return potential;
}
......
......@@ -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);
......
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);
}
}
......@@ -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();
}
......
......@@ -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,12 +25,14 @@ 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?
/**
* Cell size of the internal storage of DynamicElements. Is used in the LinkedCellsGrid.
......@@ -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;
......@@ -107,6 +113,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);
}
/** Clean up a set by removing {@code null}. */
......@@ -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);
}
......
......@@ -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());
}
......
......@@ -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(
......
......@@ -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;
......
......@@ -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;
......
......@@ -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;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment