Commit cd65cfb9 authored by Benedikt Zoennchen's avatar Benedikt Zoennchen

new robust computation of the target direction for the BHM which is enabled if...

new robust computation of the target direction for the BHM which is enabled if a target potential is available. New TimeCostFunction to compute a target potential repulsive at obstacles.
parent d1d0a432
...@@ -64,7 +64,7 @@ ...@@ -64,7 +64,7 @@
"stepLengthIntercept" : 0.4625, "stepLengthIntercept" : 0.4625,
"stepLengthSlopeSpeed" : 0.2345, "stepLengthSlopeSpeed" : 0.2345,
"stepLengthSD" : 0.036, "stepLengthSD" : 0.036,
"stepLengthDeviation" : false, "stepLengthDeviation" : true,
"navigationCluster" : false, "navigationCluster" : false,
"navigationFollower" : false, "navigationFollower" : false,
"directionWallDistance" : false, "directionWallDistance" : false,
...@@ -72,7 +72,7 @@ ...@@ -72,7 +72,7 @@
"sidewaysEvasion" : false, "sidewaysEvasion" : false,
"onlyEvadeContraFlow" : false, "onlyEvadeContraFlow" : false,
"makeSmallSteps" : false, "makeSmallSteps" : false,
"followerProximityNavigation" : true, "followerProximityNavigation" : false,
"differentBehaviour" : false, "differentBehaviour" : false,
"differentEvasionBehaviourPercentage" : [ ], "differentEvasionBehaviourPercentage" : [ ],
"varyingBehaviour" : false, "varyingBehaviour" : false,
...@@ -86,8 +86,8 @@ ...@@ -86,8 +86,8 @@
"followerDistance" : 10.0, "followerDistance" : 10.0,
"smallStepResolution" : 5, "smallStepResolution" : 5,
"plannedStepsAhead" : 5, "plannedStepsAhead" : 5,
"obstacleRepulsionReach" : 1.0, "obstacleRepulsionReach" : 0.5,
"obstacleRepulsionMaxWeight" : 0.5, "obstacleRepulsionMaxWeight" : 6.0,
"distanceToKeep" : 0.5, "distanceToKeep" : 0.5,
"backwardsAngle" : 1.5707963267948966, "backwardsAngle" : 1.5707963267948966,
"reconsiderOldTargets" : false, "reconsiderOldTargets" : false,
...@@ -103,22 +103,24 @@ ...@@ -103,22 +103,24 @@
"targetAttractionStrength" : 1.0, "targetAttractionStrength" : 1.0,
"timeCostAttributes" : { "timeCostAttributes" : {
"standardDeviation" : 0.2, "standardDeviation" : 0.2,
"type" : "UNIT", "type" : "DISTANCE_TO_OBSTACLES",
"obstacleDensityWeight" : 3.5, "obstacleDensityWeight" : 1.0,
"pedestrianSameTargetDensityWeight" : 3.5, "pedestrianSameTargetDensityWeight" : 3.5,
"pedestrianOtherTargetDensityWeight" : 3.5, "pedestrianOtherTargetDensityWeight" : 3.5,
"pedestrianWeight" : 3.5, "pedestrianWeight" : 3.5,
"queueWidthLoading" : 1.0, "queueWidthLoading" : 1.0,
"pedestrianDynamicWeight" : 6.0, "pedestrianDynamicWeight" : 6.0,
"loadingType" : "CONSTANT" "loadingType" : "CONSTANT",
"width" : 0.4,
"height" : 5.0
} }
} }
}, },
"attributesSimulation" : { "attributesSimulation" : {
"finishTime" : 200.0, "finishTime" : 500.0,
"simTimeStepLength" : 0.4, "simTimeStepLength" : 0.4,
"realTimeSimTimeRatio" : 0.0, "realTimeSimTimeRatio" : 0.0,
"writeSimulationData" : true, "writeSimulationData" : false,
"visualizationEnabled" : true, "visualizationEnabled" : true,
"printFPS" : false, "printFPS" : false,
"digitsPerCoordinate" : 2, "digitsPerCoordinate" : 2,
...@@ -138,15 +140,6 @@ ...@@ -138,15 +140,6 @@
"bounded" : true "bounded" : true
}, },
"obstacles" : [ { "obstacles" : [ {
"shape" : {
"x" : 75.0,
"y" : 77.0,
"width" : 26.1,
"height" : 20.0,
"type" : "RECTANGLE"
},
"id" : -1
}, {
"shape" : { "shape" : {
"x" : 63.5, "x" : 63.5,
"y" : 99.5, "y" : 99.5,
...@@ -155,13 +148,31 @@ ...@@ -155,13 +148,31 @@
"type" : "RECTANGLE" "type" : "RECTANGLE"
}, },
"id" : -1 "id" : -1
}, {
"shape" : {
"type" : "POLYGON",
"points" : [ {
"x" : 105.1,
"y" : 76.69999999999999
}, {
"x" : 76.4,
"y" : 77.39999999999999
}, {
"x" : 76.0,
"y" : 95.1
}, {
"x" : 107.70000000000002,
"y" : 95.19999999999999
} ]
},
"id" : -1
} ], } ],
"stairs" : [ ], "stairs" : [ ],
"targets" : [ { "targets" : [ {
"id" : 1, "id" : 1,
"absorbing" : true, "absorbing" : true,
"shape" : { "shape" : {
"x" : 76.0, "x" : 67.0,
"y" : 107.0, "y" : 107.0,
"width" : 15.0, "width" : 15.0,
"height" : 5.0, "height" : 5.0,
...@@ -187,7 +198,7 @@ ...@@ -187,7 +198,7 @@
}, },
"interSpawnTimeDistribution" : "org.vadere.state.scenario.ConstantDistribution", "interSpawnTimeDistribution" : "org.vadere.state.scenario.ConstantDistribution",
"distributionParameters" : [ 1.0 ], "distributionParameters" : [ 1.0 ],
"spawnNumber" : 100, "spawnNumber" : 500,
"maxSpawnNumberTotal" : -1, "maxSpawnNumberTotal" : -1,
"startTime" : 0.0, "startTime" : 0.0,
"endTime" : 0.0, "endTime" : 0.0,
......
...@@ -64,15 +64,15 @@ ...@@ -64,15 +64,15 @@
"stepLengthIntercept" : 0.4625, "stepLengthIntercept" : 0.4625,
"stepLengthSlopeSpeed" : 0.2345, "stepLengthSlopeSpeed" : 0.2345,
"stepLengthSD" : 0.036, "stepLengthSD" : 0.036,
"stepLengthDeviation" : false, "stepLengthDeviation" : true,
"navigationCluster" : false, "navigationCluster" : false,
"navigationFollower" : false, "navigationFollower" : false,
"directionWallDistance" : false, "directionWallDistance" : false,
"tangentialEvasion" : true, "tangentialEvasion" : false,
"sidewaysEvasion" : true, "sidewaysEvasion" : false,
"onlyEvadeContraFlow" : false, "onlyEvadeContraFlow" : false,
"makeSmallSteps" : true, "makeSmallSteps" : false,
"followerProximityNavigation" : true, "followerProximityNavigation" : false,
"differentBehaviour" : false, "differentBehaviour" : false,
"differentEvasionBehaviourPercentage" : [ ], "differentEvasionBehaviourPercentage" : [ ],
"varyingBehaviour" : false, "varyingBehaviour" : false,
...@@ -86,8 +86,8 @@ ...@@ -86,8 +86,8 @@
"followerDistance" : 10.0, "followerDistance" : 10.0,
"smallStepResolution" : 5, "smallStepResolution" : 5,
"plannedStepsAhead" : 5, "plannedStepsAhead" : 5,
"obstacleRepulsionReach" : 1.0, "obstacleRepulsionReach" : 0.5,
"obstacleRepulsionMaxWeight" : 0.5, "obstacleRepulsionMaxWeight" : 6.0,
"distanceToKeep" : 0.5, "distanceToKeep" : 0.5,
"backwardsAngle" : 1.5707963267948966, "backwardsAngle" : 1.5707963267948966,
"reconsiderOldTargets" : false, "reconsiderOldTargets" : false,
...@@ -102,22 +102,24 @@ ...@@ -102,22 +102,24 @@
"obstacleGridPenalty" : 0.1, "obstacleGridPenalty" : 0.1,
"targetAttractionStrength" : 1.0, "targetAttractionStrength" : 1.0,
"timeCostAttributes" : { "timeCostAttributes" : {
"standardDeviation" : 0.7, "standardDeviation" : 0.2,
"type" : "OBSTACLES", "type" : "DISTANCE_TO_OBSTACLES",
"obstacleDensityWeight" : 1.0, "obstacleDensityWeight" : 1.0,
"pedestrianSameTargetDensityWeight" : 3.5, "pedestrianSameTargetDensityWeight" : 3.5,
"pedestrianOtherTargetDensityWeight" : 3.5, "pedestrianOtherTargetDensityWeight" : 3.5,
"pedestrianWeight" : 3.5, "pedestrianWeight" : 3.5,
"queueWidthLoading" : 1.0, "queueWidthLoading" : 1.0,
"pedestrianDynamicWeight" : 6.0, "pedestrianDynamicWeight" : 6.0,
"loadingType" : "CONSTANT" "loadingType" : "CONSTANT",
"width" : 0.4,
"height" : 5.0
} }
} }
}, },
"attributesSimulation" : { "attributesSimulation" : {
"finishTime" : 200.0, "finishTime" : 200.0,
"simTimeStepLength" : 0.4, "simTimeStepLength" : 0.4,
"realTimeSimTimeRatio" : 0.0, "realTimeSimTimeRatio" : 0.2,
"writeSimulationData" : true, "writeSimulationData" : true,
"visualizationEnabled" : true, "visualizationEnabled" : true,
"printFPS" : false, "printFPS" : false,
...@@ -139,8 +141,8 @@ ...@@ -139,8 +141,8 @@
}, },
"obstacles" : [ { "obstacles" : [ {
"shape" : { "shape" : {
"x" : -2.5, "x" : -2.6,
"y" : 43.5, "y" : 44.3,
"width" : 23.9, "width" : 23.9,
"height" : 3.6, "height" : 3.6,
"type" : "RECTANGLE" "type" : "RECTANGLE"
...@@ -157,8 +159,8 @@ ...@@ -157,8 +159,8 @@
"id" : -1 "id" : -1
}, { }, {
"shape" : { "shape" : {
"x" : 0.4, "x" : 0.3,
"y" : 36.7, "y" : 36.5,
"width" : 29.9, "width" : 29.9,
"height" : 1.9, "height" : 1.9,
"type" : "RECTANGLE" "type" : "RECTANGLE"
...@@ -166,13 +168,22 @@ ...@@ -166,13 +168,22 @@
"id" : -1 "id" : -1
}, { }, {
"shape" : { "shape" : {
"x" : 1.6, "x" : 2.9,
"y" : 33.0, "y" : 34.1,
"width" : 33.4, "width" : 34.4,
"height" : 1.9, "height" : 1.9,
"type" : "RECTANGLE" "type" : "RECTANGLE"
}, },
"id" : -1 "id" : -1
}, {
"shape" : {
"x" : 20.1,
"y" : 43.4,
"width" : 3.3,
"height" : 4.6,
"type" : "RECTANGLE"
},
"id" : -1
} ], } ],
"stairs" : [ ], "stairs" : [ ],
"targets" : [ { "targets" : [ {
......
...@@ -6,6 +6,7 @@ import org.vadere.simulator.control.factory.SourceControllerFactory; ...@@ -6,6 +6,7 @@ import org.vadere.simulator.control.factory.SourceControllerFactory;
import org.vadere.simulator.models.DynamicElementFactory; import org.vadere.simulator.models.DynamicElementFactory;
import org.vadere.simulator.models.MainModel; import org.vadere.simulator.models.MainModel;
import org.vadere.simulator.models.Model; import org.vadere.simulator.models.Model;
import org.vadere.simulator.models.bhm.BehaviouralHeuristicsModel;
import org.vadere.simulator.models.osm.PedestrianOSM; import org.vadere.simulator.models.osm.PedestrianOSM;
import org.vadere.simulator.models.potential.PotentialFieldModel; import org.vadere.simulator.models.potential.PotentialFieldModel;
import org.vadere.simulator.models.potential.fields.IPotentialField; import org.vadere.simulator.models.potential.fields.IPotentialField;
...@@ -111,6 +112,11 @@ public class Simulation { ...@@ -111,6 +112,11 @@ public class Simulation {
IPotentialField pt = null; IPotentialField pt = null;
if(mainModel instanceof PotentialFieldModel) { if(mainModel instanceof PotentialFieldModel) {
pft = ((PotentialFieldModel) mainModel).getPotentialFieldTarget(); pft = ((PotentialFieldModel) mainModel).getPotentialFieldTarget();
} else if(mainModel instanceof BehaviouralHeuristicsModel) {
pft = ((BehaviouralHeuristicsModel) mainModel).getPotentialFieldTarget();
}
if(pft != null) {
pt = (pos, agent) -> { pt = (pos, agent) -> {
if(agent instanceof PedestrianOSM) { if(agent instanceof PedestrianOSM) {
return ((PedestrianOSM)agent).getPotential(pos); return ((PedestrianOSM)agent).getPotential(pos);
......
...@@ -62,6 +62,10 @@ public class BehaviouralHeuristicsModel implements MainModel { ...@@ -62,6 +62,10 @@ public class BehaviouralHeuristicsModel implements MainModel {
this.pedestrianEventsQueue = new PriorityQueue<>(100, new ComparatorPedestrianBHM()); this.pedestrianEventsQueue = new PriorityQueue<>(100, new ComparatorPedestrianBHM());
} }
public IPotentialFieldTarget getPotentialFieldTarget() {
return potentialFieldTarget;
}
@Override @Override
public void initialize(List<Attributes> modelAttributesList, Topography topography, public void initialize(List<Attributes> modelAttributesList, Topography topography,
AttributesAgent attributesPedestrian, Random random) { AttributesAgent attributesPedestrian, Random random) {
......
...@@ -3,11 +3,21 @@ package org.vadere.simulator.models.bhm; ...@@ -3,11 +3,21 @@ package org.vadere.simulator.models.bhm;
import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.NotNull;
import org.vadere.state.attributes.models.AttributesBHM; import org.vadere.state.attributes.models.AttributesBHM;
import org.vadere.state.scenario.Obstacle; import org.vadere.state.scenario.Obstacle;
import org.vadere.util.geometry.GeometryUtils;
import org.vadere.util.geometry.shapes.IPoint;
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.VShape;
import java.util.List; import java.util.List;
import java.util.Optional; import java.util.Optional;
/**
* Computes a vector to add to the target direction such that collisions with obstacles
* will be avoided.
*
* @author Benedikt Zoennchen
*/
public class DirectionAddendObstacleTargetPotential implements DirectionAddend { public class DirectionAddendObstacleTargetPotential implements DirectionAddend {
private final AttributesBHM attributesBHM; private final AttributesBHM attributesBHM;
...@@ -22,15 +32,74 @@ public class DirectionAddendObstacleTargetPotential implements DirectionAddend { ...@@ -22,15 +32,74 @@ public class DirectionAddendObstacleTargetPotential implements DirectionAddend {
public VPoint getDirectionAddend(@NotNull final VPoint targetDirection) { public VPoint getDirectionAddend(@NotNull final VPoint targetDirection) {
VPoint addend = VPoint.ZERO; VPoint addend = VPoint.ZERO;
Optional<Obstacle> closeObstacles = me.detectClosestObstacleProximity(me.getPosition(), me.getRadius()); VPoint footStep = targetDirection.scalarMultiply(me.getStepLength());
// compute the next position without changing the target direction.
VPoint nextPosition = (me.getPosition().add(footStep));
// get the obstacle closest to the nextPosition causing a collision
Optional<Obstacle> closeObstacles = me.detectClosestObstacleProximity(nextPosition, me.getRadius() + GeometryUtils.DOUBLE_EPS);
// if there is none, there is no need to change the target direction
if(closeObstacles.isPresent()) { if(closeObstacles.isPresent()) {
closeObstacles = me.detectClosestObstacleProximity(me.getPosition(), me.getRadius() + footStep.distanceToOrigin() + GeometryUtils.DOUBLE_EPS);
Obstacle obstacle = closeObstacles.get(); Obstacle obstacle = closeObstacles.get();
// compute the point of the obstacle shape closest to the pedestrian position
VPoint closestPoint = obstacle.getShape().closestPoint(me.getPosition()); VPoint closestPoint = obstacle.getShape().closestPoint(me.getPosition());
// compute the normal of the closest line (here we assume the obstacle is in fact a polygon!)
VPoint normal = closestPoint.subtract(me.getPosition());
// project the target direction onto the normal
IPoint p = GeometryUtils.projectOnto(targetDirection.getX(), targetDirection.getY(), normal.x, normal.y);
// if the target direction points away from the obstacle don't adjust it
if(!p.equals(VPoint.ZERO)/* && p.norm().distance(normal.norm()) < GeometryUtils.DOUBLE_EPS*/) {
// if the target direction points in the opposite direction
if(targetDirection.subtract(p).distanceToOrigin() < GeometryUtils.DOUBLE_EPS) {
VPoint lastFootStep = me.getPosition().subtract(me.getLastPosition());
addend = lastFootStep.norm();
}
else {
addend = new VPoint(p.scalarMultiply(-1.0));
}
}
} }
VPoint newTargetDirection = targetDirection.add(addend).norm();
VPoint newFootStep = newTargetDirection.scalarMultiply(me.getStepLength());
closeObstacles = me.detectClosestObstacleProximity(me.getPosition().add(newFootStep), me.getRadius());
if(closeObstacles.isPresent()) {
VPoint lastFootStep = me.getPosition().subtract(me.getLastPosition());
addend = targetDirection.scalarMultiply(-1.0).add(lastFootStep.norm());
}
return addend; return addend;
} }
private VPoint getClosestPoint(VShape shape, VPoint start, VPoint end) {
boolean contains = shape.contains(end);
VPoint closestPoint;
if(contains) {
Optional<VPoint> closestIntersectionPoint = shape.getClosestIntersectionPoint(start, end, start);
// this should never happen!
if(!closestIntersectionPoint.isPresent()) {
return end;
}
closestPoint = closestIntersectionPoint.get();
} else {
closestPoint = shape.closestPoint(end);
}
return closestPoint;
}
} }
package org.vadere.simulator.models.bhm; package org.vadere.simulator.models.bhm;
import org.jetbrains.annotations.NotNull;
import org.vadere.state.attributes.models.AttributesBHM; import org.vadere.state.attributes.models.AttributesBHM;
import org.vadere.state.scenario.Obstacle;
import org.vadere.state.scenario.Pedestrian; import org.vadere.state.scenario.Pedestrian;
import org.vadere.util.geometry.GeometryUtils;
import org.vadere.util.geometry.shapes.VPoint; import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.VShape;
import org.vadere.util.logging.Logger; import org.vadere.util.logging.Logger;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.List; import java.util.List;
import java.util.Optional;
import java.util.Random; import java.util.Random;
public class NavigationProximity implements Navigation { public class NavigationProximity implements Navigation {
...@@ -23,17 +28,75 @@ public class NavigationProximity implements Navigation { ...@@ -23,17 +28,75 @@ public class NavigationProximity implements Navigation {
this.attributesBHM = me.getAttributesBHM(); this.attributesBHM = me.getAttributesBHM();
} }
private VPoint findSmallerStep(List<Obstacle> collideObstacles, VPoint currentNextPosition) {
VPoint newNextPosition = currentNextPosition;
for(Obstacle obstacle : collideObstacles) {
newNextPosition = findCollisionFreePosition(obstacle, me.getPosition(), newNextPosition);
}
return newNextPosition;
}
private VPoint findCollisionFreePosition(@NotNull final Obstacle obstacle, VPoint start, VPoint end) {
VPoint direction = end.subtract(start);
VShape shape = obstacle.getShape();
boolean contains = shape.contains(end);
VPoint closestPoint;
if(contains) {
Optional<VPoint> closestIntersectionPoint = shape.getClosestIntersectionPoint(start, end, start);
// this should never happen!
if(!closestIntersectionPoint.isPresent()) {
return end;
}
closestPoint = closestIntersectionPoint.get();
} else {
closestPoint = shape.closestPoint(end);
}
double distance = contains ? -closestPoint.distance(end) : closestPoint.distance(end);
double diff = me.getRadius() - distance + 0.1;
assert diff > 0;
VPoint normal = end.subtract(closestPoint);
if(contains) {
normal = normal.scalarMultiply(-1.0);
}
VPoint q1 = end.add(normal.setMagnitude(diff));
VPoint q2 = q1.add(normal.rotate(Math.PI * 0.5));
VPoint newEnd = GeometryUtils.lineIntersectionPoint(q1, q2, start, end);
VPoint newDirection = newEnd.subtract(start);
// the new end generates a shorter step in the same direction?
if(newDirection.distanceToOrigin() < direction.distanceToOrigin() && direction.subtract(newDirection).distanceToOrigin() < direction.distanceToOrigin()) {
return newEnd;
} else {
return end;
}
//return newEnd;
}
@Override @Override
public VPoint getNavigationPosition() { public VPoint getNavigationPosition() {
me.action = 1; // LOGGING me.action = 1; // LOGGING
VPoint result = me.computeTargetStep(); VPoint result = me.computeTargetStep();
boolean targetDirection = true;
// this is a problem since the ped will never move!
List<Obstacle> collideObstacles = me.detectObstacleProximity(result, me.getRadius());
if(attributesBHM.isMakeSmallSteps() && !collideObstacles.isEmpty()) {
collideObstacles = me.detectObstacleProximity(result, me.getRadius());
result = findSmallerStep(collideObstacles, result);
}
if (me.evadesTangentially()) { if (me.evadesTangentially()) {
Pedestrian collisionPed = me.findCollisionPedestrian(result, false); Pedestrian collisionPed = me.findCollisionPedestrian(result, false);
if (collisionPed != null) { if (collisionPed != null) {
targetDirection = false;
// walk away if currently in a collision // walk away if currently in a collision
if (me.collidesWithPedestrian(me.getPosition(), attributesBHM.getSpaceToKeep()) if (me.collidesWithPedestrian(me.getPosition(), attributesBHM.getSpaceToKeep())
...@@ -55,6 +118,7 @@ public class NavigationProximity implements Navigation { ...@@ -55,6 +118,7 @@ public class NavigationProximity implements Navigation {
if (angleBetween > attributesBHM.getOnlyEvadeContraFlowAngle()) { if (angleBetween > attributesBHM.getOnlyEvadeContraFlowAngle()) {
result = evadeCollision(collisionPed); result = evadeCollision(collisionPed);
} }
} else { } else {
result = evadeCollision(collisionPed); result = evadeCollision(collisionPed);
...@@ -62,10 +126,18 @@ public class NavigationProximity implements Navigation { ...@@ -62,10 +126,18 @@ public class NavigationProximity implements Navigation {
}