Commit 55dd15dc authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files

Add a distance field which is computed by a brute force method i.e. computing...

Add a distance field which is computed by a brute force method i.e. computing the distance to all obstacles taking the minimum value. This is now the new default method to be used.
parent 45f7738f
......@@ -5,7 +5,7 @@ import java.util.function.Function;
import java.util.stream.Collectors;
import org.vadere.simulator.models.potential.fields.IPotentialField;
import org.vadere.simulator.models.potential.fields.ObstacleDistancePotential;
import org.vadere.simulator.models.potential.fields.PotentialFieldDistancesBruteForce;
import org.vadere.state.attributes.models.AttributesFloorField;
import org.vadere.state.attributes.scenario.AttributesObstacle;
import org.vadere.state.scenario.Car;
......@@ -49,7 +49,7 @@ public class OfflineTopographyController {
}
// add distance function
IPotentialField distanceField = new ObstacleDistancePotential(
IPotentialField distanceField = new PotentialFieldDistancesBruteForce(
topography.getObstacles().stream().map(obs -> obs.getShape()).collect(Collectors.toList()),
new VRectangle(topography.getBounds()),
new AttributesFloorField());
......
package org.vadere.simulator.control;
import java.util.LinkedList;
import java.util.function.Function;
import java.util.stream.Collectors;
import org.vadere.simulator.models.DynamicElementFactory;
import org.vadere.simulator.models.potential.fields.IPotentialField;
import org.vadere.simulator.models.potential.fields.ObstacleDistancePotential;
import org.vadere.state.attributes.models.AttributesFloorField;
import org.vadere.state.scenario.Pedestrian;
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.VRectangle;
public class TopographyController extends OfflineTopographyController {
......
......@@ -15,7 +15,6 @@ 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.EikonalSolverSFMM;
import org.vadere.util.potential.calculators.PotentialFieldCalculatorNone;
......@@ -25,15 +24,20 @@ import java.util.Collection;
/**
* @author Benedikt Zoennchen
*
* PotentialFieldDistanceEikonalEq computes the nearest distnace to any obstacle by computing
* the distance at certain discrete points lying on an Cartesian grid. Values inbetween are
* bilinear interpolated. To compute the distance at these grid points the eikonal equation is
* used by choosing obstacles to be the destination area of the propageting wave front.
*/
public class ObstacleDistancePotential implements IPotentialField {
public class PotentialFieldDistanceEikonalEq implements IPotentialField {
private static Logger logger = LogManager.getLogger(ObstacleDistancePotential.class);
private static Logger logger = LogManager.getLogger(PotentialFieldDistanceEikonalEq.class);
private final EikonalSolver eikonalSolver;
public ObstacleDistancePotential(@NotNull final Collection<VShape> obstacles,
@NotNull final VRectangle bounds,
@NotNull final AttributesFloorField attributesFloorField) {
public PotentialFieldDistanceEikonalEq(@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) {
......@@ -65,6 +69,8 @@ public class ObstacleDistancePotential implements IPotentialField {
@Override
public double getPotential(@NotNull VPoint pos, @Nullable Agent agent) {
return eikonalSolver.getPotential(pos, 0.0, 1.0);
// unknownPenalty = 0.0 since there will be no unknowns such as values at obstacles
// the fmm can cause an error mostly an underestimation of 20% near the source which are exactly the points we are interested
return eikonalSolver.getPotential(pos, 0.0, 1.2);
}
}
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.PathFindingTag;
import java.awt.*;
import java.util.Collection;
/**
* @author Benedikt Zoennchen
*
* PotentialFieldDistanceEikonalEq computes the nearest distnace to any obstacle by computing
* the distance at certain discrete points lying on an Cartesian grid. Values inbetween are
* bilinear interpolated. To compute the distance at these grid points the the exact distances
* to all obstacles are computed choosing the minimum.
*
* Note: This can be computational expensive if there are many and or complex obstacles.
*/
public class PotentialFieldDistancesBruteForce implements IPotentialField {
private static Logger logger = LogManager.getLogger(PotentialFieldDistancesBruteForce.class);
private final CellGrid cellGrid;
private final Collection<VShape> obstacles;
public PotentialFieldDistancesBruteForce(@NotNull final Collection<VShape> obstacles,
@NotNull final VRectangle bounds,
@NotNull final AttributesFloorField attributesFloorField) {
double ms = System.currentTimeMillis();
this.obstacles = obstacles;
this.cellGrid = new CellGrid(bounds.getWidth(), bounds.getHeight(), attributesFloorField.getPotentialFieldResolution(), new CellState());
this.cellGrid.pointStream().forEach(p -> computeDistanceToGridPoint(p));
logger.info("floor field initialization time:" + (System.currentTimeMillis() - ms + "[ms]"));
}
private void computeDistanceToGridPoint(@NotNull final Point gridPoint) {
VPoint point = cellGrid.pointToCoord(gridPoint);
double distance = obstacles.stream().map(shape -> shape.distance(point)).min(Double::compareTo).orElse(Double.MAX_VALUE);
cellGrid.setValue(gridPoint, new CellState(distance, PathFindingTag.Reachable));
}
@Override
public double getPotential(@NotNull VPoint pos, @Nullable Agent agent) {
return cellGrid.getInterpolatedValueAt(pos).getLeft();
}
}
......@@ -8,6 +8,8 @@ import java.util.stream.Collectors;
import java.util.stream.IntStream;
import java.util.stream.Stream;
import org.apache.commons.lang3.tuple.Pair;
import org.jetbrains.annotations.NotNull;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.math.InterpolationUtil;
import org.vadere.util.math.MathUtil;
......@@ -339,4 +341,33 @@ public class CellGrid {
return InterpolationUtil.bilinearInterpolation(z1, z2, z3, z4, t, u);
};
}
public Pair<Double, Double> getInterpolatedValueAt(@NotNull final VPoint pos) {
Point gridPoint = getNearestPointTowardsOrigin(pos);
VPoint gridPointCoord = pointToCoord(gridPoint);
int incX = 1, incY = 1;
double gridPotentials[] = new double[4];
if (pos.x >= getWidth()) {
incX = 0;
}
if (pos.y >= getHeight()) {
incY = 0;
}
gridPotentials[0] = getValue(gridPoint).potential;
gridPotentials[1] = getValue(gridPoint.x + incX, gridPoint.y).potential;
gridPotentials[2] = getValue(gridPoint.x + incX, gridPoint.y + incY).potential;
gridPotentials[3] = getValue(gridPoint.x, gridPoint.y + incY).potential;
/* Interpolate the known (potential < Double.MAX_VALUE) values. */
Pair<Double, Double> result = InterpolationUtil.bilinearInterpolationWithUnkown(
gridPotentials,
(pos.x - gridPointCoord.x) / getResolution(),
(pos.y - gridPointCoord.y) / getResolution());
return result;
}
}
......@@ -76,31 +76,10 @@ public interface EikonalSolver {
}
default double getPotential(final CellGrid potentialField, final VPoint pos, final double unknownPenalty, final double weight) {
double targetPotential = Double.MAX_VALUE;
Point gridPoint = potentialField.getNearestPointTowardsOrigin(pos);
VPoint gridPointCoord = potentialField.pointToCoord(gridPoint);
int incX = 1, incY = 1;
double gridPotentials[] = new double[4];
if (pos.x >= potentialField.getWidth()) {
incX = 0;
}
if (pos.y >= potentialField.getHeight()) {
incY = 0;
}
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;
double targetPotential = Double.MAX_VALUE;
/* Interpolate the known (potential < Double.MAX_VALUE) values. */
Pair<Double, Double> result = InterpolationUtil.bilinearInterpolationWithUnkown(
gridPotentials,
(pos.x - gridPointCoord.x) / potentialField.getResolution(),
(pos.y - gridPointCoord.y) / potentialField.getResolution());
Pair<Double, Double> result = potentialField.getInterpolatedValueAt(pos);
double tmpPotential = result.getLeft();
......
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