Currently job artifacts in CI/CD pipelines on LRZ GitLab never expire. Starting from Wed 26.1.2022 the default expiration time will be 30 days (GitLab default). Currently existing artifacts in already completed jobs will not be affected by the change. The latest artifacts for all jobs in the latest successful pipelines will be kept. More information: https://gitlab.lrz.de/help/user/admin_area/settings/continuous_integration.html#default-artifacts-expiration

Commit 6214738c authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files

replace obstacle distance computation by fast marching method.

parent 108e8601
......@@ -6,18 +6,28 @@ 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.Pedestrian;
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.awt.geom.Rectangle2D;
import java.util.*;
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 {
......@@ -71,6 +81,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,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;
......
......@@ -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,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);
}
......
......@@ -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