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.control.psychology.cognition.models.ICognitionModel;
import org.vadere.simulator.projects.ScenarioStore; import org.vadere.simulator.projects.ScenarioStore;
......
package org.vadere.simulator.control.psychology.cognition.models; 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.psychology.cognition.SelfCategory;
import org.vadere.state.scenario.Pedestrian; import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Target;
import org.vadere.state.scenario.Topography; import org.vadere.state.scenario.Topography;
import org.vadere.util.geometry.shapes.VPoint;
import java.util.Collection; import java.util.Collection;
import java.util.List;
import java.util.stream.Collectors;
/** /**
* The {@link CounterflowCognitionModel} enables a pedestrian to evade if * The {@link CounterflowCognitionModel} enables a pedestrian to evade if
...@@ -34,10 +30,10 @@ public class CounterflowCognitionModel implements ICognitionModel { ...@@ -34,10 +30,10 @@ public class CounterflowCognitionModel implements ICognitionModel {
pedestrian.setSelfCategory(SelfCategory.TARGET_ORIENTED); pedestrian.setSelfCategory(SelfCategory.TARGET_ORIENTED);
if (pedestrian.hasNextTarget()) { if (pedestrian.hasNextTarget()) {
Pedestrian closestPedestrian = getClosestPedestrianWhichIsCloserToTarget(pedestrian, topography); Pedestrian closestPedestrian = TopographyHelper.getNeighborCloserToTargetCentroid(pedestrian, topography);
if (closestPedestrian != null) { if (closestPedestrian != null) {
if (walkingDirectionDiffers(pedestrian, closestPedestrian)) { if (TopographyHelper.walkingDirectionDiffers(pedestrian, closestPedestrian, topography)) {
pedestrian.setSelfCategory(SelfCategory.EVADE); pedestrian.setSelfCategory(SelfCategory.EVADE);
} else if (closestPedestrian.getSelfCategory() == SelfCategory.EVADE) { } else if (closestPedestrian.getSelfCategory() == SelfCategory.EVADE) {
pedestrian.setSelfCategory(SelfCategory.EVADE); // Imitate behavior pedestrian.setSelfCategory(SelfCategory.EVADE); // Imitate behavior
...@@ -47,124 +43,4 @@ public class CounterflowCognitionModel implements ICognitionModel { ...@@ -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; 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.GroupMembership;
import org.vadere.state.psychology.cognition.SelfCategory; import org.vadere.state.psychology.cognition.SelfCategory;
import org.vadere.state.psychology.perception.types.ElapsedTime; import org.vadere.state.psychology.perception.types.ElapsedTime;
...@@ -7,7 +8,6 @@ import org.vadere.state.psychology.perception.types.Stimulus; ...@@ -7,7 +8,6 @@ import org.vadere.state.psychology.perception.types.Stimulus;
import org.vadere.state.psychology.perception.types.Threat; import org.vadere.state.psychology.perception.types.Threat;
import org.vadere.state.scenario.Pedestrian; import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Topography; 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.VPoint;
import java.util.Collection; import java.util.Collection;
...@@ -59,7 +59,7 @@ public class ThreatCognitionModel implements ICognitionModel { ...@@ -59,7 +59,7 @@ public class ThreatCognitionModel implements ICognitionModel {
// Gerta suggests to apply SelfCategory.OUTSIDE_THREAT_AREA // Gerta suggests to apply SelfCategory.OUTSIDE_THREAT_AREA
// so that agents directly search a safe zone if they are blocked by a wall. // 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); pedestrian.setSelfCategory(SelfCategory.OUTSIDE_THREAT_AREA);
} }
} }
...@@ -100,7 +100,7 @@ public class ThreatCognitionModel implements ICognitionModel { ...@@ -100,7 +100,7 @@ public class ThreatCognitionModel implements ICognitionModel {
double distanceToThreat = threatOrigin.distance(pedestrian.getPosition()); double distanceToThreat = threatOrigin.distance(pedestrian.getPosition());
boolean pedestrianIsInsideThreatArea = (distanceToThreat <= latestThreat.getRadius()); boolean pedestrianIsInsideThreatArea = (distanceToThreat <= latestThreat.getRadius());
boolean pedestrianIsBlockedByObstacle = pedestrianIsBlockedByObstacle(pedestrian, topography); boolean pedestrianIsBlockedByObstacle = TopographyHelper.pedestrianIsBlockedByObstacle(pedestrian, topography);
// Gerta suggests to apply SelfCategory.OUTSIDE_THREAT_AREA // Gerta suggests to apply SelfCategory.OUTSIDE_THREAT_AREA
// so that agents directly search a safe zone if they are blocked by a wall. // so that agents directly search a safe zone if they are blocked by a wall.
...@@ -111,28 +111,6 @@ public class ThreatCognitionModel implements ICognitionModel { ...@@ -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 * If a threatened ingroup pedestrian is nearby, use the same reaction as if
...@@ -147,7 +125,7 @@ public class ThreatCognitionModel implements ICognitionModel { ...@@ -147,7 +125,7 @@ public class ThreatCognitionModel implements ICognitionModel {
* This behavior is triggered by method {@link #handleThreat(Pedestrian, Stimulus)}. * This behavior is triggered by method {@link #handleThreat(Pedestrian, Stimulus)}.
*/ */
private void imitateThreatenedPedestrianIfPresent(Pedestrian pedestrian) { 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() List<Pedestrian> threatenedIngroupPeds = threatenedPedestrians.stream()
.filter(ped -> ped.getGroupMembership() == GroupMembership.IN_GROUP) .filter(ped -> ped.getGroupMembership() == GroupMembership.IN_GROUP)
.collect(Collectors.toList()); .collect(Collectors.toList());
...@@ -164,27 +142,4 @@ public class ThreatCognitionModel implements ICognitionModel { ...@@ -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.control.psychology.perception.models.IPerceptionModel;
import org.vadere.simulator.projects.ScenarioStore; import org.vadere.simulator.projects.ScenarioStore;
......
...@@ -11,10 +11,10 @@ import org.vadere.meshing.mesh.gen.AVertex; ...@@ -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.MeshPolyReader;
import org.vadere.meshing.utils.io.poly.MeshPolyWriter; import org.vadere.meshing.utils.io.poly.MeshPolyWriter;
import org.vadere.simulator.context.VadereContext; 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.cognition.models.ICognitionModel;
import org.vadere.simulator.control.psychology.perception.models.IPerceptionModel; 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.psychology.perception.StimulusController;
import org.vadere.simulator.control.scenarioelements.TargetChangerController; import org.vadere.simulator.control.scenarioelements.TargetChangerController;
import org.vadere.simulator.models.MainModel; import org.vadere.simulator.models.MainModel;
......
...@@ -8,7 +8,7 @@ import org.jetbrains.annotations.NotNull; ...@@ -8,7 +8,7 @@ import org.jetbrains.annotations.NotNull;
import org.jetbrains.annotations.Nullable; import org.jetbrains.annotations.Nullable;
import org.vadere.simulator.models.potential.combinedPotentials.CombinedPotentialStrategy; import org.vadere.simulator.models.potential.combinedPotentials.CombinedPotentialStrategy;
import org.vadere.simulator.models.potential.combinedPotentials.TargetRepulsionStrategy; 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.cognition.SelfCategory;
import org.vadere.state.psychology.perception.types.ChangeTarget; import org.vadere.state.psychology.perception.types.ChangeTarget;
import org.vadere.state.psychology.perception.types.Stimulus; import org.vadere.state.psychology.perception.types.Stimulus;
...@@ -24,8 +24,6 @@ import org.vadere.util.logging.Logger; ...@@ -24,8 +24,6 @@ import org.vadere.util.logging.Logger;
import java.awt.geom.Rectangle2D; import java.awt.geom.Rectangle2D;
import java.util.LinkedList; import java.util.LinkedList;
import java.util.List; import java.util.List;
import java.util.Vector;
import java.util.stream.Collectors;
/** /**
* A class to encapsulate the behavior of a single {@link PedestrianOSM}. * A class to encapsulate the behavior of a single {@link PedestrianOSM}.
...@@ -298,7 +296,7 @@ public class OSMBehaviorController { ...@@ -298,7 +296,7 @@ public class OSMBehaviorController {
if (pedestrian.getCombinedPotentialStrategy() instanceof TargetRepulsionStrategy) { if (pedestrian.getCombinedPotentialStrategy() instanceof TargetRepulsionStrategy) {
ScenarioElement searchPosition = (pedestrian.getSource() == null) ? pedestrian : pedestrian.getSource(); 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; assert closestTarget != null;
...@@ -310,21 +308,6 @@ public class OSMBehaviorController { ...@@ -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) { public void changeTarget(PedestrianOSM pedestrian, Topography topography) {
Stimulus mostImportantStimulus = pedestrian.getMostImportantStimulus(); Stimulus mostImportantStimulus = pedestrian.getMostImportantStimulus();
...@@ -349,19 +332,13 @@ public class OSMBehaviorController { ...@@ -349,19 +332,13 @@ public class OSMBehaviorController {
return null; return null;
} }
List<Pedestrian> closestPedestrians = getClosestPedestriansWhichAreCloserToTarget(pedestrian, topography); List<Pedestrian> closestPedestrians = TopographyHelper.getNeighborsCloserToTarget(pedestrian, topography);
if (closestPedestrians.size() > 0) { if (closestPedestrians.size() > 0) {
for (Pedestrian closestPedestrian : closestPedestrians) { for (Pedestrian closestPedestrian : closestPedestrians) {
if (closestPedestrian.hasNextTarget()) { if (closestPedestrian.hasNextTarget()) {
boolean closestPedIsCooperative = closestPedestrian.getSelfCategory() == SelfCategory.COOPERATIVE; boolean closestPedIsCooperative = closestPedestrian.getSelfCategory() == SelfCategory.COOPERATIVE;
boolean walkingDirectionDiffers = false; boolean walkingDirectionDiffers = TopographyHelper.walkingDirectionDiffers(pedestrian, closestPedestrian, topography);
double angleInRadian = calculateAngleBetweenWalkingDirections(pedestrian, closestPedestrian, topography);
if (angleInRadian == -1 || Math.toDegrees(angleInRadian) > pedestrian.getAttributes().getWalkingDirectionSameIfAngleLessOrEqual()) {
walkingDirectionDiffers = true;
}
if (closestPedIsCooperative && walkingDirectionDiffers) { if (closestPedIsCooperative && walkingDirectionDiffers) {
return (PedestrianOSM)closestPedestrian; return (PedestrianOSM)closestPedestrian;
...@@ -375,122 +352,6 @@ public class OSMBehaviorController { ...@@ -375,122 +352,6 @@ public class OSMBehaviorController {
return null; 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. * Swap two pedestrians.
* *
......
...@@ -179,20 +179,4 @@ public class CounterflowCognitionModelTest { ...@@ -179,20 +179,4 @@ public class CounterflowCognitionModelTest {
} }
} }
@Test(expected = IllegalArgumentException.class)
public void updateThrowsExceptionIfInvalidAngleCalculationTypeIsChosen() {
boolean usePedIdAsTargetId = true;
List<Pedestrian> pedestrians = createPedestrians(2, usePedIdAsTargetId);
Topography topography = createTopography(pedestrians);
pedestrians.get(0).getAttributes().setWalkingDirectionCalculation(AttributesAgent.WalkingDirectionCalculation.BY_GRADIENT);
movePedestrian(pedestrians.get(0), new VPoint(2.5,0), topography);
movePedestrian(pedestrians.get(1), new VPoint(2,0), topography);
CounterflowCognitionModel counterflowCognitionModel = new CounterflowCognitionModel();
counterflowCognitionModel.initialize(topography);
counterflowCognitionModel.update(pedestrians);
}
} }
\ No newline at end of file
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment