Commit 3e99bea3 authored by Benedikt Kleinmeier's avatar Benedikt Kleinmeier

Created "simulator/utils/topography/TopographyHelper.java" to encapsulate...

Created "simulator/utils/topography/TopographyHelper.java" to encapsulate common methods for cognition and locomotion layer which rely on the topography
parent 2949815e
Pipeline #274997 passed with stages
in 137 minutes and 48 seconds
package org.vadere.simulator.control.psychology.cognition.helper;
package org.vadere.simulator.control.psychology.cognition.helpers;
import org.vadere.simulator.control.psychology.cognition.models.ICognitionModel;
import org.vadere.simulator.projects.ScenarioStore;
......
package org.vadere.simulator.control.psychology.cognition.models;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.simulator.utils.topography.TopographyHelper;
import org.vadere.state.psychology.cognition.SelfCategory;
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;
import java.util.stream.Collectors;
/**
* The {@link CounterflowCognitionModel} enables a pedestrian to evade if
......@@ -34,10 +30,10 @@ public class CounterflowCognitionModel implements ICognitionModel {
pedestrian.setSelfCategory(SelfCategory.TARGET_ORIENTED);
if (pedestrian.hasNextTarget()) {
Pedestrian closestPedestrian = getClosestPedestrianWhichIsCloserToTarget(pedestrian, topography);
Pedestrian closestPedestrian = TopographyHelper.getNeighborCloserToTargetCentroid(pedestrian, topography);
if (closestPedestrian != null) {
if (walkingDirectionDiffers(pedestrian, closestPedestrian)) {
if (TopographyHelper.walkingDirectionDiffers(pedestrian, closestPedestrian, topography)) {
pedestrian.setSelfCategory(SelfCategory.EVADE);
} else if (closestPedestrian.getSelfCategory() == SelfCategory.EVADE) {
pedestrian.setSelfCategory(SelfCategory.EVADE); // Imitate behavior
......@@ -47,124 +43,4 @@ public class CounterflowCognitionModel implements ICognitionModel {
}
}
private Pedestrian getClosestPedestrianWhichIsCloserToTarget(Pedestrian pedestrian, Topography topography) {
VPoint positionOfPedestrian = pedestrian.getPosition();
List<Integer> targetList = pedestrian.getTargets();
Pedestrian closestPedestrian = null;
if (targetList.size() > 0) {
Target target = topography.getTarget(targetList.get(0));
VPoint targetCentroid = target.getShape().getCentroid();
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(closestPed -> pedestrian.getId() != closestPed.getId())
.filter(closestPed -> closestPed.getPosition().distance(targetCentroid) < pedestrian.getPosition().distance(targetCentroid))
.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());
if (closestPedestrians.size() > 0) {
closestPedestrian = closestPedestrians.get(0);
}
}
return closestPedestrian;
}
private boolean walkingDirectionDiffers(Pedestrian pedestrian1, Pedestrian pedestrian2) {
boolean directionIsDifferent = false;
double angleInRadian = calculateAngleBetweenWalkingDirections(pedestrian1, pedestrian2, topography);
if (angleInRadian == -1 || Math.toDegrees(angleInRadian) > pedestrian1.getAttributes().getWalkingDirectionSameIfAngleLessOrEqual()) {
directionIsDifferent = true;
}
return directionIsDifferent;
}
private double calculateAngleBetweenWalkingDirections(Pedestrian pedestrian1, Pedestrian pedestrian2, Topography topography) {
double angleInRadian = -1;
switch (pedestrian1.getAttributes().getWalkingDirectionCalculation()) {
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;
}
/**
* 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.
*/
private 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;
}
}
package org.vadere.simulator.control.psychology.cognition.models;
import org.vadere.simulator.utils.topography.TopographyHelper;
import org.vadere.state.psychology.cognition.GroupMembership;
import org.vadere.state.psychology.cognition.SelfCategory;
import org.vadere.state.psychology.perception.types.ElapsedTime;
......@@ -7,7 +8,6 @@ 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.Topography;
import org.vadere.state.simulation.FootstepHistory;
import org.vadere.util.geometry.shapes.VPoint;
import java.util.Collection;
......@@ -59,7 +59,7 @@ public class ThreatCognitionModel implements ICognitionModel {
// Gerta suggests to apply SelfCategory.OUTSIDE_THREAT_AREA
// so that agents directly search a safe zone if they are blocked by a wall.
if (pedestrianIsBlockedByObstacle(pedestrian, topography)) {
if (TopographyHelper.pedestrianIsBlockedByObstacle(pedestrian, topography)) {
pedestrian.setSelfCategory(SelfCategory.OUTSIDE_THREAT_AREA);
}
}
......@@ -100,7 +100,7 @@ public class ThreatCognitionModel implements ICognitionModel {
double distanceToThreat = threatOrigin.distance(pedestrian.getPosition());
boolean pedestrianIsInsideThreatArea = (distanceToThreat <= latestThreat.getRadius());
boolean pedestrianIsBlockedByObstacle = pedestrianIsBlockedByObstacle(pedestrian, topography);
boolean pedestrianIsBlockedByObstacle = TopographyHelper.pedestrianIsBlockedByObstacle(pedestrian, topography);
// Gerta suggests to apply SelfCategory.OUTSIDE_THREAT_AREA
// so that agents directly search a safe zone if they are blocked by a wall.
......@@ -111,28 +111,6 @@ public class ThreatCognitionModel implements ICognitionModel {
}
}
// TODO Write unit tests!
private boolean pedestrianIsBlockedByObstacle(Pedestrian pedestrian, Topography topography) {
boolean isBlocked = false;
int requiredFootSteps = 2;
double requiredSpeedToBeBlocked = 0.05;
double requiredDistanceToObstacle = 1.0;
FootstepHistory footstepHistory = pedestrian.getFootstepHistory();
if (footstepHistory.size() >= requiredFootSteps) {
if (footstepHistory.getAverageSpeedInMeterPerSecond() <= requiredSpeedToBeBlocked) {
// Watch out: This is probably a very expensive call but Gerta suggests to include it to get a realistic behavior!
if (topography.distanceToObstacle(pedestrian.getPosition()) <= requiredDistanceToObstacle) {
isBlocked = true;
}
}
}
return isBlocked;
}
/**
* If a threatened ingroup pedestrian is nearby, use the same reaction as if
......@@ -147,7 +125,7 @@ public class ThreatCognitionModel implements ICognitionModel {
* This behavior is triggered by method {@link #handleThreat(Pedestrian, Stimulus)}.
*/
private void imitateThreatenedPedestrianIfPresent(Pedestrian pedestrian) {
List<Pedestrian> threatenedPedestrians = getClosestPedestriansWithSelfCategory(pedestrian, SelfCategory.OUTSIDE_THREAT_AREA);
List<Pedestrian> threatenedPedestrians = TopographyHelper.getNeighborsWithSelfCategory(pedestrian, SelfCategory.OUTSIDE_THREAT_AREA, topography);
List<Pedestrian> threatenedIngroupPeds = threatenedPedestrians.stream()
.filter(ped -> ped.getGroupMembership() == GroupMembership.IN_GROUP)
.collect(Collectors.toList());
......@@ -164,27 +142,4 @@ public class ThreatCognitionModel implements ICognitionModel {
}
}
private List<Pedestrian> getClosestPedestriansWithSelfCategory(Pedestrian pedestrian, SelfCategory expectedSelfCategory) {
VPoint positionOfPedestrian = pedestrian.getPosition();
List<Pedestrian> closestPedestrians = topography.getSpatialMap(Pedestrian.class)
.getObjects(positionOfPedestrian, pedestrian.getAttributes().getSearchRadius());
// Filter out "me" and pedestrians with unexpected "selfCategory".
closestPedestrians = closestPedestrians.stream()
.filter(candidate -> pedestrian.getId() != candidate.getId())
.filter(candidate -> candidate.getSelfCategory() == expectedSelfCategory)
.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;
}
}
package org.vadere.simulator.control.psychology.perception.helper;
package org.vadere.simulator.control.psychology.perception.helpers;
import org.vadere.simulator.control.psychology.perception.models.IPerceptionModel;
import org.vadere.simulator.projects.ScenarioStore;
......
......@@ -11,10 +11,10 @@ import org.vadere.meshing.mesh.gen.AVertex;
import org.vadere.meshing.utils.io.poly.MeshPolyReader;
import org.vadere.meshing.utils.io.poly.MeshPolyWriter;
import org.vadere.simulator.context.VadereContext;
import org.vadere.simulator.control.psychology.cognition.helper.CognitionModelBuilder;
import org.vadere.simulator.control.psychology.cognition.helpers.CognitionModelBuilder;
import org.vadere.simulator.control.psychology.cognition.models.ICognitionModel;
import org.vadere.simulator.control.psychology.perception.models.IPerceptionModel;
import org.vadere.simulator.control.psychology.perception.helper.PerceptionModelBuilder;
import org.vadere.simulator.control.psychology.perception.helpers.PerceptionModelBuilder;
import org.vadere.simulator.control.psychology.perception.StimulusController;
import org.vadere.simulator.control.scenarioelements.TargetChangerController;
import org.vadere.simulator.models.MainModel;
......
......@@ -8,7 +8,7 @@ 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.TargetRepulsionStrategy;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.simulator.utils.topography.TopographyHelper;
import org.vadere.state.psychology.cognition.SelfCategory;
import org.vadere.state.psychology.perception.types.ChangeTarget;
import org.vadere.state.psychology.perception.types.Stimulus;
......@@ -24,8 +24,6 @@ import org.vadere.util.logging.Logger;
import java.awt.geom.Rectangle2D;
import java.util.LinkedList;
import java.util.List;
import java.util.Vector;
import java.util.stream.Collectors;
/**
* A class to encapsulate the behavior of a single {@link PedestrianOSM}.
......@@ -298,7 +296,7 @@ public class OSMBehaviorController {
if (pedestrian.getCombinedPotentialStrategy() instanceof TargetRepulsionStrategy) {
ScenarioElement searchPosition = (pedestrian.getSource() == null) ? pedestrian : pedestrian.getSource();
Target closestTarget = findClosestTarget(topography, searchPosition, pedestrian.getThreatMemory().getLatestThreat());
Target closestTarget = TopographyHelper.findClosestTargetToSource(topography, searchPosition, pedestrian.getThreatMemory().getLatestThreat());
assert closestTarget != null;
......@@ -310,21 +308,6 @@ public class OSMBehaviorController {
}
}
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();
......@@ -349,19 +332,13 @@ public class OSMBehaviorController {
return null;
}
List<Pedestrian> closestPedestrians = getClosestPedestriansWhichAreCloserToTarget(pedestrian, topography);
List<Pedestrian> closestPedestrians = TopographyHelper.getNeighborsCloserToTarget(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;
}
boolean walkingDirectionDiffers = TopographyHelper.walkingDirectionDiffers(pedestrian, closestPedestrian, topography);
if (closestPedIsCooperative && walkingDirectionDiffers) {
return (PedestrianOSM)closestPedestrian;
......@@ -375,122 +352,6 @@ public class OSMBehaviorController {
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 angle3D 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 : angle3D 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 angle3D 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 angle3D calculation type: \"%s\"", pedestrian.getAttributes().getWalkingDirectionCalculation()));
}
return vectorPedestrianToTarget;
}
/**
* Swap two pedestrians.
*
......
package org.vadere.simulator.utils.topography;
import org.jetbrains.annotations.NotNull;
import org.vadere.simulator.models.osm.PedestrianOSM;
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.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.FootstepHistory;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.Vector2D;
import java.util.List;
import java.util.stream.Collectors;
/**
* A summary static methods which help
* <ul>
* <li>to define the cognitive state of an agent or</li>
* <li>to maneuver and agent through the {@link org.vadere.state.scenario.Topography}</li>
* </ul>
*
* TODO Complete unit tests in "TopographyHelperTest" for all these methods.
*/
public class TopographyHelper {
public static Target findClosestTargetToSource(Topography topography, ScenarioElement source, Threat threat) {
VPoint sourceCentroid = source.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 static List<Pedestrian> getNeighborsWithSelfCategory(Pedestrian pedestrian, SelfCategory expectedSelfCategory, Topography topography) {
VPoint positionOfPedestrian = pedestrian.getPosition();
List<Pedestrian> closestPedestrians = topography.getSpatialMap(Pedestrian.class)
.getObjects(positionOfPedestrian, pedestrian.getAttributes().getSearchRadius());
// Filter out "me" and pedestrians with unexpected "selfCategory".
closestPedestrians = closestPedestrians.stream()
.filter(candidate -> pedestrian.getId() != candidate.getId())
.filter(candidate -> candidate.getSelfCategory() == expectedSelfCategory)
.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;
}
public static Pedestrian getNeighborCloserToTargetCentroid(Pedestrian pedestrian, Topography topography) {
VPoint positionOfPedestrian = pedestrian.getPosition();
List<Integer> targetList = pedestrian.getTargets();
Pedestrian closestPedestrian = null;
if (targetList.size() > 0) {
Target target = topography.getTarget(targetList.get(0));
VPoint targetCentroid = target.getShape().getCentroid();
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(closestPed -> pedestrian.getId() != closestPed.getId())
.filter(closestPed -> closestPed.getPosition().distance(targetCentroid) < pedestrian.getPosition().distance(targetCentroid))
.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());
if (closestPedestrians.size() > 0) {
closestPedestrian = closestPedestrians.get(0);
}
}
return closestPedestrian;
}
@NotNull
public static List<Pedestrian> getNeighborsCloserToTarget(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;
}
public static boolean pedestrianIsBlockedByObstacle(Pedestrian pedestrian, Topography topography) {
boolean isBlocked = false;
int requiredFootSteps = 2;
double requiredSpeedToBeBlocked = 0.05;
double requiredDistanceToObstacle = 1.0;
FootstepHistory footstepHistory = pedestrian.getFootstepHistory();
if (footstepHistory.size() >= requiredFootSteps) {
if (footstepHistory.getAverageSpeedInMeterPerSecond() <= requiredSpeedToBeBlocked) {
// Watch out: This is probably a very expensive call but Gerta suggests to include it to get a realistic behavior!
if (topography.distanceToObstacle(pedestrian.getPosition()) <= requiredDistanceToObstacle) {
isBlocked = true;
}
}
}
return isBlocked;
}
public static boolean walkingDirectionDiffers(Pedestrian pedestrian1, Pedestrian pedestrian2, Topography topography) {
boolean directionIsDifferent = false;
double angleInRadian = calculateAngleBetweenWalkingDirections(pedestrian1, pedestrian2, topography);
if (angleInRadian == -1 || Math.toDegrees(angleInRadian) > pedestrian1.getAttributes().getWalkingDirectionSameIfAngleLessOrEqual()) {
directionIsDifferent = true;
}
return directionIsDifferent;
}
public static double calculateAngleBetweenWalkingDirections(Pedestrian pedestrian1, Pedestrian pedestrian2, Topography topography) {
double angleInRadian = -1;
switch (pedestrian1.getAttributes().getWalkingDirectionCalculation()) {
case BY_GRADIENT:
angleInRadian = calculateAngleBetweenTargetGradients((PedestrianOSM)pedestrian1, (PedestrianOSM)pedestrian2);
break;
case BY_TARGET_CENTER:
case BY_TARGET_CLOSEST_POINT:
angleInRadian = calculateAngleBetweenTargets(pedestrian1, pedestrian2, topography);
break;