Commit 0c8ac8ea authored by Benedikt Kleinmeier's avatar Benedikt Kleinmeier

Started to implement method "swapWithClosestCooperativePedestrian()" in class...

Started to implement method "swapWithClosestCooperativePedestrian()" in class "OSMBehaviorController".

VadereUtils/src/org/vadere/util/geometry/shapes/IPoint.java:
- Implemented default method "dotProduct()" including unit tests in "IPointTest.java".
parent 14b81e8d
Pipeline #103939 failed with stages
in 52 minutes and 16 seconds
......@@ -3,6 +3,7 @@ package org.vadere.simulator.models.osm;
import org.apache.commons.lang3.tuple.Pair;
import org.jetbrains.annotations.NotNull;
import org.vadere.simulator.models.potential.combinedPotentials.CombinedPotentialStrategy;
import org.vadere.state.behavior.SalientBehavior;
import org.vadere.state.events.types.BangEvent;
import org.vadere.state.events.types.Event;
import org.vadere.state.scenario.Pedestrian;
......@@ -104,25 +105,112 @@ public class OSMBehaviorController {
}
}
/**
* Try to swap the given pedestrian with the closest cooperative pedestrian.
* Carry out the following steps:
*
* <ol>
* <li>Use topography to find a closest Pedestrian within step circle which is closer to target than the given pedestrian.</li>
* <li>Check if candidate is SalientBehavior.COOPERATIVE.</li>
* <li>Check if target orientation of candidate differs from own orientation.</li>
* <li>Swap if checks (2) and (3) are true.</li>
* </ol>
*
* @param pedestrian The pedestrian which would like to swap the position.
* @param topography The topography is required to find the neighbors of the given pedestrian.
*/
public void swapWithClosestCooperativePedestrian(PedestrianOSM pedestrian, Topography topography) {
// TODO
// 1. Use topography to find a closest Pedestrian within step circle.
// 2. Check if candidate is SalientBehavior.COOPERATIVE.
// 3. Check if target orientation of candidate differs from own orientation.
// 4. Swap if checks (2) and (3) are true.
List<Pedestrian> closestPedestrians = topography.getSpatialMap(Pedestrian.class)
.getObjects(pedestrian.getPosition(), pedestrian.getRadius() * 5);
closestPedestrians = closestPedestrians.stream().filter(candidate -> pedestrian.getId() != candidate.getId()).collect(Collectors.toList());
if (pedestrian.hasNextTarget() == false) { // Ignore pedestrians with no targets.
return;
}
List<Pedestrian> closestPedestrians = getClosestPedestriansWhichAreCloserToTarget(pedestrian, topography);
if (closestPedestrians.size() > 0) {
for (Pedestrian closestPedestrian : closestPedestrians) {
boolean closestPedIsCooperative = closestPedestrian.getSalientBehavior() == SalientBehavior.COOPERATIVE;
// TODO Implement helper method to analyze target orientation.
boolean targetOrientationDiffers = true;
// TODO Remove debug code here.
if (pedestrian.getId() == 14) {
System.out.println(String.format("Me: %s", pedestrian.getPosition()));
System.out.println(String.format("Closest pedestrians: %d", closestPedestrians.size()));
if (closestPedIsCooperative && targetOrientationDiffers) {
swapPedestrians(pedestrian, closestPedestrian);
break;
}
if (closestPedestrians.size() > 0) {
System.out.println(String.format("Closest[0]: %s", closestPedestrians.get(0).getId()));
}
}
}
@NotNull
private List<Pedestrian> getClosestPedestriansWhichAreCloserToTarget(PedestrianOSM pedestrian, Topography topography) {
VPoint positionOfPedestrian = pedestrian.getPosition();
List<Pedestrian> closestPedestrians = topography.getSpatialMap(Pedestrian.class)
.getObjects(positionOfPedestrian, pedestrian.getRadius() * 5);
closestPedestrians = closestPedestrians.stream()
.filter(candidate -> pedestrian.getId() != candidate.getId()) // Filter out "me".
.filter(candidate -> pedestrian.getTargetPotential(candidate.getPosition()) < pedestrian.getTargetPotential(pedestrian.getPosition())) // Filter out pedestrians which are farer away from target than me.
.collect(Collectors.toList());
closestPedestrians = closestPedestrians.stream()
.sorted((pedestrian1, pedestrian2) ->
Double.compare(positionOfPedestrian.distance(pedestrian1.getPosition()), positionOfPedestrian.distance(pedestrian2.getPosition())))
.collect(Collectors.toList());
return closestPedestrians;
}
/**
* 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> 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());
// TODO Maybe, use "target.getShape().getClosestPoint()" instead of "getCentroid()".
VPoint targetVectorPed1 = targetPed1.getShape().getCentroid().subtract(pedestrian1.getPosition());
VPoint targetVectorPed2 = targetPed2.getShape().getCentroid().subtract(pedestrian2.getPosition());
double dotProduct = targetVectorPed1.dotProduct(targetVectorPed2);
double multipliedMagnitudes = targetVectorPed1.distanceToOrigin() * targetVectorPed2.distanceToOrigin();
angleInRadian = Math.acos(dotProduct / multipliedMagnitudes);
}
return angleInRadian;
}
private void swapPedestrians(Pedestrian pedestrian1, Pedestrian pedestrian2) {
VPoint newPosition = pedestrian2.getPosition().clone();
VPoint oldPosition = pedestrian1.getPosition().clone();
pedestrian1.setPosition(newPosition);
pedestrian2.setPosition(oldPosition);
}
}
package org.vadere.simulator.models.osm;
import org.junit.Test;
import org.vadere.simulator.models.potential.fields.IPotentialFieldTargetGrid;
import org.vadere.state.attributes.Attributes;
import org.vadere.state.attributes.models.AttributesFloorField;
import org.vadere.state.attributes.models.AttributesOSM;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.attributes.scenario.AttributesTarget;
import org.vadere.state.scenario.Target;
import org.vadere.state.scenario.Topography;
import org.vadere.util.geometry.shapes.VCircle;
import org.vadere.util.geometry.shapes.VPoint;
import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;
import java.util.Random;
import static org.junit.Assert.*;
public class OSMBehaviorControllerTest {
private static double ALLOWED_DOUBLE_TOLERANCE = 10e-3;
private PedestrianOSM pedestrian1;
private PedestrianOSM pedestrian2;
private Topography topography;
private void createTwoTargetsAndTwoPedestrians(VPoint target1Position, VPoint target2Position, VPoint pedestrian1Position, VPoint pedestrian2Position) {
// Create a topography with two targets.
topography = new Topography();
Target target1 = new Target(new AttributesTarget());
target1.setShape(new VCircle(target1Position, 1));
target1.getAttributes().setId(1);
Target target2 = new Target(new AttributesTarget());
target2.setShape(new VCircle(target2Position, 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);
pedestrian1.setPosition(pedestrian1Position);
LinkedList<Integer> targetsPedestrian1 = new LinkedList<>();
targetsPedestrian1.add(target1.getId());
pedestrian1.setTargets(targetsPedestrian1);
pedestrian2.setPosition(new VPoint(pedestrian2Position));
LinkedList<Integer> targetsPedestrian2 = new LinkedList<>();
targetsPedestrian2.add(target2.getId());
pedestrian2.setTargets(targetsPedestrian2);
}
@Test
public void calculateAngleBetweenTargetsReturnsCorrectResultIfTargetAndPedestriansAreVerticalZeroQuarterCircle() {
createTwoTargetsAndTwoPedestrians(
new VPoint(-1, 0), // target1
new VPoint(-1, 0), // target2
new VPoint(0, 0), // pedestrian1
new VPoint(1, 0) // pedestrian2
);
OSMBehaviorController controllerUnderTest = new OSMBehaviorController();
double expectedAngle = 0;
double actualAngle = controllerUnderTest.calculateAngleBetweenTargets(pedestrian1, pedestrian2, topography);
assertEquals(expectedAngle, actualAngle, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void calculateAngleBetweenTargetsReturnsCorrectResultIfTargetAndPedestriansAreVerticalOneQuarterCircle() {
createTwoTargetsAndTwoPedestrians(
new VPoint(0, 1), // target1
new VPoint(-1, 0), // target2
new VPoint(0, 0), // pedestrian1
new VPoint(1, 0) // pedestrian2
);
OSMBehaviorController controllerUnderTest = new OSMBehaviorController();
double expectedAngle = Math.PI / 2;
double actualAngle = controllerUnderTest.calculateAngleBetweenTargets(pedestrian1, pedestrian2, topography);
assertEquals(expectedAngle, actualAngle, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void calculateAngleBetweenTargetsReturnsCorrectResultIfTargetAndPedestriansAreVerticalTwoQuarterCircle() {
createTwoTargetsAndTwoPedestrians(
new VPoint(-1, 0), // target1
new VPoint(2, 0), // target2
new VPoint(0, 0), // pedestrian1
new VPoint(1, 0) // pedestrian2
);
OSMBehaviorController controllerUnderTest = new OSMBehaviorController();
double expectedAngle = Math.PI;
double actualAngle = controllerUnderTest.calculateAngleBetweenTargets(pedestrian1, pedestrian2, topography);
assertEquals(expectedAngle, actualAngle, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void calculateAngleBetweenTargetsReturnsCorrectResultIfTargetAndPedestriansAreVerticalThreeQuarterCircle() {
createTwoTargetsAndTwoPedestrians(
new VPoint(1, 0), // target1
new VPoint(1, -1), // target2
new VPoint(0, 0), // pedestrian1
new VPoint(1, 0) // pedestrian2
);
OSMBehaviorController controllerUnderTest = new OSMBehaviorController();
double expectedAngle = Math.PI / 2;
double actualAngle = controllerUnderTest.calculateAngleBetweenTargets(pedestrian1, pedestrian2, topography);
assertEquals(expectedAngle, actualAngle, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void calculateAngleBetweenTargetsReturnsMinusOneIfFirstPedestrianHasNoTarget() {
createTwoTargetsAndTwoPedestrians(
new VPoint(1, 0), // target1
new VPoint(1, -1), // target2
new VPoint(0, 0), // pedestrian1
new VPoint(1, 0) // pedestrian2
);
pedestrian1.setTargets(new LinkedList<>());
OSMBehaviorController controllerUnderTest = new OSMBehaviorController();
double expectedAngle = -1;
double actualAngle = controllerUnderTest.calculateAngleBetweenTargets(pedestrian1, pedestrian2, topography);
assertEquals(expectedAngle, actualAngle, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void calculateAngleBetweenTargetsReturnsMinusOneIfSecondPedestrianHasNoTarget() {
createTwoTargetsAndTwoPedestrians(
new VPoint(1, 0), // target1
new VPoint(1, -1), // target2
new VPoint(0, 0), // pedestrian1
new VPoint(1, 0) // pedestrian2
);
pedestrian2.setTargets(new LinkedList<>());
OSMBehaviorController controllerUnderTest = new OSMBehaviorController();
double expectedAngle = -1;
double actualAngle = controllerUnderTest.calculateAngleBetweenTargets(pedestrian1, pedestrian2, topography);
assertEquals(expectedAngle, actualAngle, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void calculateAngleBetweenTargetsReturnsMinusOneIfBothPedestriansHaveNoTargets() {
createTwoTargetsAndTwoPedestrians(
new VPoint(1, 0), // target1
new VPoint(1, -1), // target2
new VPoint(0, 0), // pedestrian1
new VPoint(1, 0) // pedestrian2
);
pedestrian1.setTargets(new LinkedList<>());
pedestrian2.setTargets(new LinkedList<>());
OSMBehaviorController controllerUnderTest = new OSMBehaviorController();
double expectedAngle = -1;
double actualAngle = controllerUnderTest.calculateAngleBetweenTargets(pedestrian1, pedestrian2, topography);
assertEquals(expectedAngle, actualAngle, ALLOWED_DOUBLE_TOLERANCE);
}
}
\ No newline at end of file
......@@ -129,6 +129,18 @@ public interface IPoint extends Cloneable {
return getX() * point.getY() - point.getX() * getY();
}
/**
* Computes the dot product of this and the other point.
*
* This does not effect the coordinates of this.
*
* @param point the other point
* @return the dot product of this and the other
*/
default double dotProduct(IPoint point) {
return (getX() * point.getX()) + (getY() * point.getY());
}
/**
* Clones the point. This will return a copy if the point
* is immutable, otherwise this will return this.
......
......@@ -168,6 +168,8 @@ public class VPoint implements Cloneable, IPoint {
return x * point.getX() + y * point.getY();
}
@Override
public VPoint norm() {
return norm(distanceToOrigin());
......
package org.vadere.util.geometry.shapes;
import org.junit.Test;
import static org.junit.Assert.*;
public class IPointTest {
private static double ALLOWED_DOUBLE_TOLERANCE = 10e-3;
@Test
public void dotProductReturnsNullIfTwoNullVectorsAreCombined() {
VPoint nullVector1 = new VPoint(0, 0);
VPoint nullVector2 = new VPoint(0, 0);
double expectedResult = 0;
double actualResult = nullVector1.dotProduct(nullVector2);
assertEquals(expectedResult, actualResult, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void dotProductReturnsNullIfFirstVectorIsNullVector() {
VPoint nullVector = new VPoint(0, 0);
VPoint vector = new VPoint(1, 2);
double expectedResult = 0;
double actualResult = nullVector.dotProduct(vector);
assertEquals(expectedResult, actualResult, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void dotProductReturnsNullIfSecondVectorIsNullVector() {
VPoint nullVector = new VPoint(0, 0);
VPoint vector = new VPoint(1, 2);
double expectedResult = 0;
double actualResult = vector.dotProduct(nullVector);
assertEquals(expectedResult, actualResult, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void dotProductCombinesTwoPositiveVectorsProperly() {
VPoint vector1 = new VPoint(1, 2);
VPoint vector2 = new VPoint(3, 4);
double expectedResult = (vector1.x * vector2.x) + (vector1.y * vector2.y);
double actualResult = vector2.dotProduct(vector1);
assertEquals(expectedResult, actualResult, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void dotProductCombinesOnePositiveAndOneNegativeVectorProperly() {
VPoint vector1 = new VPoint(1, 2);
VPoint vector2 = new VPoint(-3, -4);
double expectedResult = (vector1.x * vector2.x) + (vector1.y * vector2.y);
double actualResult = vector2.dotProduct(vector1);
assertEquals(expectedResult, actualResult, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void dotProductCombinesOnePositiveAndOneNegativeVectorProperlyIfFirstVectorContainsASingleZero() {
VPoint vector1 = new VPoint(1, 0);
VPoint vector2 = new VPoint(-3, -4);
double expectedResult = (vector1.x * vector2.x) + (vector1.y * vector2.y);
double actualResult = vector2.dotProduct(vector1);
assertEquals(expectedResult, actualResult, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void dotProductCombinesOnePositiveAndOneNegativeVectorProperlyIfSecondVectorContainsASingleZero() {
VPoint vector1 = new VPoint(1, 2);
VPoint vector2 = new VPoint(0, -4);
double expectedResult = (vector1.x * vector2.x) + (vector1.y * vector2.y);
double actualResult = vector2.dotProduct(vector1);
assertEquals(expectedResult, actualResult, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void dotProductReturnsZeroIfBothVectorsAreOrthogonal() {
VPoint vector1 = new VPoint(2, 1);
VPoint vector2 = new VPoint(-1, 2);
double expectedResult = 0;
double actualResult = vector2.dotProduct(vector1);
assertEquals(expectedResult, actualResult, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void dotProductDoesNotChangeUsedNullVectors() {
VPoint nullVector1 = new VPoint(0, 0);
VPoint nullVector2 = new VPoint(0, 0);
double expectedResult = 0;
assertEquals(expectedResult, nullVector1.x, ALLOWED_DOUBLE_TOLERANCE);
assertEquals(expectedResult, nullVector1.y, ALLOWED_DOUBLE_TOLERANCE);
assertEquals(expectedResult, nullVector2.x, ALLOWED_DOUBLE_TOLERANCE);
assertEquals(expectedResult, nullVector2.y, ALLOWED_DOUBLE_TOLERANCE);
}
@Test
public void dotProductDoesNotChangeUsedVectors() {
VPoint vector1 = new VPoint(1, 2);
VPoint vector2 = new VPoint(3, 4);
assertEquals(1, vector1.x, ALLOWED_DOUBLE_TOLERANCE);
assertEquals(2, vector1.y, ALLOWED_DOUBLE_TOLERANCE);
assertEquals(3, vector2.x, ALLOWED_DOUBLE_TOLERANCE);
assertEquals(4, vector2.y, ALLOWED_DOUBLE_TOLERANCE);
}
}
\ 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