Commit df897e05 authored by Benedikt Kleinmeier's avatar Benedikt Kleinmeier
Browse files

Integrated "UpdateSchemeEventDriven" locomotion layer completely into...

Integrated "UpdateSchemeEventDriven" locomotion layer completely into "models/psychology/selfcategorization"
parent e8ddba60
......@@ -2,7 +2,9 @@ package org.vadere.simulator.control.psychology.perception;
import org.vadere.state.psychology.perception.types.*;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Target;
import org.vadere.state.scenario.Topography;
import org.vadere.util.geometry.shapes.VPoint;
import java.util.Collection;
import java.util.List;
......@@ -46,6 +48,7 @@ public class SimplePerceptionModel implements IPerceptionModel {
if (changeTargetStimuli.size() >= 1) {
mostImportantStimulus = changeTargetStimuli.get(0);
} else if (bangStimuli.size() >= 1) {
// TODO: Select closest bang where pedestrian is within bang radius as most important stimulus.
mostImportantStimulus = bangStimuli.get(0);
} else if (waitStimuli.size() >= 1) {
mostImportantStimulus = waitStimuli.get(0);
......
package org.vadere.simulator.models.psychology.selfcategorization.locomotion;
import org.apache.commons.lang3.tuple.Pair;
import org.jetbrains.annotations.NotNull;
import org.jetbrains.annotations.Nullable;
import org.vadere.simulator.models.osm.OptimalStepsModel;
import org.vadere.simulator.models.osm.PedestrianOSM;
import org.vadere.simulator.models.potential.combinedPotentials.CombinedPotentialStrategy;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.psychology.cognition.SelfCategory;
import org.vadere.state.psychology.perception.types.Bang;
import org.vadere.state.psychology.perception.types.ChangeTarget;
import org.vadere.state.psychology.perception.types.Stimulus;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Target;
import org.vadere.state.scenario.Topography;
import org.vadere.state.simulation.FootStep;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.Vector2D;
import org.vadere.util.logging.Logger;
import java.util.LinkedList;
import java.util.List;
import java.util.stream.Collectors;
/**
* A class to encapsulate the behavior of a single {@link PedestrianOSM}.
*
* This class can be used by {@link OptimalStepsModel} to react to
* environmental stimuli (see {@link Stimulus}) and how an agent
* has categorized itself in regard to other agents (see {@link SelfCategory}).
*
* For instance:
* <pre>
* ...
* if (mostImportantStimulus instanceof Wait) {
* osmBehaviorController.wait()
* }
* ...
* </pre>
*/
public class OSMBehaviorController {
private static Logger logger = Logger.getLogger(OSMBehaviorController.class);
public void makeStepToTarget(@NotNull final PedestrianOSM pedestrian, @NotNull final Topography topography) {
// 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());
}
/**
* Prepare move of pedestrian inside the topography. The pedestrian object already has the new
* location (Vpoint to) stored within its position attribute. This method only informs the
* topography object of the change in state.
*
* !IMPORTANT! this function calls movePedestrian which must be called ONLY ONCE for each
* pedestrian for each position. To allow preformat selection of a pedestrian the managing
* destructure is not idempotent (cannot be applied multiple time without changing result).
*
* @param topography manages simulation data
* @param pedestrian moving pedestrian. This object's position is already set.
* @param stepTime time in seconds used for the step.
*/
public void makeStep(@NotNull final PedestrianOSM pedestrian, @NotNull final Topography topography, final double stepTime) {
VPoint currentPosition = pedestrian.getPosition();
VPoint nextPosition = pedestrian.getNextPosition();
// start time
double stepStartTime = pedestrian.getTimeOfNextStep();
// end time
double stepEndTime = pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep();
assert stepEndTime >= stepStartTime && stepEndTime >= 0.0 && stepStartTime >= 0.0 : stepEndTime + "<" + stepStartTime;
if (nextPosition.equals(currentPosition)) {
pedestrian.setVelocity(new Vector2D(0, 0));
} else {
pedestrian.setPosition(nextPosition);
synchronized (topography) {
topography.moveElement(pedestrian, currentPosition);
}
// compute velocity by forward difference
Vector2D pedVelocity = new Vector2D(nextPosition.x - currentPosition.x, nextPosition.y - currentPosition.y).multiply(1.0 / stepTime);
pedestrian.setVelocity(pedVelocity);
}
// 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), stepStartTime));
FootStep currentFootstep = new FootStep(currentPosition, nextPosition, stepStartTime, stepEndTime);
pedestrian.getTrajectory().add(currentFootstep);
pedestrian.getFootstepHistory().add(currentFootstep);
}
/**
* This operation undo the last foot step of an agent. This is required to resolve conflicts by the {@link org.vadere.simulator.models.osm.updateScheme.UpdateSchemeParallel}.
*
* @param pedestrian the agent
* @param topography the topography
*/
public void undoStep(@NotNull final PedestrianOSM pedestrian, @NotNull final Topography topography) {
FootStep footStep = pedestrian.getTrajectory().removeLast();
pedestrian.getFootstepHistory().removeLast();
pedestrian.setPosition(footStep.getStart());
synchronized (topography) {
topography.moveElement(pedestrian, footStep.getEnd());
}
pedestrian.setVelocity(new Vector2D(0, 0));
}
public void wait(PedestrianOSM pedestrian, double timeStepInSec) {
// Satisfy event-driven and sequential update scheme.
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + timeStepInSec);
}
// Watch out: A bang event changes only the "CombinedPotentialStrategy".
// I.e., a new target is set for the agent. The agent does not move here!
// Therefore, trigger only a single bang event and then use "ElapsedTime" afterwards
// to let the agent walk.
public void reactToBang(PedestrianOSM pedestrian, Topography topography) {
Stimulus mostImportantStimulus = pedestrian.getMostImportantStimulus();
// TODO: Change only if "pedestrian.getCombinedPotentialStrategy != TARGET_DISTRACTION_STRATEGY".
if (mostImportantStimulus instanceof Bang) {
Bang bang = (Bang) pedestrian.getMostImportantStimulus();
Target bangOrigin = topography.getTarget(bang.getOriginAsTargetId());
LinkedList<Integer> nextTarget = new LinkedList<>();
nextTarget.add(bangOrigin.getId());
pedestrian.setTargets(nextTarget);
pedestrian.setCombinedPotentialStrategy(CombinedPotentialStrategy.TARGET_DISTRACTION_STRATEGY);
} else {
logger.debug(String.format("Expected: %s, Received: %s"),
Bang.class.getSimpleName(),
mostImportantStimulus.getClass().getSimpleName());
}
}
public void reactToTargetChange(PedestrianOSM pedestrian, Topography topography) {
Stimulus mostImportantStimulus = pedestrian.getMostImportantStimulus();
if (mostImportantStimulus instanceof ChangeTarget) {
ChangeTarget changeTarget = (ChangeTarget) pedestrian.getMostImportantStimulus();
pedestrian.setTargets(changeTarget.getNewTargetIds());
pedestrian.setNextTargetListIndex(0);
} else {
logger.debug(String.format("Expected: %s, Received: %s"),
ChangeTarget.class.getSimpleName(),
mostImportantStimulus.getClass().getSimpleName());
}
}
@Nullable
public PedestrianOSM findSwapCandidate(PedestrianOSM pedestrian, Topography topography) {
// Agents with no targets don't want to swap places.
if (pedestrian.hasNextTarget() == false) {
return null;
}
List<Pedestrian> closestPedestrians = getClosestPedestriansWhichAreCloserToTarget(pedestrian, topography);
if (closestPedestrians.size() > 0) {
for (Pedestrian closestPedestrian : closestPedestrians) {
if (closestPedestrian.hasNextTarget()) {
boolean closestPedIsCooperative = closestPedestrian.getSelfCategory() == SelfCategory.COOPERATIVE;
boolean walkingDirectionDiffers = false;
double angleInRadian = calculateAngleBetweenWalkingDirections(pedestrian, closestPedestrian, topography);
if (angleInRadian == -1 || Math.toDegrees(angleInRadian) > pedestrian.getAttributes().getWalkingDirectionSameIfAngleLessOrEqual()) {
walkingDirectionDiffers = true;
}
if (closestPedIsCooperative && walkingDirectionDiffers) {
return (PedestrianOSM)closestPedestrian;
}
} else {
return (PedestrianOSM)closestPedestrian;
}
}
}
return null;
}
@NotNull
private List<Pedestrian> getClosestPedestriansWhichAreCloserToTarget(PedestrianOSM pedestrian, Topography topography) {
VPoint positionOfPedestrian = pedestrian.getPosition();
List<Pedestrian> closestPedestrians = topography.getSpatialMap(Pedestrian.class)
.getObjects(positionOfPedestrian, pedestrian.getAttributes().getSearchRadius());
// Filter out "me" and pedestrians which are further away from target than "me".
closestPedestrians = closestPedestrians.stream()
.filter(candidate -> pedestrian.getId() != candidate.getId())
.filter(candidate -> pedestrian.getTargetPotential(candidate.getPosition()) < pedestrian.getTargetPotential(pedestrian.getPosition()))
.collect(Collectors.toList());
// Sort by distance away from "me".
closestPedestrians = closestPedestrians.stream()
.sorted((pedestrian1, pedestrian2) ->
Double.compare(
positionOfPedestrian.distance(pedestrian1.getPosition()),
positionOfPedestrian.distance(pedestrian2.getPosition())
))
.collect(Collectors.toList());
return closestPedestrians;
}
private double calculateAngleBetweenWalkingDirections(PedestrianOSM pedestrian1, Pedestrian pedestrian2, Topography topography) {
double angleInRadian = -1;
switch (pedestrian1.getAttributes().getWalkingDirectionCalculation()) {
case BY_GRADIENT:
angleInRadian = calculateAngleBetweenTargetGradients(pedestrian1, (PedestrianOSM)pedestrian2);
break;
case BY_TARGET_CENTER:
case BY_TARGET_CLOSEST_POINT:
angleInRadian = calculateAngleBetweenTargets(pedestrian1, pedestrian2, topography);
break;
default:
throw new IllegalArgumentException(String.format("Unsupported calculation type: \"%s\"",
pedestrian1.getAttributes().getWalkingDirectionCalculation()));
}
return angleInRadian;
}
public double calculateAngleBetweenTargetGradients(PedestrianOSM pedestrian1, PedestrianOSM pedestrian2) {
double angleInRadian = -1;
Vector2D targetGradientPedestrian1 = pedestrian1.getTargetGradient(pedestrian1.getPosition());
Vector2D targetGradientPedestrian2 = pedestrian2.getTargetGradient(pedestrian2.getPosition());
double dotProduct = targetGradientPedestrian1.dotProduct(targetGradientPedestrian2);
double multipliedMagnitudes = targetGradientPedestrian1.distanceToOrigin() * targetGradientPedestrian2.distanceToOrigin();
angleInRadian = Math.acos(dotProduct / multipliedMagnitudes);
return angleInRadian;
}
/**
* Calculate the angle between the two vectors v1 and v2 where
* v1 = (TargetPedestrian1 - pedestrian1) and v2 = (TargetPedestrian2 - pedestrian2):
*
* <pre>
* T2 o o T1
* ^ ^
* \a/
* x
* / \
* P1 o o P2
*
* T1: target of pedestrian 1
* T2: target of pedestrian 2
* P1: pedestrian 1
* P2: pedestrian 2
* a : angle between the two vectors
* </pre>
*
* This is required to decide if pedestrian1 and pedestrian2 can be swapped because they have different walking
* directions.
*
* @return An angle between 0 and <i>pi</i> radian or -1 if at least one of the given pedestrians has no target.
*/
public double calculateAngleBetweenTargets(Pedestrian pedestrian1, Pedestrian pedestrian2, Topography topography) {
double angleInRadian = -1;
if (pedestrian1.hasNextTarget() && pedestrian2.hasNextTarget()) {
Target targetPed1 = topography.getTarget(pedestrian1.getNextTargetId());
Target targetPed2 = topography.getTarget(pedestrian2.getNextTargetId());
VPoint targetVectorPed1 = calculateVectorPedestrianToTarget(pedestrian1, targetPed1);
VPoint targetVectorPed2 = calculateVectorPedestrianToTarget(pedestrian2, targetPed2);
double dotProduct = targetVectorPed1.dotProduct(targetVectorPed2);
double multipliedMagnitudes = targetVectorPed1.distanceToOrigin() * targetVectorPed2.distanceToOrigin();
angleInRadian = Math.acos(dotProduct / multipliedMagnitudes);
}
return angleInRadian;
}
private VPoint calculateVectorPedestrianToTarget(Pedestrian pedestrian, Target target) {
VPoint vectorPedestrianToTarget = null;
if (pedestrian.getAttributes().getWalkingDirectionCalculation() == AttributesAgent.WalkingDirectionCalculation.BY_TARGET_CENTER) {
vectorPedestrianToTarget = target.getShape().getCentroid().subtract(pedestrian.getPosition());
} else if (pedestrian.getAttributes().getWalkingDirectionCalculation() == AttributesAgent.WalkingDirectionCalculation.BY_TARGET_CLOSEST_POINT) {
VPoint closestTargetPoint = target.getShape().closestPoint(pedestrian.getPosition());
vectorPedestrianToTarget = closestTargetPoint.subtract(pedestrian.getPosition());
} else {
throw new IllegalArgumentException(String.format("Unsupported angle calculation type: \"%s\"", pedestrian.getAttributes().getWalkingDirectionCalculation()));
}
return vectorPedestrianToTarget;
}
/**
* Swap two pedestrians.
*
* Watch out: This method manipulates pedestrian2 which is contained in a queue
* sorted by timeOfNextStep! The calling code must re-add pedestrian2 after
* invoking this method.
*/
public void swapPedestrians(PedestrianOSM pedestrian1, PedestrianOSM pedestrian2, Topography topography) {
VPoint newPosition = pedestrian2.getPosition().clone();
VPoint oldPosition = pedestrian1.getPosition().clone();
pedestrian1.setNextPosition(newPosition);
pedestrian2.setNextPosition(oldPosition);
// Synchronize movement of both pedestrians
double startTimeStep = pedestrian1.getTimeOfNextStep();
double durationStep = Math.max(pedestrian1.getDurationNextStep(), pedestrian2.getDurationNextStep());
double endTimeStep = startTimeStep + durationStep;
// We interrupt the current footstep of pedestrian 2 to sync it with
// pedestrian 1. It is only required for the sequential update scheme
// since pedestrian 2 might have done some steps in this time step and
// is ahead (with respect to the time) of pedestrian 1.
// We remove those steps which is not a good solution!
if(!pedestrian2.getTrajectory().isEmpty()) {
pedestrian2.getTrajectory().adjustEndTime(startTimeStep);
}
pedestrian1.setTimeOfNextStep(startTimeStep);
pedestrian2.setTimeOfNextStep(startTimeStep);
makeStep(pedestrian1, topography, durationStep);
makeStep(pedestrian2, topography, durationStep);
pedestrian1.setTimeOfNextStep(endTimeStep);
pedestrian2.setTimeOfNextStep(endTimeStep);
}
}
package org.vadere.simulator.models.psychology.selfcategorization.locomotion;
import org.jetbrains.annotations.NotNull;
import org.vadere.simulator.models.osm.OSMBehaviorController;
import org.vadere.simulator.models.osm.PedestrianOSM;
import org.vadere.simulator.models.osm.updateScheme.UpdateSchemeOSM;
import org.vadere.simulator.models.psychology.selfcategorization.PedestrianSelfCatThreat;
import org.vadere.state.psychology.cognition.SelfCategory;
import org.vadere.state.psychology.perception.types.*;
import org.vadere.state.scenario.*;
......@@ -14,13 +13,13 @@ import java.util.PriorityQueue;
public class UpdateSchemeEventDriven implements DynamicElementAddListener, DynamicElementRemoveListener {
private final Topography topography;
protected PriorityQueue<PedestrianOSM> pedestrianEventsQueue;
protected PriorityQueue<PedestrianSelfCatThreat> pedestrianEventsQueue;
private final OSMBehaviorController osmBehaviorController;
public UpdateSchemeEventDriven(@NotNull final Topography topography) {
this.topography = topography;
this.pedestrianEventsQueue = new PriorityQueue<>(100, new ComparatorPedestrianOSM());
this.pedestrianEventsQueue.addAll(topography.getElements(PedestrianOSM.class));
this.pedestrianEventsQueue = new PriorityQueue<>(100, new ComparatorPedestrianSelfCatThreat());
this.pedestrianEventsQueue.addAll(topography.getElements(PedestrianSelfCatThreat.class));
this.osmBehaviorController = new OSMBehaviorController();
}
......@@ -29,25 +28,24 @@ public class UpdateSchemeEventDriven implements DynamicElementAddListener, Dynam
if(!pedestrianEventsQueue.isEmpty()) {
// event driven update ignores time credits!
while (pedestrianEventsQueue.peek().getTimeOfNextStep() < currentTimeInSec) {
PedestrianOSM ped = pedestrianEventsQueue.poll();
PedestrianSelfCatThreat ped = pedestrianEventsQueue.poll();
update(ped, timeStepInSec, currentTimeInSec);
//System.out.println(ped.getId());
pedestrianEventsQueue.add(ped);
}
}
}
private 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();
for(PedestrianSelfCatThreat pedestrian : topography.getElements(PedestrianSelfCatThreat.class)) {
pedestrian.clearStrides();
pedestrian.clearFootSteps();
}
}
private void update(@NotNull final PedestrianOSM pedestrian, final double timeStepInSec, final double currentTimeInSec) {
private void update(@NotNull final PedestrianSelfCatThreat 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);
......@@ -57,40 +55,29 @@ public class UpdateSchemeEventDriven implements DynamicElementAddListener, Dynam
Stimulus mostImportantStimulus = pedestrian.getMostImportantStimulus();
if (mostImportantStimulus instanceof ElapsedTime) {
double stepDuration = pedestrian.getDurationNextStep();
if (pedestrian.getSelfCategory() == SelfCategory.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);
osmBehaviorController.makeStepToTarget(pedestrian, topography);
} else if (pedestrian.getSelfCategory() == SelfCategory.COOPERATIVE) {
// this call will also invoke setTimeOfNextStep
PedestrianOSM candidate = osmBehaviorController.findSwapCandidate(pedestrian, topography);
//TODO: Benedikt Kleinmeier:
if(candidate != null) {
//if(Math.abs(pedestrian.getTimeOfNextStep() - candidate.getTimeOfNextStep()) < MathUtil.EPSILON) {
pedestrianEventsQueue.remove(candidate);
osmBehaviorController.swapPedestrians(pedestrian, candidate, topography);
pedestrianEventsQueue.add(candidate);
/*} else {
pedestrian.setTimeOfNextStep(candidate.getTimeOfNextStep());
}*/
pedestrianEventsQueue.remove(candidate);
osmBehaviorController.swapPedestrians(pedestrian, candidate, topography);
pedestrianEventsQueue.add((PedestrianSelfCatThreat)candidate);
} else {
pedestrian.updateNextPosition();
osmBehaviorController.makeStep(pedestrian, topography, pedestrian.getDurationNextStep());
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep());
osmBehaviorController.makeStepToTarget(pedestrian, topography);
}
// TODO: else if (pedestrian.getSelfCategory() == SelfCategory.LEFT_BANG_AREA)
// Change target to safe zone and "makeStepToTarget()".
}
} else if (mostImportantStimulus instanceof Wait || mostImportantStimulus instanceof WaitInArea) {
osmBehaviorController.wait(pedestrian, timeStepInSec);
} else if (mostImportantStimulus instanceof Bang) {
// TODO: Increase pedestrians free-flow velocity by a fixed factor of 1.5 or 2.
osmBehaviorController.reactToBang(pedestrian, topography);
// Set time of next step. Otherwise, the internal OSM event queue hangs endlessly.
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep());
osmBehaviorController.makeStepToTarget(pedestrian, topography);
} else if (mostImportantStimulus instanceof ChangeTarget) {
osmBehaviorController.reactToTargetChange(pedestrian, topography);
// Set time of next step. Otherwise, the internal OSM event queue hangs endlessly.
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep());
}
......@@ -98,7 +85,7 @@ public class UpdateSchemeEventDriven implements DynamicElementAddListener, Dynam
@Override
public void elementAdded(DynamicElement element) {
pedestrianEventsQueue.add((PedestrianOSM) element);
pedestrianEventsQueue.add((PedestrianSelfCatThreat) element);
}
@Override
......@@ -109,9 +96,9 @@ public class UpdateSchemeEventDriven implements DynamicElementAddListener, Dynam
/**
* Compares the time of the next possible move.
*/
private class ComparatorPedestrianOSM implements Comparator<PedestrianOSM> {
private class ComparatorPedestrianSelfCatThreat implements Comparator<PedestrianSelfCatThreat> {
@Override
public int compare(PedestrianOSM ped1, PedestrianOSM ped2) {
public int compare(PedestrianSelfCatThreat ped1, PedestrianSelfCatThreat ped2) {
int timeCompare = Double.compare(ped1.getTimeOfNextStep(), ped2.getTimeOfNextStep());
if(timeCompare != 0) {
return timeCompare;
......
......@@ -26,6 +26,8 @@ public class Pedestrian extends Agent {
private boolean isChild; // TODO should actually be an attribute or a member of a subclass
private boolean isLikelyInjured; // TODO should actually be an attribute or a member of a subclass
// TODO: Save also "Stimulus perceivedThreat" and "SelfCategory ingroup".
// Maybe here, or in "PsychologyStatus".
private PsychologyStatus psychologyStatus;
private LinkedList<Integer> groupIds; // TODO should actually be an attribute or a member of a subclass
......
Supports Markdown
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