Commit 65f369b8 authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files

issue #260: fix problem with future foot steps.

parent 4b5b8c78
Pipeline #150411 failed with stages
in 63 minutes and 51 seconds
......@@ -66,6 +66,7 @@ public class PedestrianBHM extends Pedestrian {
this.random = random;
this.attributesBHM = attributesBHM;
this.topography = topography;
this.timeOfNextStep = INVALID_NEXT_EVENT_TIME;
this.setVelocity(new Vector2D(0, 0));
......@@ -170,26 +171,34 @@ public class PedestrianBHM extends Pedestrian {
}
// for the first step after creation, timeOfNextStep has to be initialized
if (getTimeOfNextStep() == 0) {
if (getTimeOfNextStep() == INVALID_NEXT_EVENT_TIME) {
timeOfNextStep = currentTimeInSec;
return;
}
durationNextStep = stepLength / getFreeFlowSpeed();
double startTimeStep = timeOfNextStep;
double endTimeStep = timeOfNextStep + durationNextStep;
timeOfNextStep = endTimeStep;
Event mostImportantEvent = getMostImportantEvent();
VPoint position = getPosition();
if (mostImportantEvent instanceof ElapsedTimeEvent) {
updateTargetDirection();
nextPosition = navigation.getNavigationPosition();
makeStep();
timeOfNextStep += durationNextStep;
} else if (mostImportantEvent instanceof WaitEvent || mostImportantEvent instanceof WaitInAreaEvent) {
timeOfNextStep += durationNextStep;
// do nothing
} else {
throw new UnsupportedEventException(mostImportantEvent, this.getClass());
}
addFootStepToTrajectory(new FootStep(position, getPosition(), timeOfNextStep, timeOfNextStep + durationNextStep));
if(currentTimeInSec < startTimeStep) {
System.out.println("WTF");
}
addFootStepToTrajectory(new FootStep(position, getPosition(), startTimeStep, endTimeStep));
}
/**
......
......@@ -2,11 +2,14 @@ package org.vadere.simulator.models.osm;
import org.apache.commons.lang3.tuple.Pair;
import org.jetbrains.annotations.NotNull;
import org.jetbrains.annotations.Nullable;
import org.vadere.simulator.models.potential.combinedPotentials.CombinedPotentialStrategy;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.behavior.SalientBehavior;
import org.vadere.state.events.types.BangEvent;
import org.vadere.state.events.types.Event;
import org.vadere.state.events.types.WaitEvent;
import org.vadere.state.events.types.WaitInAreaEvent;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Target;
import org.vadere.state.scenario.Topography;
......@@ -16,6 +19,7 @@ import org.vadere.util.geometry.shapes.Vector2D;
import java.util.LinkedList;
import java.util.List;
import java.util.PriorityQueue;
import java.util.stream.Collectors;
/**
......@@ -81,8 +85,8 @@ public class OSMBehaviorController {
pedestrian.addFootStepToTrajectory(new FootStep(currentPosition, nextPosition, stepStartTime, stepEndTime));
}
public void wait(PedestrianOSM pedestrian) {
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep());
public void wait(PedestrianOSM pedestrian, double timeStepInSec) {
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + timeStepInSec);
}
// Watch out: A bang event changes only the "CombinedPotentialStrategy".
......@@ -110,6 +114,9 @@ public class OSMBehaviorController {
* Try to swap the given pedestrian with the closest cooperative pedestrian.
* Carry out the following steps:
*
* TODO: Refactoring, this class should not have access to the event queue. Idea: use "old" events which will be ignored i.e.
* one agent might have multiple events in the queue.
*
* <ol>
* <li>Use topography to find a close pedestrian within step circle which is closer to target than the given pedestrian.</li>
* <li>Check if candidate is SalientBehavior.COOPERATIVE.</li>
......@@ -120,13 +127,12 @@ public class OSMBehaviorController {
* @param pedestrian The pedestrian which would like to swap the position.
* @param topography The topography is required to find the neighbors of the given pedestrian.
*/
public void swapWithClosestCooperativePedestrian(PedestrianOSM pedestrian, Topography topography) {
/*public void swapWithClosestCooperativePedestrian(PedestrianOSM pedestrian, Topography topography, PriorityQueue<PedestrianOSM> queue) {
if (pedestrian.hasNextTarget() == false) { // Ignore pedestrians with no targets.
// this can cause problems if the pedestrian desired speed is 0 (see speed adjuster)
pedestrian.updateNextPosition();
makeStep(pedestrian, topography, pedestrian.getDurationNextStep());
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep());
return;
}
......@@ -146,7 +152,7 @@ public class OSMBehaviorController {
}
if (closestPedIsCooperative && targetOrientationDiffers) {
swapPedestrians(pedestrian, (PedestrianOSM)closestPedestrian, topography);
swapPedestrians(pedestrian, (PedestrianOSM)closestPedestrian, topography, queue);
pedestriansSwapped = true;
break;
}
......@@ -159,6 +165,34 @@ public class OSMBehaviorController {
makeStep(pedestrian, topography, pedestrian.getDurationNextStep());
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep());
}
}*/
@Nullable
public PedestrianOSM findSwapCandidate(PedestrianOSM pedestrian, Topography topography) {
if(pedestrian.hasNextTarget() == false) {
return null;
}
List<Pedestrian> closestPedestrians = getClosestPedestriansWhichAreCloserToTarget(pedestrian, topography);
if (closestPedestrians.size() > 0) {
for (Pedestrian closestPedestrian : closestPedestrians) {
boolean closestPedIsCooperative = closestPedestrian.getSalientBehavior() == SalientBehavior.COOPERATIVE;
boolean targetOrientationDiffers = false;
// TODO: Use "pedestrian.getTargetGradient()" instead of "calculateAngleBetweenTargets()".
double angleInRadian = calculateAngleBetweenTargets(pedestrian, closestPedestrian, topography);
if (angleInRadian == -1 || Math.toDegrees(angleInRadian) > pedestrian.getAttributes().getTargetOrientationAngleThreshold()) {
targetOrientationDiffers = true;
}
if (closestPedIsCooperative && targetOrientationDiffers) {
return (PedestrianOSM)closestPedestrian;
}
}
}
return null;
}
@NotNull
......@@ -241,7 +275,7 @@ public class OSMBehaviorController {
return vectorPedestrianToTarget;
}
private void swapPedestrians(PedestrianOSM pedestrian1, PedestrianOSM pedestrian2, Topography topography) {
public void swapPedestrians(PedestrianOSM pedestrian1, PedestrianOSM pedestrian2, Topography topography) {
VPoint newPosition = pedestrian2.getPosition().clone();
VPoint oldPosition = pedestrian1.getPosition().clone();
......@@ -250,13 +284,26 @@ public class OSMBehaviorController {
// Use "makeStep()" to swap both pedestrians to avoid "java.lang.AssertionError:
// Number of pedestrians in LinkedCellGrid does not match number of pedestrians in topography".
makeStep(pedestrian1, topography, pedestrian1.getDurationNextStep());
makeStep(pedestrian2, topography, pedestrian2.getDurationNextStep());
double startTimeStep = pedestrian1.getTimeOfNextStep();
double durationStep = pedestrian1.getDurationNextStep();
double endTimeStep = startTimeStep + durationStep;
// Note: Here we manipulate a pedestrian which is contained in the queue sorted by timeOfNextStep!
pedestrian1.setTimeOfNextStep(startTimeStep);
pedestrian2.setTimeOfNextStep(startTimeStep);
makeStep(pedestrian1, topography, durationStep);
makeStep(pedestrian2, topography, durationStep);
// TODO The experiment showed that speed decreased (to half of free-flow velocity).
// Therefore, use "pedestrian.getDurationNextStep() * 2".
pedestrian1.setTimeOfNextStep(pedestrian1.getTimeOfNextStep() + pedestrian1.getDurationNextStep());
pedestrian2.setTimeOfNextStep(pedestrian2.getTimeOfNextStep() + pedestrian2.getDurationNextStep());
}
pedestrian1.setTimeOfNextStep(endTimeStep);
pedestrian2.setTimeOfNextStep(endTimeStep);
pedestrian1.setTimeCredit(pedestrian1.getTimeCredit() - durationStep);
if(pedestrian2.getMostImportantEvent() instanceof WaitEvent || pedestrian2.getMostImportantEvent() instanceof WaitInAreaEvent) {
pedestrian2.setTimeCredit(pedestrian1.getTimeCredit() - durationStep);
}
}
}
......@@ -82,7 +82,7 @@ public class PedestrianOSM extends Pedestrian {
this.speedAdjusters = speedAdjusters;
this.stepSizeAdjusters = new LinkedList<>();
this.relevantPedestrians = new HashSet<>();
this.timeCredit = 0;
this.timeCredit = INVALID_NEXT_EVENT_TIME;
this.setVelocity(new Vector2D(0, 0));
......
......@@ -50,28 +50,36 @@ public class UpdateSchemeEventDriven implements UpdateSchemeOSM {
}
protected void update(@NotNull final PedestrianOSM pedestrian, final double timeStepInSec, final double currentTimeInSec) {
// for the first step after creation, timeOfNextStep has to be initialized
if (pedestrian.getTimeOfNextStep() == Pedestrian.INVALID_NEXT_EVENT_TIME) {
pedestrian.setTimeOfNextStep(currentTimeInSec);
return;
}
Event mostImportantEvent = pedestrian.getMostImportantEvent();
if (mostImportantEvent instanceof ElapsedTimeEvent) {
VPoint oldPosition = pedestrian.getPosition();
double stepDuration = pedestrian.getDurationNextStep();
// for the first step after creation, timeOfNextStep has to be initialized
if (pedestrian.getTimeOfNextStep() == 0) {
pedestrian.setTimeOfNextStep(currentTimeInSec - timeStepInSec);
}
if (pedestrian.getSalientBehavior() == SalientBehavior.TARGET_ORIENTED) {
// this can cause problems if the pedestrian desired speed is 0 (see speed adjuster)
pedestrian.updateNextPosition();
osmBehaviorController.makeStep(pedestrian, topography, stepDuration);
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + stepDuration);
} else if (pedestrian.getSalientBehavior() == SalientBehavior.COOPERATIVE) {
osmBehaviorController.swapWithClosestCooperativePedestrian(pedestrian, topography);
// this call will also invoke setTimeOfNextStep
PedestrianOSM candidate = osmBehaviorController.findSwapCandidate(pedestrian, topography);
if(candidate != null) {
pedestrianEventsQueue.remove(candidate);
osmBehaviorController.swapPedestrians(pedestrian, candidate, topography);
pedestrianEventsQueue.add(candidate);
} else {
pedestrian.updateNextPosition();
osmBehaviorController.makeStep(pedestrian, topography, pedestrian.getDurationNextStep());
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep());
}
}
} else if (mostImportantEvent instanceof WaitEvent || mostImportantEvent instanceof WaitInAreaEvent) {
osmBehaviorController.wait(pedestrian);
osmBehaviorController.wait(pedestrian, timeStepInSec);
} else if (mostImportantEvent instanceof BangEvent) {
osmBehaviorController.reactToBang(pedestrian, topography);
......
......@@ -15,6 +15,10 @@ import java.util.Collection;
import java.util.LinkedList;
import java.util.List;
/**
* TODO: explain the concept of timeCredit!
* TODO: in the long term, replace timeCredit by eventTime (see event driven update)!
*/
public class UpdateSchemeSequential implements UpdateSchemeOSM {
private final Topography topography;
......@@ -41,10 +45,10 @@ public class UpdateSchemeSequential implements UpdateSchemeOSM {
protected void update(@NotNull final PedestrianOSM pedestrian, final double timeStepInSec) {
Event mostImportantEvent = pedestrian.getMostImportantEvent();
pedestrian.setTimeCredit(pedestrian.getTimeCredit() + timeStepInSec);
if (mostImportantEvent instanceof ElapsedTimeEvent) {
VPoint oldPosition = pedestrian.getPosition();
pedestrian.clearStrides();
pedestrian.setTimeCredit(pedestrian.getTimeCredit() + timeStepInSec);
if (pedestrian.getSalientBehavior() == SalientBehavior.TARGET_ORIENTED) {
while (pedestrian.getTimeCredit() > pedestrian.getDurationNextStep()) {
......@@ -54,13 +58,21 @@ public class UpdateSchemeSequential implements UpdateSchemeOSM {
}
} else if (pedestrian.getSalientBehavior() == SalientBehavior.COOPERATIVE) {
osmBehaviorController.swapWithClosestCooperativePedestrian(pedestrian, topography);
PedestrianOSM candidate = osmBehaviorController.findSwapCandidate(pedestrian, topography);
if(candidate != null) {
osmBehaviorController.swapPedestrians(pedestrian, candidate, topography);
} else {
pedestrian.updateNextPosition();
osmBehaviorController.makeStep(pedestrian, topography, pedestrian.getDurationNextStep());
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep());
}
}
} else if (mostImportantEvent instanceof WaitEvent || mostImportantEvent instanceof WaitInAreaEvent) {
osmBehaviorController.wait(pedestrian);
osmBehaviorController.wait(pedestrian, timeStepInSec);
} else if (mostImportantEvent instanceof BangEvent) {
osmBehaviorController.reactToBang(pedestrian, topography);
pedestrian.setTimeCredit(pedestrian.getTimeCredit() - timeStepInSec);
}
}
......
......@@ -17,6 +17,7 @@ public class Pedestrian extends Agent {
// Constants
public static double PEDESTRIAN_MAX_SPEED_METER_PER_SECOND = 12.0;
public static final double INVALID_NEXT_EVENT_TIME = -1.0;
// Variables
private int idAsTarget; // TODO should actually be an attribute or a member of a subclass
......@@ -122,7 +123,6 @@ public class Pedestrian extends Agent {
}else{
if(time > currentFootStep.getEndTime()){
// This happens for example if a pedestrian is waiting (see Events)
// TODO: check with Bene K. if this is okay, or a better way?
return currentFootStep.getEnd();
}else{
return FootStep.interpolateFootStep(currentFootStep, time);
......
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