Commit 21ad9c1d authored by Stefan Schuhbaeck's avatar Stefan Schuhbaeck
Browse files

Merge branch 'master' into 151_diff_origin

parents 184bdd82 e9873189
Pipeline #76228 failed with stages
in 46 minutes and 3 seconds
package org.vadere.simulator.models.ode;
import java.util.Collection;
import java.util.List;
import java.util.Random;
import java.util.stream.Collectors;
import org.apache.commons.math3.exception.MathIllegalNumberException;
import org.apache.commons.math3.ode.FirstOrderIntegrator;
......@@ -13,6 +15,7 @@ import org.vadere.state.scenario.Car;
import org.vadere.state.scenario.DynamicElement;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Topography;
import org.vadere.state.simulation.FootStep;
import org.vadere.state.types.ScenarioElementType;
import org.vadere.util.geometry.shapes.Vector2D;
import org.vadere.util.geometry.shapes.VPoint;
......@@ -72,6 +75,9 @@ public abstract class ODEModel<T extends DynamicElement, TAttributes extends Att
// get pedestrian and car data
Collection<T> dynamicElements = topography.getElements(type);
List<T> orderedDynamicElements = topography.getElements(type).stream().collect(Collectors.toList());
List<VPoint> positions = orderedDynamicElements.stream().map(ped -> ped.getPosition()).collect(Collectors.toList());
double[] y;
// if no peds are present, return
......@@ -117,6 +123,15 @@ public abstract class ODEModel<T extends DynamicElement, TAttributes extends Att
}
updateElementPositions(type, simTimeInSec, topography, equations, y);
for(int i = 0; i < orderedDynamicElements.size(); i++) {
DynamicElement element = orderedDynamicElements.get(i);
if (element.getType() == ScenarioElementType.PEDESTRIAN) {
Pedestrian pedestrian = (Pedestrian)element;
pedestrian.clearFootSteps();
pedestrian.getFootSteps().add(new FootStep(positions.get(i), pedestrian.getPosition(), lastSimTimeInSec, simTimeInSec));
}
}
}
// reset the time
......@@ -128,8 +143,7 @@ public abstract class ODEModel<T extends DynamicElement, TAttributes extends Att
* double vector.
* The {@link AbstractModelEquations} are used to get the correct positions from the vector.
*/
public static <T extends DynamicElement> void updateElementPositions(Class<T> type, double simTimeInSec,
Topography topography, AbstractModelEquations equations, double[] y) {
public static <T extends DynamicElement> void updateElementPositions(Class<T> type, double t, Topography topography, AbstractModelEquations equations, double[] y) {
Collection<T> dynamicElements = topography.getElements(type);
......
......@@ -25,9 +25,8 @@ public class UpdateSchemeEventDriven implements UpdateSchemeOSM {
@Override
public void update(final double timeStepInSec, final double currentTimeInSec) {
for(PedestrianOSM pedestrianOSM : topography.getElements(PedestrianOSM.class)) {
pedestrianOSM.clearStrides();
}
clearStrides(topography);
if(!pedestrianEventsQueue.isEmpty()) {
// event driven update ignores time credits!
......
......@@ -11,6 +11,7 @@ import org.vadere.state.scenario.DynamicElementAddListener;
import org.vadere.state.scenario.DynamicElementRemoveListener;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Topography;
import org.vadere.state.simulation.FootStep;
import org.vadere.state.types.UpdateType;
import org.vadere.util.geometry.shapes.Vector2D;
import org.vadere.util.geometry.shapes.VPoint;
......@@ -28,6 +29,16 @@ public interface UpdateSchemeOSM extends DynamicElementRemoveListener<Pedestrian
void update(double timeStepInSec, double currentTimeInSec);
default void clearStrides(@NotNull final Topography topography) {
/**
* strides and foot steps have no influence on the simulation itself, i.e. they are saved to analyse trajectories
*/
for(PedestrianOSM pedestrianOSM : topography.getElements(PedestrianOSM.class)) {
pedestrianOSM.clearStrides();
pedestrianOSM.clearFootSteps();
}
}
static UpdateSchemeOSM create(@NotNull final UpdateType updateType, @NotNull final Topography topography, final Random random) {
switch (updateType) {
case SEQUENTIAL: return new UpdateSchemeSequential(topography);
......@@ -73,6 +84,13 @@ public interface UpdateSchemeOSM extends DynamicElementRemoveListener<Pedestrian
default void makeStep(@NotNull final Topography topography, @NotNull final PedestrianOSM pedestrian, final double stepTime) {
VPoint currentPosition = pedestrian.getPosition();
VPoint nextPosition = pedestrian.getNextPosition();
// start time
double timeOfNextStep = pedestrian.getTimeOfNextStep();
// end time
double entTimeOfStep = pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep();
if (nextPosition.equals(currentPosition)) {
pedestrian.setTimeCredit(0);
pedestrian.setVelocity(new Vector2D(0, 0));
......@@ -85,7 +103,11 @@ public interface UpdateSchemeOSM extends DynamicElementRemoveListener<Pedestrian
pedestrian.setVelocity(pedVelocity);
}
pedestrian.getStrides().add(Pair.of(currentPosition.distance(nextPosition), pedestrian.getTimeOfNextStep()));
/**
* strides and foot steps have no influence on the simulation itself, i.e. they are saved to analyse trajectories
*/
pedestrian.getStrides().add(Pair.of(currentPosition.distance(nextPosition), timeOfNextStep));
pedestrian.getFootSteps().add(new FootStep(currentPosition, nextPosition, timeOfNextStep, entTimeOfStep));
}
}
......@@ -32,6 +32,8 @@ public class UpdateSchemeParallel implements UpdateSchemeOSM {
@Override
public void update(double timeStepInSec, double currentTimeInSec) {
clearStrides(topography);
movedPedestrians.clear();
CallMethod[] callMethods = {CallMethod.SEEK, CallMethod.MOVE, CallMethod.CONFLICTS, CallMethod.STEPS};
List<Future<?>> futures;
......
......@@ -18,6 +18,7 @@ public class UpdateSchemeSequential implements UpdateSchemeOSM {
@Override
public void update(double timeStepInSec, double currentTimeInSec) {
clearStrides(topography);
update(topography.getElements(Pedestrian.class), timeStepInSec);
}
......
......@@ -82,9 +82,9 @@ public class PotentialFieldPedestrianCompactSoftshell implements PotentialFieldA
}
/*
@Override
public double getAgentPotential(VPoint pos, Agent pedestrian,
/*@Override
public double getAgentPotential(IPoint pos, Agent pedestrian,
Agent otherPedestrian) {
double radii = pedestrian.getRadius() + otherPedestrian.getRadius(); // 2* r_p (sivers-2016b)
double potential = 0;
......@@ -98,18 +98,18 @@ public class PotentialFieldPedestrianCompactSoftshell implements PotentialFieldA
int perPower = this.attributes.getPersonalSpacePower(); // not defined in sivers-2016b (perPower = 1)
double factor = this.attributes.getIntimateSpaceFactor(); // a_p
if (distance < personalWidth + otherPedestrian.getRadius()) {
if (distance < personalWidth + pedestrian.getRadius()) {
// implementation differs from sivers-2016b here: \delta_{per} + r_p (note: radii = 2*r_p)
potential += this.height * Math.exp(4 / (Math.pow(distance / (personalWidth + radii), (2 * perPower)) - 1));
potential += this.height * Math.exp(4 / (Math.pow(distance / (personalWidth + pedestrian.getRadius()), (2 * perPower)) - 1));
}
if (distance < this.intimateWidth + otherPedestrian.getRadius()) {
if (distance < this.intimateWidth + pedestrian.getRadius()) {
// implementation differs from sivers-2016b here: \delta_{int} + r_p (note: radii = 2*r_p)
potential += this.height / factor
* Math.exp(4 / (Math.pow(distance / (this.intimateWidth + pedestrian.getRadius()), (2 * intPower)) - 1));
}
if (distance < radii) {
// implementations differs from sivers-2016b here : Math.power(distance / (radii),2)
potential += 1000 * Math.exp(1 / (Math.pow(distance / pedestrian.getRadius(), 2) - 1));
potential += 1000 * Math.exp(1 / (Math.pow(distance / radii, 2) - 1));
}
}
return potential;
......
package org.vadere.simulator.models.potential.solver.calculators.mesh;
import org.apache.commons.lang3.tuple.Triple;
import org.apache.log4j.Level;
import org.apache.log4j.LogManager;
import org.apache.log4j.Logger;
import org.jetbrains.annotations.NotNull;
import org.vadere.meshing.utils.debug.DebugGui;
import org.vadere.meshing.utils.debug.SimpleTriCanvas;
import org.vadere.util.geometry.GeometryUtils;
import org.vadere.meshing.mesh.inter.IFace;
import org.vadere.meshing.mesh.inter.IHalfEdge;
......@@ -13,7 +12,6 @@ import org.vadere.meshing.mesh.inter.IMesh;
import org.vadere.meshing.mesh.inter.IIncrementalTriangulation;
import org.vadere.meshing.mesh.inter.IVertex;
import org.vadere.util.geometry.shapes.IPoint;
import org.vadere.util.geometry.shapes.VLine;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.VShape;
import org.vadere.util.math.InterpolationUtil;
......@@ -24,13 +22,16 @@ import org.vadere.util.data.cellgrid.IPotentialPoint;
import org.vadere.simulator.models.potential.solver.timecost.ITimeCostFunction;
import org.vadere.util.math.IDistanceFunction;
import java.awt.*;
import java.util.Collection;
import java.util.Comparator;
import java.util.HashMap;
import java.util.HashSet;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import java.util.Optional;
import java.util.PriorityQueue;
import java.util.Set;
import java.util.function.Function;
import java.util.function.Predicate;
......@@ -48,6 +49,10 @@ import java.util.function.Predicate;
public class EikonalSolverFMMTriangulation<P extends IPotentialPoint, V extends IVertex<P>, E extends IHalfEdge<P>, F extends IFace<P>> implements EikonalSolver {
private static Logger logger = LogManager.getLogger(EikonalSolverFMMTriangulation.class);
private Set<F> nonAccuteTris = new HashSet<>();
private Map<Triple<P, P, P>, Double> angles = new HashMap();
private Map<Triple<P, P, P>, Double> sinPhis = new HashMap();
private Map<Triple<P, P, P>, Double> cosPhis = new HashMap();
static {
logger.setLevel(Level.INFO);
......@@ -58,6 +63,10 @@ public class EikonalSolverFMMTriangulation<P extends IPotentialPoint, V extends
*/
private ITimeCostFunction timeCostFunction;
private IDistanceFunction distFunc;
private Collection<V> targetVertices;
/**
* The triangulation the solver uses.
*/
......@@ -154,30 +163,12 @@ public class EikonalSolverFMMTriangulation<P extends IPotentialPoint, V extends
this.calculationFinished = false;
this.timeCostFunction = timeCostFunction;
this.narrowBand = new PriorityQueue<>(pointComparator);
for(V vertex : targetVertices) {
P potentialPoint = getMesh().getPoint(vertex);
double distance = -distFunc.apply(potentialPoint);
if(potentialPoint.getPathFindingTag() != PathFindingTag.Undefined) {
narrowBand.remove(vertex);
}
potentialPoint.setPotential(Math.min(potentialPoint.getPotential(), distance / timeCostFunction.costAt(potentialPoint)));
potentialPoint.setPathFindingTag(PathFindingTag.Reached);
narrowBand.add(vertex);
for(V v : triangulation.getMesh().getAdjacentVertexIt(vertex)) {
P potentialP = getMesh().getPoint(v);
if(potentialP.getPathFindingTag() == PathFindingTag.Undefined) {
double dist = Math.max(-distFunc.apply(potentialP), 0);
logger.debug("T at " + potentialP + " = " + dist);
potentialP.setPotential(Math.min(potentialP.getPotential(), dist / timeCostFunction.costAt(potentialP)));
potentialP.setPathFindingTag(PathFindingTag.Reachable);
narrowBand.add(v);
}
}
this.distFunc = distFunc;
this.targetVertices = targetVertices;
for(F face : triangulation.getMesh().getFaces()) {
if(isNonAcute(face)) {
nonAccuteTris.add(face);
}
}
}
......@@ -218,6 +209,37 @@ public class EikonalSolverFMMTriangulation<P extends IPotentialPoint, V extends
}
}
public void reset() {
triangulation.getMesh().streamPoints().forEach(p -> p.setPathFindingTag(PathFindingTag.Undefined));
triangulation.getMesh().streamPoints().forEach(p -> p.setPotential(Double.MAX_VALUE));
calculationFinished = false;
for(V vertex : targetVertices) {
P potentialPoint = getMesh().getPoint(vertex);
double distance = -distFunc.apply(potentialPoint);
if(potentialPoint.getPathFindingTag() != PathFindingTag.Undefined) {
narrowBand.remove(vertex);
}
potentialPoint.setPotential(Math.min(potentialPoint.getPotential(), distance / timeCostFunction.costAt(potentialPoint)));
potentialPoint.setPathFindingTag(PathFindingTag.Reached);
narrowBand.add(vertex);
for(V v : triangulation.getMesh().getAdjacentVertexIt(vertex)) {
P potentialP = getMesh().getPoint(v);
if(potentialP.getPathFindingTag() == PathFindingTag.Undefined) {
double dist = Math.max(-distFunc.apply(potentialP), 0);
logger.debug("T at " + potentialP + " = " + dist);
potentialP.setPotential(Math.min(potentialP.getPotential(), dist / timeCostFunction.costAt(potentialP)));
potentialP.setPathFindingTag(PathFindingTag.Reachable);
narrowBand.add(v);
}
}
}
}
// unknownPenalty is ignored.
@Override
public double getPotential(@NotNull final IPoint pos, final double unknownPenalty, final double weight) {
......@@ -358,6 +380,10 @@ public class EikonalSolverFMMTriangulation<P extends IPotentialPoint, V extends
return angle1 > rightAngle + GeometryUtils.DOUBLE_EPS;
}
private boolean isNonAcute(@NotNull final F face) {
return isNonAcute(triangulation.getMesh().getEdge(face));
}
/**
* Updates a point given a triangle. The point can only be updated if the
* triangle triangleContains it and the other two points are in the frozen band.
......@@ -378,10 +404,10 @@ public class EikonalSolverFMMTriangulation<P extends IPotentialPoint, V extends
if(isFeasibleForComputation(p1) && isFeasibleForComputation(p2)) {
if(!isNonAcute(halfEdge)) {
if(!nonAccuteTris.contains(face)) {
double potential = computePotential(point, p1, p2);
//logger.info("compute potential " + potential);
return computePotential(point, p1, p2);
return computePotential(point, p1, p2);
} // we only try to find a virtual vertex if both points are already frozen
else {
logger.debug("special case for non-acute triangle");
......@@ -520,7 +546,6 @@ public class EikonalSolverFMMTriangulation<P extends IPotentialPoint, V extends
* @return the traveling time T at <tt>point</tt> by using the triangle (point, point1, point2) for the computation
*/
private double computePotential(final P point, final P point1, final P point2) {
// see: Sethian, Level Set Methods and Fast Marching Methods, page 124.
P p1; // A
P p2; // B
......@@ -543,15 +568,17 @@ public class EikonalSolverFMMTriangulation<P extends IPotentialPoint, V extends
double b = p1.distance(point);
double c = p1.distance(p2);
double phi = GeometryUtils.angle(p1, point, p2);
double cosphi = Math.cos(phi);
//double phi = angle(p1, point, p2);
double cosphi = cosPhi(p1, point, p2);
double sinPhi = sinPhi(p1, point, p2);
double F = 1.0 / timeCostFunction.costAt(point);
// solve x2 t^2 + x1 t + x0 == 0
double x2 = a * a + b * b - 2 * a * b * cosphi;
double x1 = 2 * b * u * (a * cosphi - b);
double x0 = b * b * (u * u - F * F * a * a * Math.sin(phi) * Math.sin(phi));
double x0 = b * b * (u * u - F * F * a * a * sinPhi * sinPhi);
double t = solveQuadratic(x2, x1, x0);
double inTriangle = (b * (t - u) / t);
......@@ -562,6 +589,42 @@ public class EikonalSolverFMMTriangulation<P extends IPotentialPoint, V extends
}
}
private double sinPhi(P p1, P p, P p2) {
Double sinPhi = sinPhis.get(Triple.of(p, p1, p2));
if(sinPhi == null) {
sinPhi = Math.sin(angle(p1, p, p2));
sinPhis.put(Triple.of(p, p1, p2), sinPhi);
return sinPhi;
}
else {
return sinPhi;
}
}
private double cosPhi(P p1, P p, P p2) {
Double cosPhi = cosPhis.get(Triple.of(p, p1, p2));
if(cosPhi == null) {
cosPhi = Math.cos(angle(p1, p, p2));
cosPhis.put(Triple.of(p, p1, p2), cosPhi);
return cosPhi;
}
else {
return cosPhi;
}
}
private double angle(P p1, P p, P p2) {
Double angle = angles.get(Triple.of(p, p1, p2));
if(angle == null) {
angle = GeometryUtils.angle(p1, p, p2);
angles.put(Triple.of(p, p1, p2), angle);
return angle;
}
else {
return angle;
}
}
/**
* Solves the quadratic equation given by (a*x^2+b*x+c=0).
*
......
......@@ -4,6 +4,7 @@ import java.util.Random;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.simulation.FootStep;
import org.vadere.util.geometry.shapes.Vector2D;
import org.vadere.util.geometry.shapes.VPoint;
......@@ -11,12 +12,14 @@ public class PedestrianReynolds extends Pedestrian {
private Vector2D lastMovement;
private double startTime;
private double lastSimTimeInSec;
public PedestrianReynolds(AttributesAgent attributesPedestrian, Random random) {
super(attributesPedestrian, random);
this.lastMovement = new Vector2D(0, 0);
this.startTime = -1;
this.lastSimTimeInSec = -1;
}
public VPoint getLastMovement() {
......@@ -32,9 +35,16 @@ public class PedestrianReynolds extends Pedestrian {
if (startTime < 0) {
startTime = simTime;
lastSimTimeInSec = 0;
}
this.setPosition(this.getPosition().add(mov));
VPoint oldPosition = getPosition();
VPoint newPosition = oldPosition.add(mov);
setPosition(newPosition);
clearFootSteps();
getFootSteps().add(new FootStep(oldPosition, newPosition, lastSimTimeInSec, simTime));
lastSimTimeInSec = simTime;
}
}
package org.vadere.simulator.projects.dataprocessing.processor;
import org.vadere.annotation.factories.dataprocessors.DataProcessorClass;
import org.vadere.simulator.control.SimulationState;
import org.vadere.simulator.projects.dataprocessing.ProcessorManager;
import org.vadere.simulator.projects.dataprocessing.datakey.PedestrianIdKey;
import org.vadere.simulator.projects.dataprocessing.datakey.TimestepKey;
import org.vadere.simulator.projects.dataprocessing.datakey.TimestepPedestrianIdKey;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.simulation.FootStep;
import org.vadere.state.util.StateJsonConverter;
import java.util.LinkedList;
import java.util.Map;
import java.util.stream.Collectors;
/**
* During one time step a pedestrian my move multiple times which is saved by {@link Pedestrian#getFootSteps()}, i.e. the list of {@link FootStep}s
* will be adjusted after each update(simTimeInSec) call such that it contains the foot steps which started at the lastSimTimeInSec!
*
* This processor writes out all those {@link FootStep}s using the standard JSON-format, e.g. one foot steps:
* [{"startTime":26.588661014252686,"endTime":27.123123483931312,"start":{"x":29.4730189272315,"y":24.965262390895376},"end":{"x":29.59817287115996,"y":25.182035380547074}}]
*
* @author Benedikt Zoennchen
*/
@DataProcessorClass()
public class PedestrianFootStepProcessor extends DataProcessor<TimestepPedestrianIdKey, LinkedList<FootStep>>{
private double lastSimTime;
public PedestrianFootStepProcessor() {
super("strides");
lastSimTime = 0.0;
}
Map<PedestrianIdKey, LinkedList<FootStep>> getPositions(TimestepKey timestepKey) {
return this.getData().entrySet().stream()
.filter(e -> e.getKey().getTimestep().equals(timestepKey.getTimestep()))
.collect(Collectors.toMap(e -> new PedestrianIdKey(e.getKey().getPedestrianId()), Map.Entry::getValue));
}
@Override
protected void doUpdate(final SimulationState state) {
Integer timeStep = state.getStep();
for (Pedestrian pedestrian : state.getTopography().getElements(Pedestrian.class)) {
LinkedList<FootStep> copy = pedestrian.getFootSteps()
.stream()
//.filter(footStep -> footStep.getEndTime() > lastSimTime)
//.filter(footStep -> footStep.getEndTime() <= state.getSimTimeInSec())
.collect(Collectors.toCollection(LinkedList::new));
putValue(new TimestepPedestrianIdKey(timeStep, pedestrian.getId()), copy);
}
lastSimTime = state.getSimTimeInSec();
}
@Override
public void init(final ProcessorManager manager) {
super.init(manager);
}
@Override
public String[] toStrings(TimestepPedestrianIdKey key) {
LinkedList<FootStep> strides = this.getValue(key);
StringBuilder builder = new StringBuilder();
if(strides == null) {
return new String[]{"{}"};
}
else {
builder.append("[");
String stridesString = StateJsonConverter.serialidzeObject(strides);
builder.append("]");
return new String[]{stridesString};
}
}
}
\ No newline at end of file
package org.vadere.simulator.projects.dataprocessing.processor;
import org.vadere.simulator.control.SimulationState;
import org.vadere.simulator.projects.dataprocessing.datakey.PedestrianIdKey;
import org.vadere.state.attributes.processor.AttributesPedestrianLineCrossProcessor;
public class PedestrianLineCrossProcessor extends DataProcessor<PedestrianIdKey, Double> {
public PedestrianLineCrossProcessor() {
super("crossTime");
setAttributes(new AttributesPedestrianLineCrossProcessor());
}
@Override
protected void doUpdate(SimulationState state) {
}
}
package org.vadere.simulator.projects.io;
import com.fasterxml.jackson.core.type.TypeReference;
import org.apache.commons.math3.util.Pair;
import org.apache.log4j.LogManager;
import org.apache.log4j.Logger;
......@@ -9,7 +11,9 @@ import org.vadere.simulator.projects.dataprocessing.processor.PedestrianPosition
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.scenario.Agent;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.simulation.FootStep;
import org.vadere.state.simulation.Step;
import org.vadere.state.util.StateJsonConverter;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.io.IOUtils;
......@@ -52,6 +56,7 @@ public class TrajectoryReader {
private Set<String> targetIdKeys;