Skip to content

GitLab

V
vadere
Commit 3e99bea3 authored by Benedikt Kleinmeier's avatar Benedikt Kleinmeier
Browse files
Options

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
          Hide whitespace changes
          Inline Side-by-side
          Showing with 685 additions and 579 deletions
          VadereSimulator/src/org/vadere/simulator/control/psychology/cognition/helpers/CognitionModelBuilder.java
          View file @ 3e99bea3
          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;
          ......
          VadereSimulator/src/org/vadere/simulator/control/psychology/cognition/models/CounterflowCognitionModel.java
          View file @ 3e99bea3
          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;
          }
          }
          VadereSimulator/src/org/vadere/simulator/control/psychology/cognition/models/ThreatCognitionModel.java
          View file @ 3e99bea3
          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;
          }
          }
          VadereSimulator/src/org/vadere/simulator/control/psychology/perception/helpers/PerceptionModelBuilder.java
          View file @ 3e99bea3
          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;
          ......
          VadereSimulator/src/org/vadere/simulator/control/simulation/ScenarioRun.java
          View file @ 3e99bea3
          ......@@ -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;
          ......
          VadereSimulator/src/org/vadere/simulator/models/osm/OSMBehaviorController.java
          View file @ 3e99bea3
          ......@@ -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.
          *
          ......
          VadereSimulator/src/org/vadere/simulator/utils/topography/TopographyHelper.java 0 → 100644
          View file @ 3e99bea3
          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());