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

Use output of cognition layer instead of perception layer in locomotion layer (not tested so far)

Moved "selfcategorization/locomotion/OSMBehaviorController.java" to "osm/OSMBehaviorController.java"
parent 2fb363ec
package org.vadere.simulator.models.bhm;
import java.util.Collection;
import java.util.LinkedList;
import java.util.List;
import java.util.Optional;
import java.util.Random;
import org.jetbrains.annotations.NotNull;
import org.jetbrains.annotations.Nullable;
import org.vadere.simulator.models.potential.fields.IPotentialFieldTarget;
import org.vadere.state.attributes.models.AttributesBHM;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.psychology.perception.exceptions.UnsupportedStimulusException;
import org.vadere.state.psychology.perception.types.ElapsedTime;
import org.vadere.state.psychology.perception.types.Stimulus;
import org.vadere.state.psychology.perception.types.Wait;
import org.vadere.state.psychology.perception.types.WaitInArea;
import org.vadere.state.psychology.cognition.SelfCategory;
import org.vadere.state.scenario.Obstacle;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Target;
......@@ -27,6 +17,8 @@ import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.Vector2D;
import org.vadere.util.logging.Logger;
import java.util.*;
public class PedestrianBHM extends Pedestrian {
private static Logger logger = Logger.getLogger(PedestrianBHM.class);
......@@ -182,16 +174,17 @@ public class PedestrianBHM extends Pedestrian {
double endTimeStep = timeOfNextStep + durationNextStep;
timeOfNextStep = endTimeStep;
Stimulus mostImportantStimulus = getMostImportantStimulus();
SelfCategory selfCategory = getSelfCategory();
VPoint position = getPosition();
if (mostImportantStimulus instanceof ElapsedTime) {
if (selfCategory == SelfCategory.TARGET_ORIENTED) {
updateTargetDirection();
nextPosition = navigation.getNavigationPosition();
makeStep();
} else if (mostImportantStimulus instanceof Wait || mostImportantStimulus instanceof WaitInArea) {
} else if (selfCategory == SelfCategory.WAIT) {
// do nothing
} else {
throw new UnsupportedStimulusException(mostImportantStimulus, this.getClass());
throw new IllegalArgumentException("Unsupported SelfCategory: " + selfCategory);
}
FootStep currentFootstep = new FootStep(position, getPosition(), startTimeStep, endTimeStep);
......
......@@ -4,12 +4,15 @@ 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.simulator.models.potential.combinedPotentials.TargetAttractionStrategy;
import org.vadere.simulator.models.potential.combinedPotentials.TargetRepulsionStrategy;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.psychology.cognition.SelfCategory;
import org.vadere.state.psychology.perception.types.Threat;
import org.vadere.state.psychology.perception.types.ChangeTarget;
import org.vadere.state.psychology.perception.types.Stimulus;
import org.vadere.state.psychology.perception.types.Threat;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.ScenarioElement;
import org.vadere.state.scenario.Target;
import org.vadere.state.scenario.Topography;
import org.vadere.state.simulation.FootStep;
......@@ -41,6 +44,13 @@ 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
......@@ -110,36 +120,76 @@ public class OSMBehaviorController {
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + timeStepInSec);
}
// Watch out: A threat stimulus 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 threat stimulus and then use "ElapsedTime"
// afterwards to let the agent walk.
public void reactToBang(PedestrianOSM pedestrian, Topography topography) {
Stimulus mostImportantStimulus = pedestrian.getMostImportantStimulus();
public void maximizeDistanceToThreatAndIncreaseSpeed(PedestrianOSM pedestrian, Topography topography) {
Stimulus perceivedThreat = pedestrian.getPerceivedThreat();
if (mostImportantStimulus instanceof Threat) {
Threat threat = (Threat) pedestrian.getMostImportantStimulus();
Target bangOrigin = topography.getTarget(threat.getOriginAsTargetId());
if (perceivedThreat instanceof Threat && pedestrian.getCombinedPotentialStrategy() instanceof TargetAttractionStrategy) {
Threat threat = (Threat) perceivedThreat;
Target threatOrigin = topography.getTarget(threat.getOriginAsTargetId());
LinkedList<Integer> nextTarget = new LinkedList<>();
nextTarget.add(bangOrigin.getId());
nextTarget.add(threatOrigin.getId());
pedestrian.setTargets(nextTarget);
pedestrian.setCombinedPotentialStrategy(CombinedPotentialStrategy.TARGET_REPULSION_STRATEGY);
// TODO: Maybe, sample speed-up from a distribution or define it as a configurable attribute.
double escapeSpeed = pedestrian.getFreeFlowSpeed() * 2.0;
pedestrian.setFreeFlowSpeed(escapeSpeed);
} else {
logger.debug(String.format("Expected: %s, Received: %s",
Threat.class.getSimpleName(),
mostImportantStimulus.getClass().getSimpleName()));
perceivedThreat.getClass().getSimpleName()));
}
}
/**
* In dangerous situation humans tend to escape to familiar places (safe zones).
* A pedestrian selects the target which is closest to its source as safe zone.
* Or if pedestrian has no target, select closest target as safe zone.
*
* TODO: Clarify with Gerta if this is really a plausible assumption for safe zones.
* An easier approach is to just use the closest target as safe zone.
*/
public void changeTargetToSafeZone(PedestrianOSM pedestrian, Topography topography) {
if (pedestrian.getCombinedPotentialStrategy() instanceof TargetRepulsionStrategy) {
ScenarioElement searchPosition = (pedestrian.getSource() == null) ? pedestrian : pedestrian.getSource();
Target closestTarget = findClosestTarget(topography, searchPosition, (Threat) pedestrian.getPerceivedThreat());
assert closestTarget != null;
if (closestTarget != null) {
pedestrian.setSingleTarget(closestTarget.getId(), false);
}
pedestrian.setCombinedPotentialStrategy(CombinedPotentialStrategy.TARGET_ATTRACTION_STRATEGY);
}
}
public void reactToTargetChange(PedestrianOSM pedestrian, Topography topography) {
private Target findClosestTarget(Topography topography, ScenarioElement scenarioElement, Threat threat) {
VPoint sourceCentroid = scenarioElement.getShape().getCentroid();
List<Target> sortedTargets = topography.getTargets().stream()
.filter(target -> target.getId() != threat.getOriginAsTargetId())
.sorted((target1, target2) -> Double.compare(
sourceCentroid.distance(target1.getShape().getCentroid()),
sourceCentroid.distance(target2.getShape().getCentroid())))
.collect(Collectors.toList());
Target closestTarget = (sortedTargets.isEmpty()) ? null : sortedTargets.get(0);
return closestTarget;
}
public void changeTarget(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(),
......
......@@ -4,7 +4,6 @@ import org.jetbrains.annotations.NotNull;
import org.vadere.simulator.models.osm.OSMBehaviorController;
import org.vadere.simulator.models.osm.PedestrianOSM;
import org.vadere.state.psychology.cognition.SelfCategory;
import org.vadere.state.psychology.perception.types.*;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Topography;
......@@ -48,43 +47,31 @@ public class UpdateSchemeEventDriven implements UpdateSchemeOSM {
return;
}
Stimulus mostImportantStimulus = pedestrian.getMostImportantStimulus();
SelfCategory selfCategory = pedestrian.getSelfCategory();
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);
} 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());
}*/
} else {
pedestrian.updateNextPosition();
osmBehaviorController.makeStep(pedestrian, topography, pedestrian.getDurationNextStep());
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep());
}
// TODO: Maybe, use a state table with function pointers to a template function myFunc(ped, topography, time)
if (selfCategory == SelfCategory.TARGET_ORIENTED) {
osmBehaviorController.makeStepToTarget(pedestrian, topography);
} else if (selfCategory == SelfCategory.COOPERATIVE) {
PedestrianOSM candidate = osmBehaviorController.findSwapCandidate(pedestrian, topography);
if (candidate != null) {
pedestrianEventsQueue.remove(candidate);
osmBehaviorController.swapPedestrians(pedestrian, candidate, topography);
pedestrianEventsQueue.add(candidate);
} else {
osmBehaviorController.makeStepToTarget(pedestrian, topography);
}
} else if (mostImportantStimulus instanceof Wait || mostImportantStimulus instanceof WaitInArea) {
} else if (selfCategory == SelfCategory.INSIDE_THREAT_AREA) {
osmBehaviorController.maximizeDistanceToThreatAndIncreaseSpeed(pedestrian, topography);
osmBehaviorController.makeStepToTarget(pedestrian, topography);
} else if (selfCategory == SelfCategory.OUTSIDE_THREAT_AREA) {
osmBehaviorController.changeTargetToSafeZone(pedestrian, topography);
osmBehaviorController.makeStepToTarget(pedestrian, topography);
} else if (selfCategory == SelfCategory.WAIT) {
osmBehaviorController.wait(pedestrian, timeStepInSec);
} else if (mostImportantStimulus instanceof Threat) {
osmBehaviorController.reactToBang(pedestrian, topography);
// Set time of next step. Otherwise, the internal OSM event queue hangs endlessly.
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep());
} else if (mostImportantStimulus instanceof ChangeTarget) {
osmBehaviorController.reactToTargetChange(pedestrian, topography);
} else if (selfCategory == SelfCategory.CHANGE_TARGET) {
osmBehaviorController.changeTarget(pedestrian, topography);
// Set time of next step. Otherwise, the internal OSM event queue hangs endlessly.
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep());
}
......
......@@ -31,6 +31,8 @@ public class UpdateSchemeSequential implements UpdateSchemeOSM {
}
protected void update(@NotNull final Collection<Pedestrian> pedestrianOSMS, final double currentTimeInSec, final double timeStepInSec) {
// TODO: Clarify with Bene if we can call "clearStrides()" here directly like in "UpdateSchemeEventDriven"
// and omit in in invoked "update()".
for (Pedestrian pedestrian : pedestrianOSMS) {
if(!skipUdate.contains(pedestrian)) {
update((PedestrianOSM) pedestrian, currentTimeInSec, timeStepInSec);
......@@ -48,26 +50,34 @@ public class UpdateSchemeSequential implements UpdateSchemeOSM {
pedestrian.setTimeOfNextStep(currentTimeInSec);
}
if (mostImportantStimulus instanceof ElapsedTime) {
SelfCategory selfCategory = pedestrian.getSelfCategory();
if (selfCategory == SelfCategory.TARGET_ORIENTED) {
pedestrian.clearStrides();
stepForward(pedestrian, currentTimeInSec, timeStepInSec);
} else if (selfCategory == SelfCategory.COOPERATIVE) {
pedestrian.clearStrides();
if (pedestrian.getSelfCategory() == SelfCategory.TARGET_ORIENTED) {
PedestrianOSM candidate = osmBehaviorController.findSwapCandidate(pedestrian, topography);
if(candidate != null) {
osmBehaviorController.swapPedestrians(pedestrian, candidate, topography);
// here we update not only pedestrian but also candidate, therefore candidate is already treated and will be skipped.
skipUdate.add(candidate);
} else {
stepForward(pedestrian, currentTimeInSec, timeStepInSec);
} else if (pedestrian.getSelfCategory() == SelfCategory.COOPERATIVE) {
PedestrianOSM candidate = osmBehaviorController.findSwapCandidate(pedestrian, topography);
if(candidate != null) {
osmBehaviorController.swapPedestrians(pedestrian, candidate, topography);
// here we update not only pedestrian but also candidate, therefore candidate is already treated and will be skipped.
skipUdate.add(candidate);
} else {
stepForward(pedestrian, currentTimeInSec, timeStepInSec);
}
}
} else if (mostImportantStimulus instanceof Wait || mostImportantStimulus instanceof WaitInArea) {
} else if (selfCategory == SelfCategory.INSIDE_THREAT_AREA) {
osmBehaviorController.maximizeDistanceToThreatAndIncreaseSpeed(pedestrian, topography);
osmBehaviorController.makeStepToTarget(pedestrian, topography);
} else if (selfCategory == SelfCategory.OUTSIDE_THREAT_AREA) {
osmBehaviorController.changeTargetToSafeZone(pedestrian, topography);
osmBehaviorController.makeStepToTarget(pedestrian, topography);
} else if (selfCategory == SelfCategory.WAIT) {
osmBehaviorController.wait(pedestrian, timeStepInSec);
} else if (mostImportantStimulus instanceof Threat) {
osmBehaviorController.reactToBang(pedestrian, topography);
} else if (mostImportantStimulus instanceof ChangeTarget) {
osmBehaviorController.reactToTargetChange(pedestrian, topography);
} else if (selfCategory == SelfCategory.CHANGE_TARGET) {
osmBehaviorController.changeTarget(pedestrian, topography);
// Set time of next step. Otherwise, the internal OSM event queue hangs endlessly.
pedestrian.setTimeOfNextStep(pedestrian.getTimeOfNextStep() + pedestrian.getDurationNextStep());
}
}
......
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.simulator.models.potential.combinedPotentials.TargetAttractionStrategy;
import org.vadere.simulator.models.potential.combinedPotentials.TargetRepulsionStrategy;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.psychology.cognition.SelfCategory;
import org.vadere.state.psychology.perception.types.Threat;
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.ScenarioElement;
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);
}
public void maximizeDistanceToThreatAndIncreaseSpeed(PedestrianOSM pedestrian, Topography topography) {
Stimulus perceivedThreat = pedestrian.getPerceivedThreat();
if (perceivedThreat instanceof Threat && pedestrian.getCombinedPotentialStrategy() instanceof TargetAttractionStrategy) {
Threat threat = (Threat) perceivedThreat;
Target threatOrigin = topography.getTarget(threat.getOriginAsTargetId());
LinkedList<Integer> nextTarget = new LinkedList<>();
nextTarget.add(threatOrigin.getId());
pedestrian.setTargets(nextTarget);
pedestrian.setCombinedPotentialStrategy(CombinedPotentialStrategy.TARGET_REPULSION_STRATEGY);
// TODO: Maybe, sample speed-up from a distribution or define it as a configurable attribute.
double escapeSpeed = pedestrian.getFreeFlowSpeed() * 2.0;
pedestrian.setFreeFlowSpeed(escapeSpeed);
} else {
logger.debug(String.format("Expected: %s, Received: %s",
Threat.class.getSimpleName(),
perceivedThreat.getClass().getSimpleName()));
}
}
/**
* In dangerous situation humans tend to escape to familiar places (safe zones).
* A pedestrian selects the target which is closest to its source as safe zone.
* Or if pedestrian has no target, select closest target as safe zone.
*
* TODO: Clarify with Gerta if this is really a plausible assumption for safe zones.
* An easier approach is to just use the closest target as safe zone.
*/
public void changeTargetToSafeZone(PedestrianOSM pedestrian, Topography topography) {
if (pedestrian.getCombinedPotentialStrategy() instanceof TargetRepulsionStrategy) {
ScenarioElement searchPosition = (pedestrian.getSource() == null) ? pedestrian : pedestrian.getSource();
Target closestTarget = findClosestTarget(topography, searchPosition, (Threat) pedestrian.getPerceivedThreat());
assert closestTarget != null;
if (closestTarget != null) {
pedestrian.setSingleTarget(closestTarget.getId(), false);
}
pedestrian.setCombinedPotentialStrategy(CombinedPotentialStrategy.TARGET_ATTRACTION_STRATEGY);
}
}
private Target findClosestTarget(Topography topography, ScenarioElement scenarioElement, Threat threat) {
VPoint sourceCentroid = scenarioElement.getShape().getCentroid();
List<Target> sortedTargets = topography.getTargets().stream()
.filter(target -> target.getId() != threat.getOriginAsTargetId())
.sorted((target1, target2) -> Double.compare(
sourceCentroid.distance(target1.getShape().getCentroid()),
sourceCentroid.distance(target2.getShape().getCentroid())))
.collect(Collectors.toList());
Target closestTarget = (sortedTargets.isEmpty()) ? null : sortedTargets.get(0);
return closestTarget;
}
public void changeTarget(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".