Commit 7c2211d7 authored by Benedikt Kleinmeier's avatar Benedikt Kleinmeier
Browse files

Added "angleCalculationType" and "targetOrientationAngleThreshold" to...

Added "angleCalculationType" and "targetOrientationAngleThreshold" to "AttributesAgent" and use it in "OSMBehaviorController".
parent 33e3de47
...@@ -3,6 +3,7 @@ package org.vadere.simulator.models.osm; ...@@ -3,6 +3,7 @@ package org.vadere.simulator.models.osm;
import org.apache.commons.lang3.tuple.Pair; import org.apache.commons.lang3.tuple.Pair;
import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.NotNull;
import org.vadere.simulator.models.potential.combinedPotentials.CombinedPotentialStrategy; import org.vadere.simulator.models.potential.combinedPotentials.CombinedPotentialStrategy;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.behavior.SalientBehavior; import org.vadere.state.behavior.SalientBehavior;
import org.vadere.state.events.types.BangEvent; import org.vadere.state.events.types.BangEvent;
import org.vadere.state.events.types.Event; import org.vadere.state.events.types.Event;
...@@ -133,8 +134,13 @@ public class OSMBehaviorController { ...@@ -133,8 +134,13 @@ public class OSMBehaviorController {
if (closestPedestrians.size() > 0) { if (closestPedestrians.size() > 0) {
for (Pedestrian closestPedestrian : closestPedestrians) { for (Pedestrian closestPedestrian : closestPedestrians) {
boolean closestPedIsCooperative = closestPedestrian.getSalientBehavior() == SalientBehavior.COOPERATIVE; boolean closestPedIsCooperative = closestPedestrian.getSalientBehavior() == SalientBehavior.COOPERATIVE;
// TODO Use method "calculateAngleBetweenTargets()" to decide if target orientation of both pedestrians differs. boolean targetOrientationDiffers = false;
boolean targetOrientationDiffers = true;
double angleInRadian = calculateAngleBetweenTargets(pedestrian, closestPedestrian, topography);
if (angleInRadian == -1 || Math.toDegrees(angleInRadian) > pedestrian.getAttributes().getTargetOrientationAngleThreshold()) {
targetOrientationDiffers = true;
}
if (closestPedIsCooperative && targetOrientationDiffers) { if (closestPedIsCooperative && targetOrientationDiffers) {
swapPedestrians(pedestrian, (PedestrianOSM)closestPedestrian, topography); swapPedestrians(pedestrian, (PedestrianOSM)closestPedestrian, topography);
...@@ -205,9 +211,8 @@ public class OSMBehaviorController { ...@@ -205,9 +211,8 @@ public class OSMBehaviorController {
Target targetPed1 = topography.getTarget(pedestrian1.getNextTargetId()); Target targetPed1 = topography.getTarget(pedestrian1.getNextTargetId());
Target targetPed2 = topography.getTarget(pedestrian2.getNextTargetId()); Target targetPed2 = topography.getTarget(pedestrian2.getNextTargetId());
// TODO Maybe, use "target.getShape().getClosestPoint()" instead of "getCentroid()". VPoint targetVectorPed1 = calculateVectorPedestrianToTarget(pedestrian1, targetPed1);
VPoint targetVectorPed1 = targetPed1.getShape().getCentroid().subtract(pedestrian1.getPosition()); VPoint targetVectorPed2 = calculateVectorPedestrianToTarget(pedestrian2, targetPed2);
VPoint targetVectorPed2 = targetPed2.getShape().getCentroid().subtract(pedestrian2.getPosition());
double dotProduct = targetVectorPed1.dotProduct(targetVectorPed2); double dotProduct = targetVectorPed1.dotProduct(targetVectorPed2);
double multipliedMagnitudes = targetVectorPed1.distanceToOrigin() * targetVectorPed2.distanceToOrigin(); double multipliedMagnitudes = targetVectorPed1.distanceToOrigin() * targetVectorPed2.distanceToOrigin();
...@@ -218,15 +223,30 @@ public class OSMBehaviorController { ...@@ -218,15 +223,30 @@ public class OSMBehaviorController {
return angleInRadian; return angleInRadian;
} }
private VPoint calculateVectorPedestrianToTarget(Pedestrian pedestrian, Target target) {
VPoint vectorPedestrianToTarget = null;
if (pedestrian.getAttributes().getAngleCalculationType() == AttributesAgent.AngleCalculationType.USE_CENTER) {
vectorPedestrianToTarget = target.getShape().getCentroid().subtract(pedestrian.getPosition());
} else if (pedestrian.getAttributes().getAngleCalculationType() == AttributesAgent.AngleCalculationType.USE_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().getAngleCalculationType()));
}
return vectorPedestrianToTarget;
}
private void swapPedestrians(PedestrianOSM pedestrian1, PedestrianOSM pedestrian2, Topography topography) { private void swapPedestrians(PedestrianOSM pedestrian1, PedestrianOSM pedestrian2, Topography topography) {
// TODO Use "makeStep()" to swap both pedestrians to avoid
// "java.lang.AssertionError: Number of pedestrians in LinkedCellGrid does not match number of pedestrians in topography".
VPoint newPosition = pedestrian2.getPosition().clone(); VPoint newPosition = pedestrian2.getPosition().clone();
VPoint oldPosition = pedestrian1.getPosition().clone(); VPoint oldPosition = pedestrian1.getPosition().clone();
pedestrian1.setNextPosition(newPosition); pedestrian1.setNextPosition(newPosition);
pedestrian2.setNextPosition(oldPosition); pedestrian2.setNextPosition(oldPosition);
// Use "makeStep()" to swap both pedestrians to avoid "java.lang.AssertionError:
// Number of pedestrians in LinkedCellGrid does not match number of pedestrians in topography".
makeStep(pedestrian1, topography, pedestrian1.getDurationNextStep()); makeStep(pedestrian1, topography, pedestrian1.getDurationNextStep());
makeStep(pedestrian2, topography, pedestrian2.getDurationNextStep()); makeStep(pedestrian2, topography, pedestrian2.getDurationNextStep());
} }
......
...@@ -11,6 +11,7 @@ import org.vadere.state.scenario.Target; ...@@ -11,6 +11,7 @@ import org.vadere.state.scenario.Target;
import org.vadere.state.scenario.Topography; import org.vadere.state.scenario.Topography;
import org.vadere.util.geometry.shapes.VCircle; import org.vadere.util.geometry.shapes.VCircle;
import org.vadere.util.geometry.shapes.VPoint; import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.VRectangle;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.LinkedList; import java.util.LinkedList;
...@@ -138,6 +139,125 @@ public class OSMBehaviorControllerTest { ...@@ -138,6 +139,125 @@ public class OSMBehaviorControllerTest {
assertEquals(expectedAngle, actualAngle, ALLOWED_DOUBLE_TOLERANCE); assertEquals(expectedAngle, actualAngle, ALLOWED_DOUBLE_TOLERANCE);
} }
@Test
public void calculateAngleBetweenTargetsReturnsCorrectResultIfPedestrian1UsesAngleCalculationTypeUseCenter() {
// Create a topography with two targets.
topography = new Topography();
// Position of target1
Target target1 = new Target(new AttributesTarget());
target1.setShape(new VRectangle(4, 0, 1, 1));
target1.getAttributes().setId(1);
// Position of target2
Target target2 = new Target(new AttributesTarget());
target2.setShape(new VCircle(0, 0, 1));
target2.getAttributes().setId(2);
topography.addTarget(target1);
topography.addTarget(target2);
// Create two pedestrians and assign them the two targets from above.
AttributesAgent attributesAgent = new AttributesAgent();
AttributesOSM attributesOSM = new AttributesOSM();
int seed = 1;
List<Attributes> floorFieldAttributes = new ArrayList<>();
floorFieldAttributes.add(new AttributesFloorField());
IPotentialFieldTargetGrid potentialFieldTargetGrid = IPotentialFieldTargetGrid.createPotentialField(floorFieldAttributes,
topography,
new AttributesAgent(),
attributesOSM.getTargetPotentialModel());
pedestrian1 = new PedestrianOSM(new AttributesOSM(), attributesAgent, topography, new Random(1),
potentialFieldTargetGrid, null, null, null, null);
pedestrian2 = new PedestrianOSM(new AttributesOSM(), attributesAgent, topography, new Random(1),
potentialFieldTargetGrid, null, null, null, null);
// Position of pedestrian1
pedestrian1.setPosition(new VPoint(1, 0));
LinkedList<Integer> targetsPedestrian1 = new LinkedList<>();
targetsPedestrian1.add(target1.getId());
pedestrian1.setTargets(targetsPedestrian1);
// Watch out: the closest point to ped1 is (4, 0) while center is (4.5, 0.5).
pedestrian1.getAttributes().setAngleCalculationType(AttributesAgent.AngleCalculationType.USE_CENTER);
// Position of pedestrian2
pedestrian2.setPosition(new VPoint(2, 0));
LinkedList<Integer> targetsPedestrian2 = new LinkedList<>();
targetsPedestrian2.add(target2.getId());
pedestrian2.setTargets(targetsPedestrian2);
OSMBehaviorController controllerUnderTest = new OSMBehaviorController();
VPoint centerOfTarget1Rectangle = new VPoint(4.5, 0.5);
double expectedAngle = Math.PI - Math.atan2(centerOfTarget1Rectangle.y, centerOfTarget1Rectangle.x);
double actualAngle = controllerUnderTest.calculateAngleBetweenTargets(pedestrian1, pedestrian2, topography);
// It seems that "Math.atan2()" is not as precise as expected. Therefore, increase the allowed tolerance.
assertEquals(expectedAngle, actualAngle, 10e-1);
}
@Test
public void calculateAngleBetweenTargetsReturnsCorrectResultIfPedestrian1UsesAngleCalculationTypeUseClosestPoint() {
// Create a topography with two targets.
topography = new Topography();
// Position of target1
Target target1 = new Target(new AttributesTarget());
target1.setShape(new VRectangle(4, 0, 1, 1));
target1.getAttributes().setId(1);
// Position of target2
Target target2 = new Target(new AttributesTarget());
target2.setShape(new VCircle(0, 0, 1));
target2.getAttributes().setId(2);
topography.addTarget(target1);
topography.addTarget(target2);
// Create two pedestrians and assign them the two targets from above.
AttributesAgent attributesAgent = new AttributesAgent();
AttributesOSM attributesOSM = new AttributesOSM();
int seed = 1;
List<Attributes> floorFieldAttributes = new ArrayList<>();
floorFieldAttributes.add(new AttributesFloorField());
IPotentialFieldTargetGrid potentialFieldTargetGrid = IPotentialFieldTargetGrid.createPotentialField(floorFieldAttributes,
topography,
new AttributesAgent(),
attributesOSM.getTargetPotentialModel());
pedestrian1 = new PedestrianOSM(new AttributesOSM(), attributesAgent, topography, new Random(1),
potentialFieldTargetGrid, null, null, null, null);
pedestrian2 = new PedestrianOSM(new AttributesOSM(), attributesAgent, topography, new Random(1),
potentialFieldTargetGrid, null, null, null, null);
// Position of pedestrian1
pedestrian1.setPosition(new VPoint(1, 0));
LinkedList<Integer> targetsPedestrian1 = new LinkedList<>();
targetsPedestrian1.add(target1.getId());
pedestrian1.setTargets(targetsPedestrian1);
// Watch out: the closest point to ped1 is (4, 0) while center is (4.5, 0.5).
pedestrian1.getAttributes().setAngleCalculationType(AttributesAgent.AngleCalculationType.USE_CLOSEST_POINT);
// Position of pedestrian2
pedestrian2.setPosition(new VPoint(2, 0));
LinkedList<Integer> targetsPedestrian2 = new LinkedList<>();
targetsPedestrian2.add(target2.getId());
pedestrian2.setTargets(targetsPedestrian2);
OSMBehaviorController controllerUnderTest = new OSMBehaviorController();
double expectedAngle = Math.PI;
double actualAngle = controllerUnderTest.calculateAngleBetweenTargets(pedestrian1, pedestrian2, topography);
assertEquals(expectedAngle, actualAngle, ALLOWED_DOUBLE_TOLERANCE);
}
@Test @Test
public void calculateAngleBetweenTargetsReturnsMinusOneIfFirstPedestrianHasNoTarget() { public void calculateAngleBetweenTargetsReturnsMinusOneIfFirstPedestrianHasNoTarget() {
createTwoTargetsAndTwoPedestrians( createTwoTargetsAndTwoPedestrians(
......
...@@ -10,6 +10,10 @@ package org.vadere.state.attributes.scenario; ...@@ -10,6 +10,10 @@ package org.vadere.state.attributes.scenario;
*/ */
public class AttributesAgent extends AttributesDynamicElement { public class AttributesAgent extends AttributesDynamicElement {
// calculate the angle between agent's current position and the target by using either the center of the target or
// the closest point between agent and target
public enum AngleCalculationType { USE_CENTER, USE_CLOSEST_POINT };
// from weidmann-1992 page 18, deviates in seitz-2016c page 2 (Methods): 2.0 // from weidmann-1992 page 18, deviates in seitz-2016c page 2 (Methods): 2.0
private double radius = 0.2; private double radius = 0.2;
...@@ -37,6 +41,35 @@ public class AttributesAgent extends AttributesDynamicElement { ...@@ -37,6 +41,35 @@ public class AttributesAgent extends AttributesDynamicElement {
// agents search for other scenario elements (e.g., other agents) within this radius // agents search for other scenario elements (e.g., other agents) within this radius
private double searchRadius = 1.0; private double searchRadius = 1.0;
// the more robust strategy should be use the target's center for angle calculation
// because the closest point can vary while the agent moves through the topography.
private AngleCalculationType angleCalculationType = AngleCalculationType.USE_CENTER;
/* angle in degree which is used to decide if two pedestrians move into the same direction
*
* The angle "a" is calculated 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>
*
* If the calculated angle "a" is equal or below this threshold, it is assumed that both pedestrians move into
* the same direction and both cannot be swapped.
*/
private double targetOrientationAngleThreshold = 45.0;
public AttributesAgent() { public AttributesAgent() {
this(-1); this(-1);
} }
...@@ -59,6 +92,7 @@ public class AttributesAgent extends AttributesDynamicElement { ...@@ -59,6 +92,7 @@ public class AttributesAgent extends AttributesDynamicElement {
this.acceleration = other.acceleration; this.acceleration = other.acceleration;
this.footStepsToStore = other.footStepsToStore; this.footStepsToStore = other.footStepsToStore;
this.searchRadius = other.searchRadius; this.searchRadius = other.searchRadius;
this.angleCalculationType = other.angleCalculationType;
} }
// Getters... // Getters...
...@@ -95,6 +129,10 @@ public class AttributesAgent extends AttributesDynamicElement { ...@@ -95,6 +129,10 @@ public class AttributesAgent extends AttributesDynamicElement {
public double getSearchRadius() { return searchRadius; } public double getSearchRadius() { return searchRadius; }
public AngleCalculationType getAngleCalculationType() { return angleCalculationType; }
public double getTargetOrientationAngleThreshold() { return targetOrientationAngleThreshold; }
// Setters... // Setters...
public void setRadius(double radius) { public void setRadius(double radius) {
...@@ -141,4 +179,14 @@ public class AttributesAgent extends AttributesDynamicElement { ...@@ -141,4 +179,14 @@ public class AttributesAgent extends AttributesDynamicElement {
checkSealed(); checkSealed();
this.searchRadius = searchRadius; this.searchRadius = searchRadius;
} }
public void setAngleCalculationType(AngleCalculationType angleCalculationType) {
checkSealed();
this.angleCalculationType = angleCalculationType;
}
public void setTargetOrientationAngleThreshold(double targetOrientationAngleThreshold) {
checkSealed();
this.targetOrientationAngleThreshold = targetOrientationAngleThreshold;
}
} }
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