From cd65cfb97fd8d80491dfad36625039b4ac5169f3 Mon Sep 17 00:00:00 2001 From: Benedikt Zoennchen Date: Tue, 14 May 2019 15:50:13 +0200 Subject: [PATCH] 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. --- ...ield_navigation_test_displaced_ok.scenario | 51 ++++--- .../floor_field_navigation_test_ok.scenario | 47 +++--- .../vadere/simulator/control/Simulation.java | 6 + .../bhm/BehaviouralHeuristicsModel.java | 4 + ...irectionAddendObstacleTargetPotential.java | 73 +++++++++- .../models/bhm/NavigationProximity.java | 80 ++++++++++- .../simulator/models/bhm/PedestrianBHM.java | 136 +++++++++++------- .../simulator/models/bhm/TargetDirection.java | 8 ++ .../models/bhm/TargetDirectionClose.java | 33 +++++ .../models/bhm/TargetDirectionEuclidean.java | 23 +++ .../bhm/TargetDirectionGeoGradient.java | 44 ++++++ .../models/bhm/TargetDirectionGeoOptimum.java | 77 ++++++++++ .../TargetDirectionGeoOptimumBruteForce.java | 61 ++++++++ .../fields/PotentialFieldTarget.java | 3 +- .../fields/PotentialFieldTargetGrid.java | 11 ++ .../solver/timecost/ITimeCostFunction.java | 4 +- .../TimeCostFunctionFactory.java | 7 + .../TimeCostFunctionObstacleDistance.java | 51 +++++++ .../PotentialFieldTargetQueuingGrid.java | 2 +- .../models/AttributesFloorField.java | 5 + .../attributes/models/AttributesTimeCost.java | 50 +++---- .../models/TimeCostFunctionType.java | 32 +++++ .../vadere/util/geometry/GeometryUtils.java | 70 ++++++++- .../vadere/util/geometry/shapes/VPoint.java | 6 - .../vadere/util/geometry/shapes/VPolygon.java | 72 +++++++++- .../util/geometry/shapes/VRectangle.java | 18 +++ .../vadere/util/geometry/shapes/VRing.java | 19 +++ .../vadere/util/geometry/shapes/VShape.java | 3 + .../vadere/util/math/GoldenSectionSearch.java | 57 ++++++++ 29 files changed, 912 insertions(+), 141 deletions(-) create mode 100644 VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirection.java create mode 100644 VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionClose.java create mode 100644 VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionEuclidean.java create mode 100644 VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionGeoGradient.java create mode 100644 VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionGeoOptimum.java create mode 100644 VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionGeoOptimumBruteForce.java create mode 100644 VadereSimulator/src/org/vadere/simulator/models/potential/timeCostFunction/TimeCostFunctionObstacleDistance.java create mode 100644 VadereState/src/org/vadere/state/attributes/models/TimeCostFunctionType.java create mode 100644 VadereUtils/src/org/vadere/util/math/GoldenSectionSearch.java diff --git a/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_displaced_ok.scenario b/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_displaced_ok.scenario index 35edbc820..01df5d916 100644 --- a/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_displaced_ok.scenario +++ b/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_displaced_ok.scenario @@ -64,7 +64,7 @@ "stepLengthIntercept" : 0.4625, "stepLengthSlopeSpeed" : 0.2345, "stepLengthSD" : 0.036, - "stepLengthDeviation" : false, + "stepLengthDeviation" : true, "navigationCluster" : false, "navigationFollower" : false, "directionWallDistance" : false, @@ -72,7 +72,7 @@ "sidewaysEvasion" : false, "onlyEvadeContraFlow" : false, "makeSmallSteps" : false, - "followerProximityNavigation" : true, + "followerProximityNavigation" : false, "differentBehaviour" : false, "differentEvasionBehaviourPercentage" : [ ], "varyingBehaviour" : false, @@ -86,8 +86,8 @@ "followerDistance" : 10.0, "smallStepResolution" : 5, "plannedStepsAhead" : 5, - "obstacleRepulsionReach" : 1.0, - "obstacleRepulsionMaxWeight" : 0.5, + "obstacleRepulsionReach" : 0.5, + "obstacleRepulsionMaxWeight" : 6.0, "distanceToKeep" : 0.5, "backwardsAngle" : 1.5707963267948966, "reconsiderOldTargets" : false, @@ -103,22 +103,24 @@ "targetAttractionStrength" : 1.0, "timeCostAttributes" : { "standardDeviation" : 0.2, - "type" : "UNIT", - "obstacleDensityWeight" : 3.5, + "type" : "DISTANCE_TO_OBSTACLES", + "obstacleDensityWeight" : 1.0, "pedestrianSameTargetDensityWeight" : 3.5, "pedestrianOtherTargetDensityWeight" : 3.5, "pedestrianWeight" : 3.5, "queueWidthLoading" : 1.0, "pedestrianDynamicWeight" : 6.0, - "loadingType" : "CONSTANT" + "loadingType" : "CONSTANT", + "width" : 0.4, + "height" : 5.0 } } }, "attributesSimulation" : { - "finishTime" : 200.0, + "finishTime" : 500.0, "simTimeStepLength" : 0.4, "realTimeSimTimeRatio" : 0.0, - "writeSimulationData" : true, + "writeSimulationData" : false, "visualizationEnabled" : true, "printFPS" : false, "digitsPerCoordinate" : 2, @@ -138,15 +140,6 @@ "bounded" : true }, "obstacles" : [ { - "shape" : { - "x" : 75.0, - "y" : 77.0, - "width" : 26.1, - "height" : 20.0, - "type" : "RECTANGLE" - }, - "id" : -1 - }, { "shape" : { "x" : 63.5, "y" : 99.5, @@ -155,13 +148,31 @@ "type" : "RECTANGLE" }, "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" : [ ], "targets" : [ { "id" : 1, "absorbing" : true, "shape" : { - "x" : 76.0, + "x" : 67.0, "y" : 107.0, "width" : 15.0, "height" : 5.0, @@ -187,7 +198,7 @@ }, "interSpawnTimeDistribution" : "org.vadere.state.scenario.ConstantDistribution", "distributionParameters" : [ 1.0 ], - "spawnNumber" : 100, + "spawnNumber" : 500, "maxSpawnNumberTotal" : -1, "startTime" : 0.0, "endTime" : 0.0, diff --git a/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_ok.scenario b/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_ok.scenario index 5bdf1e829..8a1db756e 100644 --- a/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_ok.scenario +++ b/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_ok.scenario @@ -64,15 +64,15 @@ "stepLengthIntercept" : 0.4625, "stepLengthSlopeSpeed" : 0.2345, "stepLengthSD" : 0.036, - "stepLengthDeviation" : false, + "stepLengthDeviation" : true, "navigationCluster" : false, "navigationFollower" : false, "directionWallDistance" : false, - "tangentialEvasion" : true, - "sidewaysEvasion" : true, + "tangentialEvasion" : false, + "sidewaysEvasion" : false, "onlyEvadeContraFlow" : false, - "makeSmallSteps" : true, - "followerProximityNavigation" : true, + "makeSmallSteps" : false, + "followerProximityNavigation" : false, "differentBehaviour" : false, "differentEvasionBehaviourPercentage" : [ ], "varyingBehaviour" : false, @@ -86,8 +86,8 @@ "followerDistance" : 10.0, "smallStepResolution" : 5, "plannedStepsAhead" : 5, - "obstacleRepulsionReach" : 1.0, - "obstacleRepulsionMaxWeight" : 0.5, + "obstacleRepulsionReach" : 0.5, + "obstacleRepulsionMaxWeight" : 6.0, "distanceToKeep" : 0.5, "backwardsAngle" : 1.5707963267948966, "reconsiderOldTargets" : false, @@ -102,22 +102,24 @@ "obstacleGridPenalty" : 0.1, "targetAttractionStrength" : 1.0, "timeCostAttributes" : { - "standardDeviation" : 0.7, - "type" : "OBSTACLES", + "standardDeviation" : 0.2, + "type" : "DISTANCE_TO_OBSTACLES", "obstacleDensityWeight" : 1.0, "pedestrianSameTargetDensityWeight" : 3.5, "pedestrianOtherTargetDensityWeight" : 3.5, "pedestrianWeight" : 3.5, "queueWidthLoading" : 1.0, "pedestrianDynamicWeight" : 6.0, - "loadingType" : "CONSTANT" + "loadingType" : "CONSTANT", + "width" : 0.4, + "height" : 5.0 } } }, "attributesSimulation" : { "finishTime" : 200.0, "simTimeStepLength" : 0.4, - "realTimeSimTimeRatio" : 0.0, + "realTimeSimTimeRatio" : 0.2, "writeSimulationData" : true, "visualizationEnabled" : true, "printFPS" : false, @@ -139,8 +141,8 @@ }, "obstacles" : [ { "shape" : { - "x" : -2.5, - "y" : 43.5, + "x" : -2.6, + "y" : 44.3, "width" : 23.9, "height" : 3.6, "type" : "RECTANGLE" @@ -157,8 +159,8 @@ "id" : -1 }, { "shape" : { - "x" : 0.4, - "y" : 36.7, + "x" : 0.3, + "y" : 36.5, "width" : 29.9, "height" : 1.9, "type" : "RECTANGLE" @@ -166,13 +168,22 @@ "id" : -1 }, { "shape" : { - "x" : 1.6, - "y" : 33.0, - "width" : 33.4, + "x" : 2.9, + "y" : 34.1, + "width" : 34.4, "height" : 1.9, "type" : "RECTANGLE" }, "id" : -1 + }, { + "shape" : { + "x" : 20.1, + "y" : 43.4, + "width" : 3.3, + "height" : 4.6, + "type" : "RECTANGLE" + }, + "id" : -1 } ], "stairs" : [ ], "targets" : [ { diff --git a/VadereSimulator/src/org/vadere/simulator/control/Simulation.java b/VadereSimulator/src/org/vadere/simulator/control/Simulation.java index acb407546..826500e7f 100644 --- a/VadereSimulator/src/org/vadere/simulator/control/Simulation.java +++ b/VadereSimulator/src/org/vadere/simulator/control/Simulation.java @@ -6,6 +6,7 @@ import org.vadere.simulator.control.factory.SourceControllerFactory; import org.vadere.simulator.models.DynamicElementFactory; import org.vadere.simulator.models.MainModel; 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.potential.PotentialFieldModel; import org.vadere.simulator.models.potential.fields.IPotentialField; @@ -111,6 +112,11 @@ public class Simulation { IPotentialField pt = null; if(mainModel instanceof PotentialFieldModel) { pft = ((PotentialFieldModel) mainModel).getPotentialFieldTarget(); + } else if(mainModel instanceof BehaviouralHeuristicsModel) { + pft = ((BehaviouralHeuristicsModel) mainModel).getPotentialFieldTarget(); + } + + if(pft != null) { pt = (pos, agent) -> { if(agent instanceof PedestrianOSM) { return ((PedestrianOSM)agent).getPotential(pos); diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/BehaviouralHeuristicsModel.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/BehaviouralHeuristicsModel.java index 58a090a89..0f5f16c9e 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/bhm/BehaviouralHeuristicsModel.java +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/BehaviouralHeuristicsModel.java @@ -62,6 +62,10 @@ public class BehaviouralHeuristicsModel implements MainModel { this.pedestrianEventsQueue = new PriorityQueue<>(100, new ComparatorPedestrianBHM()); } + public IPotentialFieldTarget getPotentialFieldTarget() { + return potentialFieldTarget; + } + @Override public void initialize(List modelAttributesList, Topography topography, AttributesAgent attributesPedestrian, Random random) { diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacleTargetPotential.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacleTargetPotential.java index 8c5f357a1..57e64b11d 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacleTargetPotential.java +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacleTargetPotential.java @@ -3,11 +3,21 @@ package org.vadere.simulator.models.bhm; import org.jetbrains.annotations.NotNull; import org.vadere.state.attributes.models.AttributesBHM; 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.VShape; import java.util.List; 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 { private final AttributesBHM attributesBHM; @@ -22,15 +32,74 @@ public class DirectionAddendObstacleTargetPotential implements DirectionAddend { public VPoint getDirectionAddend(@NotNull final VPoint targetDirection) { VPoint addend = VPoint.ZERO; - Optional 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 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()) { + closeObstacles = me.detectClosestObstacleProximity(me.getPosition(), me.getRadius() + footStep.distanceToOrigin() + GeometryUtils.DOUBLE_EPS); Obstacle obstacle = closeObstacles.get(); + + // compute the point of the obstacle shape closest to the pedestrian position 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; } + + private VPoint getClosestPoint(VShape shape, VPoint start, VPoint end) { + boolean contains = shape.contains(end); + VPoint closestPoint; + + if(contains) { + Optional 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; + } } diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/NavigationProximity.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/NavigationProximity.java index d8889e5bb..1f24485b3 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/bhm/NavigationProximity.java +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/NavigationProximity.java @@ -1,12 +1,17 @@ package org.vadere.simulator.models.bhm; +import org.jetbrains.annotations.NotNull; import org.vadere.state.attributes.models.AttributesBHM; +import org.vadere.state.scenario.Obstacle; 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.VShape; import org.vadere.util.logging.Logger; import java.util.ArrayList; import java.util.List; +import java.util.Optional; import java.util.Random; public class NavigationProximity implements Navigation { @@ -23,17 +28,75 @@ public class NavigationProximity implements Navigation { this.attributesBHM = me.getAttributesBHM(); } + private VPoint findSmallerStep(List 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 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 public VPoint getNavigationPosition() { me.action = 1; // LOGGING VPoint result = me.computeTargetStep(); + boolean targetDirection = true; + + // this is a problem since the ped will never move! + List collideObstacles = me.detectObstacleProximity(result, me.getRadius()); + if(attributesBHM.isMakeSmallSteps() && !collideObstacles.isEmpty()) { + collideObstacles = me.detectObstacleProximity(result, me.getRadius()); + result = findSmallerStep(collideObstacles, result); + } if (me.evadesTangentially()) { Pedestrian collisionPed = me.findCollisionPedestrian(result, false); if (collisionPed != null) { + targetDirection = false; // walk away if currently in a collision if (me.collidesWithPedestrian(me.getPosition(), attributesBHM.getSpaceToKeep()) @@ -55,6 +118,7 @@ public class NavigationProximity implements Navigation { if (angleBetween > attributesBHM.getOnlyEvadeContraFlowAngle()) { result = evadeCollision(collisionPed); + } } else { result = evadeCollision(collisionPed); @@ -62,10 +126,18 @@ public class NavigationProximity implements Navigation { } } - if (me.collidesWithPedestrianOnPath(result) || me.collidesWithObstacle(result) || - // make sure that more distance is kept for numerical stability with step or wait heuristic - (!me.evadesTangentially() && - me.collidesWithPedestrian(result, 2 * attributesBHM.getSpaceToKeep()))) { + + /* + * Make no step if: + * 1) there would be a collision with another pedestrian + * 2) there would be a collision with an obstacle and me does not walk in the origin target direction. + * Remark: if we would not allow collisions me would never move again + * 3) me does not evade tangentially and there is a collision with another pedestrian in a larger area. + * Remark: this is for numerical stability of the step or wait heuristic + */ + if (me.collidesWithPedestrianOnPath(result) || + (me.collidesWithObstacle(result) && !targetDirection) || + (!me.evadesTangentially() && me.collidesWithPedestrian(result, 2 * attributesBHM.getSpaceToKeep()))) { /*if( me.collidesWithObstacle(result) ) { System.out.println("obs collision " + me.getId()); diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/PedestrianBHM.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/PedestrianBHM.java index 9f52cf425..7c8a9f464 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/bhm/PedestrianBHM.java +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/PedestrianBHM.java @@ -22,10 +22,8 @@ import org.vadere.state.scenario.Target; import org.vadere.state.scenario.Topography; import org.vadere.state.simulation.FootStep; import org.vadere.util.geometry.GeometryUtils; -import org.vadere.util.geometry.shapes.VCircle; import org.vadere.util.geometry.shapes.VLine; import org.vadere.util.geometry.shapes.VPoint; -import org.vadere.util.geometry.shapes.VShape; import org.vadere.util.geometry.shapes.Vector2D; import org.vadere.util.logging.Logger; @@ -46,7 +44,7 @@ public class PedestrianBHM extends Pedestrian { private VPoint targetDirection; private final transient Navigation navigation; - private final List directionAddends; + private final transient List directionAddends; protected int action; @@ -54,6 +52,7 @@ public class PedestrianBHM extends Pedestrian { private boolean evadesSideways; private int remainCounter; private transient @Nullable IPotentialFieldTarget potentialFieldTarget; + private transient TargetDirection targetDirectionStrategy; public PedestrianBHM(Topography topography, AttributesAgent attributesPedestrian, AttributesBHM attributesBHM, Random random) { @@ -105,8 +104,17 @@ public class PedestrianBHM extends Pedestrian { } setNextTargetListIndex(0); - setEvasionStrategy(); + setTargetDirectionStrategy(); + } + + private void setTargetDirectionStrategy() { + if(isPotentialFieldInUse()) { + TargetDirection base = new TargetDirectionGeoGradient(this, potentialFieldTarget); + targetDirectionStrategy = new TargetDirectionClose(this, potentialFieldTarget, base); + } else { + targetDirectionStrategy = new TargetDirectionEuclidean(this); + } } private boolean isPotentialFieldInUse() { @@ -238,7 +246,7 @@ public class PedestrianBHM extends Pedestrian { // target direction methods... VPoint computeTargetStep() { - return UtilsBHM.getTargetStep(this, this.getPosition(), this.getTargetDirection()); + return UtilsBHM.getTargetStep(this, getPosition(), getTargetDirection()); } /** @@ -256,28 +264,16 @@ public class PedestrianBHM extends Pedestrian { } if (hasNextTarget()) { - VShape targetShape = topography.getTarget(getNextTargetId()).getShape(); - if (!targetShape.contains(getPosition())) { + Target target = topography.getTarget(getNextTargetId()); + if (!target.getShape().contains(getPosition())) { - // use just euklid direction to the target - if(potentialFieldTarget == null) { - VPoint targetPoint = targetShape.closestPoint(getPosition()); - targetDirection = targetPoint.subtract(getPosition()).norm(); - } - else { - Vector2D vec = new Vector2D(computeTargetDirectionByGradient()); - if(vec.getLength() < GeometryUtils.DOUBLE_EPS) { - targetDirection = VPoint.ZERO; - } - else { - targetDirection = vec.norm(); - } - } + targetDirection = targetDirectionStrategy.getTargetDirection(target); for (DirectionAddend da : directionAddends) { targetDirection = targetDirection.add(da.getDirectionAddend(targetDirection)); } + //TODO: if this happens it might cause problems dependent on the heuristics choose. if(targetDirection.distanceToOrigin() < GeometryUtils.DOUBLE_EPS) { targetDirection = VPoint.ZERO; } @@ -288,33 +284,6 @@ public class PedestrianBHM extends Pedestrian { } } - private VPoint computeTargetDirectionByGradient() { - return potentialFieldTarget.getTargetPotentialGradient(getPosition(), this).multiply(-1.0); - } - - private VPoint computeTargetDirectionByOptimaization() { - Vector2D gradient = potentialFieldTarget.getTargetPotentialGradient(getPosition(), this).multiply(-1.0); - double angle = GeometryUtils.angleTo(gradient, new VPoint(1, 0)); - List possibleNextPositions = GeometryUtils.getDiscDiscretizationPoints( - random, - false, - new VCircle(getPosition(), stepLength), - 1, - 15, - angle, - 2*Math.PI); - - VPoint nextOptimalPos = possibleNextPositions.stream() - .filter(p -> !collidesWithObstacle(p)) - .min( - (p1, p2) -> Double.compare(potentialFieldTarget.getPotential(p1, this), - potentialFieldTarget.getPotential(p2, this)) - - ).get(); - - return nextOptimalPos.subtract(getPosition()); - } - /** * Set the last target if beyond a certain threshold. */ @@ -385,8 +354,11 @@ public class PedestrianBHM extends Pedestrian { double minDistance = Double.MAX_VALUE; VLine stepLine = new VLine(getPosition(), position); + double len = stepLine.length(); + VPoint midPoint = stepLine.midPoint(); - for (Pedestrian other : topography.getElements(Pedestrian.class)) { + for (Pedestrian other : topography.getSpatialMap(Pedestrian.class) + .getObjects(midPoint, len *0.5 + 2 * getRadius() + attributesBHM.getSpaceToKeep())) { if (other.getId() != getId()) { double distance = stepLine.distance(other.getPosition()) - @@ -422,7 +394,7 @@ public class PedestrianBHM extends Pedestrian { * This does not check collisions on the path, just collisions with position! */ public boolean collidesWithObstacle(VPoint position) { - if (detectObstacleProximity(position, this.getRadius()).size() == 0) { + if (detectObstacleProximity(position, getRadius()).isEmpty()) { return false; } else { return true; @@ -438,7 +410,7 @@ public class PedestrianBHM extends Pedestrian { List result = new LinkedList<>(); for (Obstacle obstacle : obstacles) { - if (obstacle.getShape().distance(position) < proximity) { + if (obstacle.getShape().distance(position) < proximity) { result.add(obstacle); } } @@ -456,6 +428,7 @@ public class PedestrianBHM extends Pedestrian { double distance = obstacle.getShape().distance(position); if (distance < proximity && distance < minDistance) { obs = obstacle; + minDistance = distance; } } return Optional.ofNullable(obs); @@ -510,4 +483,65 @@ public class PedestrianBHM extends Pedestrian { return 1; } } + + /* + Benedikt Zoennchen: These methods are my attempt to use the (negative) gradient of the traveling time for computing the target direction which + does not work reliable at the moment. The (negative) gradient might point inside an obstacle! + + private VPoint computeTargetDirectionByStepGradient() { + double distance = topography.getTarget(getNextTargetId()).getShape().distance(getPosition()); + if(distance > 0 && distance < getStepLength()) { + return topography.getTarget(getNextTargetId()).getShape().closestPoint(getPosition()).setMagnitude(getStepLength()); + } + + VPoint bestArg = getPosition(); + double bestVal = potentialFieldTarget.getPotential(bestArg, this); + + double h = 0.01; + VPoint nextPosition = getPosition(); + double stepLenSq = getStepLength() * getStepLength(); + + while (Math.abs(nextPosition.distanceSq(getPosition()) - stepLenSq) > h) { + VPoint gradient = potentialFieldTarget.getTargetPotentialGradient(nextPosition, this).multiply(-1.0); + nextPosition = nextPosition.add(gradient.scalarMultiply(h)); + double val = potentialFieldTarget.getPotential(nextPosition, this); + if(val < bestVal) { + bestVal = val; + } else { + break; + } + + } + + return nextPosition.subtract(getPosition()).norm(); + } + + private VPoint computeTargetDirectionByLeap() { + double distance = topography.getTarget(getNextTargetId()).getShape().distance(getPosition()); + if(distance > 0 && distance < getStepLength()) { + return topography.getTarget(getNextTargetId()).getShape().closestPoint(getPosition()).setMagnitude(getStepLength()); + } else { + VPoint gradient1 = computeAdaptedGradient(computeTargetDirectionByGradient()); + VPoint gradient2 = computeAdaptedGradient(potentialFieldTarget.getTargetPotentialGradient(getPosition().add(gradient1.setMagnitude(getStepLength())), this).multiply(-1.0)); + return gradient1.add(gradient2).norm(); + } + } + + private VPoint computeAdaptedGradient(@NotNull final VPoint gradient) { + VPoint newGradient = gradient; + + // agent may walked inside an obstacle + if(gradient.distanceSq(new VPoint(0,0)) < GeometryUtils.DOUBLE_EPS) { + Optional obstacle = detectClosestObstacleProximity(getPosition(), getRadius()); + if(obstacle.isPresent()) { + VPoint closestPoint = obstacle.get().getShape().closestPoint(getPosition()); + + VPoint direction = getPosition().subtract(closestPoint); + newGradient = direction.setMagnitude(direction.distanceToOrigin() + getRadius()); + } + } + + return newGradient; + } + */ } diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirection.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirection.java new file mode 100644 index 000000000..53b0ecc6e --- /dev/null +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirection.java @@ -0,0 +1,8 @@ +package org.vadere.simulator.models.bhm; + +import org.vadere.state.scenario.Target; +import org.vadere.util.geometry.shapes.VPoint; + +public interface TargetDirection { + VPoint getTargetDirection(final Target target); +} diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionClose.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionClose.java new file mode 100644 index 000000000..3a1ea6836 --- /dev/null +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionClose.java @@ -0,0 +1,33 @@ +package org.vadere.simulator.models.bhm; + +import org.jetbrains.annotations.NotNull; +import org.vadere.simulator.models.potential.fields.IPotentialFieldTarget; +import org.vadere.state.scenario.Target; +import org.vadere.util.geometry.shapes.VPoint; + +public class TargetDirectionClose implements TargetDirection { + + private final PedestrianBHM pedestrianBHM; + private final TargetDirection targetDirection; + private final TargetDirection closeTargetDirectionFallback; + private final IPotentialFieldTarget targetPotentialField; + + public TargetDirectionClose(@NotNull final PedestrianBHM pedestrianBHM, @NotNull final IPotentialFieldTarget targetPotentialField, @NotNull final TargetDirection targetDirection) { + this.pedestrianBHM = pedestrianBHM; + this.targetDirection = targetDirection; + this.targetPotentialField = targetPotentialField; + this.closeTargetDirectionFallback = new TargetDirectionEuclidean(pedestrianBHM); + } + + @Override + public VPoint getTargetDirection(@NotNull final Target target) { + VPoint position = pedestrianBHM.getPosition(); + VPoint direction = targetDirection.getTargetDirection(target); + VPoint nextPosition = UtilsBHM.getTargetStep(pedestrianBHM, position, direction); + if(targetPotentialField.getPotential(nextPosition, pedestrianBHM) <= 0) { + return closeTargetDirectionFallback.getTargetDirection(target); + } else { + return direction; + } + } +} diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionEuclidean.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionEuclidean.java new file mode 100644 index 000000000..e1cd522f3 --- /dev/null +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionEuclidean.java @@ -0,0 +1,23 @@ +package org.vadere.simulator.models.bhm; + +import org.jetbrains.annotations.NotNull; +import org.vadere.state.scenario.Target; +import org.vadere.util.geometry.shapes.VPoint; + +/** + * Computes the target direction for a target which is defined by exactly one shape using the Euclidean distance. + * This will NOT incorporate any obstacles between the current pedestrian position and the target shape. + * + * @author Benedikt Zoennchen + */ +public class TargetDirectionEuclidean implements TargetDirection { + private PedestrianBHM pedestrianBHM; + + public TargetDirectionEuclidean(@NotNull final PedestrianBHM pedestrianBHM) { + this.pedestrianBHM = pedestrianBHM; + } + + public VPoint getTargetDirection(final Target target) { + return target.getShape().closestPoint(pedestrianBHM.getPosition()); + } +} diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionGeoGradient.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionGeoGradient.java new file mode 100644 index 000000000..9a410641b --- /dev/null +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionGeoGradient.java @@ -0,0 +1,44 @@ +package org.vadere.simulator.models.bhm; + +import org.jetbrains.annotations.NotNull; +import org.vadere.simulator.models.potential.fields.IPotentialFieldTarget; +import org.vadere.state.scenario.Target; +import org.vadere.util.geometry.GeometryUtils; +import org.vadere.util.geometry.shapes.VPoint; +import org.vadere.util.logging.Logger; + +public class TargetDirectionGeoGradient implements TargetDirection { + private static final Logger logger = Logger.getLogger(TargetDirectionGeoGradient.class); + private PedestrianBHM pedestrianBHM; + private IPotentialFieldTarget targetPotentialField; + private TargetDirection fallBackStrategy; + private TargetDirection closeFallBackStrategy; + private double maxAngleBetweenGradients = Math.PI / 18; // difference of 180 / 18 = 10 degree + + public TargetDirectionGeoGradient( + @NotNull final PedestrianBHM pedestrianBHM, + @NotNull final IPotentialFieldTarget targetPotentialField) { + this.pedestrianBHM = pedestrianBHM; + this.targetPotentialField = targetPotentialField; + this.fallBackStrategy = new TargetDirectionGeoOptimum(pedestrianBHM, targetPotentialField); + this.closeFallBackStrategy = new TargetDirectionEuclidean(pedestrianBHM); + } + + @Override + public VPoint getTargetDirection(@NotNull final Target target) { + VPoint position = pedestrianBHM.getPosition(); + double stepLength = pedestrianBHM.getStepLength(); + + VPoint gradient1 = targetPotentialField.getTargetPotentialGradient(position, pedestrianBHM).multiply(-1.0); + if(gradient1.distanceToOrigin() > GeometryUtils.DOUBLE_EPS) { + VPoint gradient2 = targetPotentialField.getTargetPotentialGradient(position.add(gradient1.setMagnitude(stepLength)), pedestrianBHM).multiply(-1.0); + + if(gradient2.distanceToOrigin() > GeometryUtils.DOUBLE_EPS && UtilsBHM.angle(gradient1, gradient2) < maxAngleBetweenGradients) { + logger.info("gradient direction"); + return gradient1.norm(); + } + } + + return fallBackStrategy.getTargetDirection(target); + } +} diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionGeoOptimum.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionGeoOptimum.java new file mode 100644 index 000000000..4d9784636 --- /dev/null +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionGeoOptimum.java @@ -0,0 +1,77 @@ +package org.vadere.simulator.models.bhm; + +import org.jetbrains.annotations.NotNull; +import org.vadere.simulator.models.potential.fields.IPotentialFieldTarget; +import org.vadere.state.scenario.Target; +import org.vadere.util.geometry.GeometryUtils; +import org.vadere.util.geometry.shapes.VPoint; +import org.vadere.util.logging.Logger; +import org.vadere.util.math.GoldenSectionSearch; +import java.util.function.Function; + +public class TargetDirectionGeoOptimum implements TargetDirection { + private static final Logger logger = Logger.getLogger(TargetDirectionGeoOptimum.class); + private PedestrianBHM pedestrianBHM; + private IPotentialFieldTarget targetPotentialField; + private TargetDirection fallBackStrategy; + + public TargetDirectionGeoOptimum( + @NotNull final PedestrianBHM pedestrianBHM, + @NotNull final IPotentialFieldTarget targetPotentialField) { + this.pedestrianBHM = pedestrianBHM; + this.targetPotentialField = targetPotentialField; + this.fallBackStrategy = new TargetDirectionGeoOptimumBruteForce(pedestrianBHM, targetPotentialField); + } + + /** + * This method computes the target direction by a golden section search of the target potential function + * on the step circle of this agent. + * + * @return the target direction + */ + @Override + public VPoint getTargetDirection(@NotNull final Target target) { + VPoint position = pedestrianBHM.getPosition(); + VPoint lastPosition = pedestrianBHM.getLastPosition(); + double stepLength = pedestrianBHM.getStepLength(); + + logger.info("optimum direction"); + VPoint gradient = targetPotentialField.getTargetPotentialGradient(position, pedestrianBHM).multiply(-1.0); + double tol = 0.01; // + double a0 = -0.99 * Math.PI; + double b0 = 0.99 * Math.PI; + double gradLen = gradient.distanceToOrigin(); + + if(gradLen < GeometryUtils.DOUBLE_EPS && lastPosition != null) { + gradient = position.subtract(lastPosition); + } else if(gradLen < GeometryUtils.DOUBLE_EPS) { + logger.warn("no valid gradient!"); + gradient = new VPoint(1,0); + } + + VPoint bestArg = gradient.setMagnitude(stepLength); + Function f = rad -> targetPotentialField.getPotential(bestArg.rotate(rad).add(position), pedestrianBHM); + double[] ab = GoldenSectionSearch.gss(f, a0, b0, tol); + double rad = ab[1]; + double fa = f.apply(ab[0]); + double fb = f.apply(ab[1]); + double bestVal = fb; + + if(fa < f.apply(ab[1])) { + rad = ab[0]; + bestVal = fa; + } + + double currentPotential = targetPotentialField.getPotential(position, pedestrianBHM); + double firstGuess = f.apply(0.0); + // test if the first guess i.e. the point in (negative) gradient direction is even better. + if(firstGuess < bestVal && firstGuess < currentPotential) { + return bestArg.norm(); + } else if(bestVal < currentPotential) { + return bestArg.rotate(rad).norm(); + } else { + // in this case this optimization failed i.e. the agent would go backwards! => back to brute force! + return fallBackStrategy.getTargetDirection(target); + } + } +} diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionGeoOptimumBruteForce.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionGeoOptimumBruteForce.java new file mode 100644 index 000000000..62867fc2d --- /dev/null +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionGeoOptimumBruteForce.java @@ -0,0 +1,61 @@ +package org.vadere.simulator.models.bhm; + +import org.jetbrains.annotations.NotNull; +import org.vadere.simulator.models.potential.fields.IPotentialFieldTarget; +import org.vadere.state.scenario.Target; +import org.vadere.util.geometry.GeometryUtils; +import org.vadere.util.geometry.shapes.VCircle; +import org.vadere.util.geometry.shapes.VPoint; +import org.vadere.util.logging.Logger; + +import java.util.Comparator; +import java.util.List; + +public class TargetDirectionGeoOptimumBruteForce implements TargetDirection { + private static final Logger logger = Logger.getLogger(TargetDirectionGeoOptimumBruteForce.class); + private PedestrianBHM pedestrianBHM; + private IPotentialFieldTarget targetPotentialField; + private int nPoints = 100; + private TargetDirection fallback; + + public TargetDirectionGeoOptimumBruteForce( + @NotNull final PedestrianBHM pedestrianBHM, + @NotNull final IPotentialFieldTarget targetPotentialField) { + this.pedestrianBHM = pedestrianBHM; + this.targetPotentialField = targetPotentialField; + this.fallback = new TargetDirectionEuclidean(pedestrianBHM); + } + + /** + * This method computes the target direction by a brute force optimization evaluating many equidistant points + * an the step circle of the this agent which is rather slow. + * + * @return the target direction + */ + @Override + public VPoint getTargetDirection(@NotNull final Target target) { + logger.warn("expensive optimization evaluation of " + nPoints); + VPoint position = pedestrianBHM.getPosition(); + double stepLength = pedestrianBHM.getStepLength(); + + VPoint gradient = targetPotentialField.getTargetPotentialGradient(position, pedestrianBHM).multiply(-1.0); + double angle = GeometryUtils.angleTo(gradient, new VPoint(1, 0)); + List possibleNextPositions = GeometryUtils.getDiscDiscretizationPoints( + new VCircle(position, stepLength), + 1, + nPoints, + angle, + 2*Math.PI); + VPoint nextOptimalPos = possibleNextPositions.stream() + //.filter(p -> !collidesWithObstacle(p)) + .min(Comparator.comparingDouble(p -> targetPotentialField.getPotential(p, pedestrianBHM))).get(); + + // result is inside the target => take the shortest Euclidean step + if(targetPotentialField.getPotential(nextOptimalPos, pedestrianBHM) <= 0) { + return fallback.getTargetDirection(target); + } + else { + return nextOptimalPos.subtract(position); + } + } +} diff --git a/VadereSimulator/src/org/vadere/simulator/models/potential/fields/PotentialFieldTarget.java b/VadereSimulator/src/org/vadere/simulator/models/potential/fields/PotentialFieldTarget.java index 8b54e0d0f..4ccb0362a 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/potential/fields/PotentialFieldTarget.java +++ b/VadereSimulator/src/org/vadere/simulator/models/potential/fields/PotentialFieldTarget.java @@ -17,6 +17,7 @@ import org.vadere.util.geometry.shapes.VPoint; import org.vadere.util.geometry.shapes.VShape; import org.vadere.util.geometry.shapes.Vector2D; import org.vadere.util.logging.Logger; +import org.vadere.util.math.InterpolationUtil; import org.vadere.util.math.MathUtil; import java.util.HashMap; @@ -104,7 +105,7 @@ public class PotentialFieldTarget implements IPotentialFieldTarget { @Override public Vector2D getTargetPotentialGradient(VPoint pos, Agent ped) { - double potential = getPotential(pos, ped); + //double potential = getPotential(pos, ped); // according to https://en.wikipedia.org/wiki/Numerical_differentiation#Practical_considerations_using_floating_point_arithmetic double eps = Math.max(pos.x, pos.y) * MathUtil.EPSILON; //double eps = 0.001; diff --git a/VadereSimulator/src/org/vadere/simulator/models/potential/fields/PotentialFieldTargetGrid.java b/VadereSimulator/src/org/vadere/simulator/models/potential/fields/PotentialFieldTargetGrid.java index f83f025cc..a22728503 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/potential/fields/PotentialFieldTargetGrid.java +++ b/VadereSimulator/src/org/vadere/simulator/models/potential/fields/PotentialFieldTargetGrid.java @@ -3,10 +3,14 @@ package org.vadere.simulator.models.potential.fields; import org.jetbrains.annotations.NotNull; import org.vadere.state.attributes.models.AttributesFloorField; import org.vadere.state.attributes.scenario.AttributesAgent; +import org.vadere.state.scenario.Agent; import org.vadere.state.scenario.Topography; import org.vadere.util.data.cellgrid.CellGrid; import org.vadere.simulator.models.potential.solver.calculators.EikonalSolver; import org.vadere.simulator.models.potential.solver.calculators.cartesian.AGridEikonalSolver; +import org.vadere.util.geometry.shapes.VPoint; +import org.vadere.util.geometry.shapes.Vector2D; +import org.vadere.util.math.InterpolationUtil; import java.util.HashMap; import java.util.Map; @@ -37,4 +41,11 @@ public class PotentialFieldTargetGrid extends PotentialFieldTarget implements IP return map; } + + @Override + public Vector2D getTargetPotentialGradient(VPoint pos, Agent ped) { + double[] grad = new double[2]; + InterpolationUtil.getGradientMollified(getCellGrids().get(ped.getNextTargetId()), new double[]{pos.getX(), pos.getY()}, grad, 0.1); + return new Vector2D(grad[0], grad[1]); + } } diff --git a/VadereSimulator/src/org/vadere/simulator/models/potential/solver/timecost/ITimeCostFunction.java b/VadereSimulator/src/org/vadere/simulator/models/potential/solver/timecost/ITimeCostFunction.java index d5125efe3..468207f4b 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/potential/solver/timecost/ITimeCostFunction.java +++ b/VadereSimulator/src/org/vadere/simulator/models/potential/solver/timecost/ITimeCostFunction.java @@ -21,7 +21,7 @@ public interface ITimeCostFunction { /** * Prepares the dynamic timeCostFunction for the next step. */ - void update(); + default void update(){} /** * Indicates that this ITimeCostFunction is for generating a dynamic @@ -30,5 +30,5 @@ public interface ITimeCostFunction { * @return true => this ITimeCostFunction is for generating a dynaic * potential field, otherwise false */ - boolean needsUpdate(); + default boolean needsUpdate(){return false;} } diff --git a/VadereSimulator/src/org/vadere/simulator/models/potential/timeCostFunction/TimeCostFunctionFactory.java b/VadereSimulator/src/org/vadere/simulator/models/potential/timeCostFunction/TimeCostFunctionFactory.java index 17885d1af..ac66b3a9a 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/potential/timeCostFunction/TimeCostFunctionFactory.java +++ b/VadereSimulator/src/org/vadere/simulator/models/potential/timeCostFunction/TimeCostFunctionFactory.java @@ -10,6 +10,7 @@ import org.vadere.state.scenario.Topography; import org.vadere.state.types.PedestrianAttitudeType; import org.vadere.simulator.models.potential.solver.timecost.ITimeCostFunction; import org.vadere.simulator.models.potential.solver.timecost.UnitTimeCostFunction; +import org.vadere.util.math.IDistanceFunction; /** * The TimeCostFunctionFactory creates the TimeCostFunctions with the currently @@ -129,6 +130,12 @@ public class TimeCostFunctionFactory { case OBSTACLES: { return create(timeCostAttributes, topography, scale); } + case DISTANCE_TO_OBSTACLES: + return new TimeCostFunctionObstacleDistance( + new UnitTimeCostFunction(), + p -> topography.distanceToObstacle(p), + timeCostAttributes.getHeight(), + timeCostAttributes.getWidth()); default: { throw new IllegalArgumentException(timeCostAttributes.getType() + " - no such time-cost function exists!"); diff --git a/VadereSimulator/src/org/vadere/simulator/models/potential/timeCostFunction/TimeCostFunctionObstacleDistance.java b/VadereSimulator/src/org/vadere/simulator/models/potential/timeCostFunction/TimeCostFunctionObstacleDistance.java new file mode 100644 index 000000000..802487427 --- /dev/null +++ b/VadereSimulator/src/org/vadere/simulator/models/potential/timeCostFunction/TimeCostFunctionObstacleDistance.java @@ -0,0 +1,51 @@ +package org.vadere.simulator.models.potential.timeCostFunction; + +import org.jetbrains.annotations.NotNull; +import org.vadere.simulator.models.potential.solver.timecost.ITimeCostFunction; +import org.vadere.util.geometry.shapes.IPoint; + +import java.util.function.Function; + +public class TimeCostFunctionObstacleDistance implements ITimeCostFunction { + + private final transient ITimeCostFunction timeCostFunction; + private final transient Function obstacleDistanceFunction; + private final double height; + private final double width; + + public TimeCostFunctionObstacleDistance( + @NotNull final ITimeCostFunction timeCostFunction, + @NotNull final Function obstacleDistanceFunction, + final double height, + final double width) { + this.timeCostFunction = timeCostFunction; + this.obstacleDistanceFunction = obstacleDistanceFunction; + this.height = height; + this.width = width; + } + + @Override + public double costAt(IPoint p) { + double timeCost = timeCostFunction.costAt(p); + double distance = obstacleDistanceFunction.apply(p); + if(distance > 0 && distance < width) { + timeCost += (1-(distance/ width)) * height; + } + return timeCost; + } + + @Override + public void update() { + timeCostFunction.update(); + } + + @Override + public boolean needsUpdate() { + return timeCostFunction.needsUpdate(); + } + + @Override + public String toString() { + return "(obstacle distance function(x)) + " + timeCostFunction; + } +} diff --git a/VadereSimulator/src/org/vadere/simulator/models/queuing/PotentialFieldTargetQueuingGrid.java b/VadereSimulator/src/org/vadere/simulator/models/queuing/PotentialFieldTargetQueuingGrid.java index b8666fe8d..148cf1c38 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/queuing/PotentialFieldTargetQueuingGrid.java +++ b/VadereSimulator/src/org/vadere/simulator/models/queuing/PotentialFieldTargetQueuingGrid.java @@ -9,7 +9,7 @@ import org.vadere.simulator.models.potential.solver.timecost.UnitTimeCostFunctio import org.vadere.state.attributes.Attributes; import org.vadere.state.attributes.models.AttributesFloorField; import org.vadere.state.attributes.models.AttributesQueuingGame; -import org.vadere.state.attributes.models.AttributesTimeCost.TimeCostFunctionType; +import org.vadere.state.attributes.models.TimeCostFunctionType; import org.vadere.state.attributes.scenario.AttributesAgent; import org.vadere.state.scenario.Agent; import org.vadere.state.scenario.DynamicElementAddListener; diff --git a/VadereState/src/org/vadere/state/attributes/models/AttributesFloorField.java b/VadereState/src/org/vadere/state/attributes/models/AttributesFloorField.java index 2d7fa2e0f..8ccdbabbb 100644 --- a/VadereState/src/org/vadere/state/attributes/models/AttributesFloorField.java +++ b/VadereState/src/org/vadere/state/attributes/models/AttributesFloorField.java @@ -14,6 +14,11 @@ public class AttributesFloorField extends Attributes { * * TODO [refactoring]: However potentialFieldResolution is also used for the {@link org.vadere.simulator.models.potential.solver.timecost.ITimeCostFunction} * for the density computation, i.e. it is the resolution of the matrix used in the discrete convolution. This should be changed! + * Furthermore, theare are many unused parameters in {@link AttributesTimeCost}. + * Solution: + * (1) change AttributesTimeCost timeCostAttributes to ITimeCostFunction like the potential classes in AttributesOSM + * (2) split AttributesTimeCost timeCostAttributes into multiple classes + * (3) add a new AttributesTimeCost into the top level (i.e. attributesModel) json. */ private double potentialFieldResolution = 0.1; private double obstacleGridPenalty = 0.1; diff --git a/VadereState/src/org/vadere/state/attributes/models/AttributesTimeCost.java b/VadereState/src/org/vadere/state/attributes/models/AttributesTimeCost.java index 3eb90169e..2deb89d33 100644 --- a/VadereState/src/org/vadere/state/attributes/models/AttributesTimeCost.java +++ b/VadereState/src/org/vadere/state/attributes/models/AttributesTimeCost.java @@ -4,38 +4,12 @@ import org.vadere.annotation.factories.attributes.ModelAttributeClass; import org.vadere.state.attributes.Attributes; /** - * Provides attributes for a pedestrian, like body radius, height, gender... - * Currently implemented: radius [m, default: 0.195]. - * + * Provides all! parameters for all! time cost functions. + * + * TODO: split AttributesTimeCost timeCostAttributes into multiple classes see comment in {@link AttributesFloorField} */ @ModelAttributeClass public class AttributesTimeCost extends Attributes { - /** - * The different time cost function types that represents different scenario - * types. - * - */ - public enum TimeCostFunctionType { - /** a static middle scale navigation. */ - UNIT, - /** a dynamic middle scale navigation (navigation around groups). */ - NAVIGATION, - - /** a dynamic middle scale navigation (queuing). */ - QUEUEING, - - /** for the queueing game */ - QUEUEING_GAME, - - /** for the queueing game */ - NAVIGATION_GAME, - - /** - * uses TimeCostObstacleDensity to get a smooth potential field around - * obstacles - */ - OBSTACLES - } public enum LoadingType { /** use one single loading for all pedestrians. */ @@ -92,7 +66,25 @@ public class AttributesTimeCost extends Attributes { // @SerializedName("loadingType") private LoadingType loadingType = LoadingType.CONSTANT; + /** + * only used in TimeCostFunctionObstacleDistance + */ + private double width = 0.2; + + /** + * only used in TimeCostFunctionObstacleDistance + */ + private double height = 1.0; + // Getters... + public double getWidth() { + return width; + } + + public double getHeight() { + return height; + } + public double getStandardDeviation() { return standardDeviation; } diff --git a/VadereState/src/org/vadere/state/attributes/models/TimeCostFunctionType.java b/VadereState/src/org/vadere/state/attributes/models/TimeCostFunctionType.java new file mode 100644 index 000000000..2720e4cad --- /dev/null +++ b/VadereState/src/org/vadere/state/attributes/models/TimeCostFunctionType.java @@ -0,0 +1,32 @@ +package org.vadere.state.attributes.models; + +/** + * The different time cost function types that represents different scenario + * types. + * + * @author Benedikt Zoennchen + * + */ +public enum TimeCostFunctionType { + /** a static middle scale navigation. */ + UNIT, + /** a dynamic middle scale navigation (navigation around groups). */ + NAVIGATION, + + /** a dynamic middle scale navigation (queuing). */ + QUEUEING, + + /** for the queueing game */ + QUEUEING_GAME, + + /** for the queueing game */ + NAVIGATION_GAME, + + /** + * uses TimeCostObstacleDensity to get a smooth potential field around + * obstacles + */ + OBSTACLES, + + DISTANCE_TO_OBSTACLES +} \ No newline at end of file diff --git a/VadereUtils/src/org/vadere/util/geometry/GeometryUtils.java b/VadereUtils/src/org/vadere/util/geometry/GeometryUtils.java index 7019fdea0..42dbb83a6 100644 --- a/VadereUtils/src/org/vadere/util/geometry/GeometryUtils.java +++ b/VadereUtils/src/org/vadere/util/geometry/GeometryUtils.java @@ -1,6 +1,7 @@ package org.vadere.util.geometry; import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; import org.vadere.util.geometry.shapes.IPoint; import org.vadere.util.geometry.shapes.VCircle; import org.vadere.util.geometry.shapes.VLine; @@ -179,13 +180,14 @@ public class GeometryUtils { * @return a set of points which are positioned inside a disc segment */ public static List getDiscDiscretizationPoints( - @NotNull final Random random, + @Nullable final Random random, final boolean varyDirection, @NotNull final VCircle circle, final int numberOfCircles, final int numberOfPointsOfLargestCircle, final double anchorAngle, final double angle) { + assert !varyDirection || random != null; double randOffset = varyDirection ? random.nextDouble() : 0; List reachablePositions = new ArrayList<>(); @@ -218,6 +220,15 @@ public class GeometryUtils { return reachablePositions; } + public static List getDiscDiscretizationPoints( + @NotNull final VCircle circle, + final int numberOfCircles, + final int numberOfPointsOfLargestCircle, + final double anchorAngle, + final double angle) { + return getDiscDiscretizationPoints(null, false, circle, numberOfCircles, numberOfPointsOfLargestCircle, anchorAngle, angle); + } + /** * Computes the point on the line segment that is closest to the given point * point. from: @@ -452,6 +463,12 @@ public class GeometryUtils { return (ccw1 < 0 && ccw2 > 0) || (ccw1 > 0 && ccw2 < 0); } + public static boolean intersectLine(@NotNull final VLine line, @NotNull final IPoint p1, @NotNull final IPoint p2) { + double ccw1 = ccw(new VPoint(line.getP1()), new VPoint(line.getP2()), p1); + double ccw2 = ccw(new VPoint(line.getP1()), new VPoint(line.getP2()), p2); + return (ccw1 < 0 && ccw2 > 0) || (ccw1 > 0 && ccw2 < 0); + } + public static boolean intersectLine(final double pX, final double pY, final double qX, final double qY, final double p1X, final double p1Y, final double p2X, final double p2Y) { double ccw1 = ccw(pX, pY, qX, qY, p1X, p1Y); double ccw2 = ccw(pX, pY, qX, qY, p2X, p2Y); @@ -531,6 +548,10 @@ public class GeometryUtils { return intersectLine(p, q, p1, p2) && intersectLine(p1, p2, p, q); } + public static boolean intersectLineSegment(@NotNull VLine line, @NotNull final IPoint p1, @NotNull final IPoint p2) { + return intersectLine(new VPoint(line.getP1()), new VPoint(line.getP2()), p1, p2) && intersectLine(p1, p2, new VPoint(line.getP1()), new VPoint(line.getP2())); + } + /** * Tests if the first line-segment (p,q) intersects the second line-segment (p1,p2). * @@ -1000,12 +1021,13 @@ public class GeometryUtils { final double x4, final double y4) { assert new VLine(new VPoint(x1, y1), new VPoint(x2, y2)).intersectsLine(x3, y3, x4, y4); - double d = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4); + return intersectionPoint(x1, y1, x2, y2, x3, y3, x4, y4); + /*double d = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4); assert d != 0; double x = ((x1 * y2 - y1 - x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4)) / d; double y = ((x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (y3 * y4 - y3 * x4)) / d; - return new VPoint(x, y); + return new VPoint(x, y);*/ } public static VPoint lineIntersectionPoint(final VPoint p1, final VPoint p2, final VPoint q1, final VPoint q2) { @@ -1209,4 +1231,46 @@ public class GeometryUtils { return new VPolygon(path2D); } + + /** + * Computes the projection of a onto b. + * See: https://en.wikipedia.org/wiki/Vector_projection + * + * @param ax x-coordinate of a + * @param ay y-coordinate of a + * @param bx x-coordinate of b + * @param by y-coordinate of b + * @return the projection of a onto b + */ + public static IPoint projectOnto(double ax, double ay, double bx, double by) { + assert bx * bx + by * by > GeometryUtils.DOUBLE_EPS; + double blen = Math.sqrt(bx * bx + by * by); + double bxn = bx / blen; + double byn = by / blen; + + // scalar product + double alpha = ax * bxn + ay * byn; + IPoint a1 = new VPoint(bxn * alpha, byn * alpha); + return a1; + } + + /** + * Projects the point (ax, ay) onto the line defined by (p = (px, py), q = (qx, qy)). + * + * @param ax x-coordinate of a + * @param ay y-coordinate of a + * @param px x-coordinate of p + * @param py y-coordinate of p + * @param qx x-coordinate of q + * @param qy y-coordinate of q + * + * @return he projection of a onto the line (p,q) + */ + public static IPoint projectOntoLine(double ax, double ay, double px, double py, double qx, double qy) { + double bx = qx - px; + double by = qy - py; + double apx = ax - px; + double apy = ay - py; + return projectOnto(apx, apy, bx, by).add(new VPoint(px, py)); + } } diff --git a/VadereUtils/src/org/vadere/util/geometry/shapes/VPoint.java b/VadereUtils/src/org/vadere/util/geometry/shapes/VPoint.java index 8cfc2f012..f29632af4 100644 --- a/VadereUtils/src/org/vadere/util/geometry/shapes/VPoint.java +++ b/VadereUtils/src/org/vadere/util/geometry/shapes/VPoint.java @@ -128,12 +128,6 @@ public class VPoint implements Cloneable, IPoint { } public VPoint rotate(final double radAngle) { - VPoint result = new VPoint(x * Math.cos(radAngle) - y * Math.sin(radAngle), - x * Math.sin(radAngle) + y * Math.cos(radAngle)); - - if(Double.isNaN(result.getX())) { - System.out.println("wtf"); - } return new VPoint(x * Math.cos(radAngle) - y * Math.sin(radAngle), x * Math.sin(radAngle) + y * Math.cos(radAngle)); } diff --git a/VadereUtils/src/org/vadere/util/geometry/shapes/VPolygon.java b/VadereUtils/src/org/vadere/util/geometry/shapes/VPolygon.java index 52240c641..22aee7ab6 100644 --- a/VadereUtils/src/org/vadere/util/geometry/shapes/VPolygon.java +++ b/VadereUtils/src/org/vadere/util/geometry/shapes/VPolygon.java @@ -8,6 +8,7 @@ import java.util.Collections; import java.util.ArrayList; import java.util.LinkedList; import java.util.List; +import java.util.Optional; import org.vadere.util.geometry.GeometryUtils; @@ -392,11 +393,20 @@ public class VPolygon extends Path2D.Double implements VShape { @Override public double distance(IPoint target) { - if (contains(target)) { - return -closestPoint(target).distance(target); - } else { - return closestPoint(target).distance(target); + try { + if (contains(target)) { + return -closestPoint(target).distance(target); + } else { + return closestPoint(target).distance(target); + } + } catch (NullPointerException ex) { + if (contains(target)) { + return -closestPoint(target).distance(target); + } else { + return closestPoint(target).distance(target); + } } + } @Override @@ -449,6 +459,60 @@ public class VPolygon extends Path2D.Double implements VShape { return resultPoint; } + @Override + public Optional getClosestIntersectionPoint(VPoint q1, VPoint q2, VPoint r) { + double currentMinDistance = java.lang.Double.MAX_VALUE; + VPoint resultPoint = null; + + PathIterator iterator = this.getPathIterator(null); + + double[] first = null; + double[] last = new double[2]; + double[] next = new double[2]; + VPoint currentIntersectionPoint; + + iterator.currentSegment(next); + iterator.next(); + + while (!iterator.isDone()) { + last[0] = next[0]; + last[1] = next[1]; + + if(first == null) { + first = new double[]{last[0], last[1]}; + } + + iterator.currentSegment(next); + VLine line = new VLine(last[0], last[1], next[0], next[1]); + if(GeometryUtils.intersectLine(line, q1, q2)) { + currentIntersectionPoint = GeometryUtils.lineIntersectionPoint(new VLine(last[0], + last[1], next[0], next[1]), q1.getX(), q1.getY(), q2.getX(), q2.getY()); + + if (currentIntersectionPoint.distance(r) < currentMinDistance) { + currentMinDistance = currentIntersectionPoint.distance(r); + resultPoint = currentIntersectionPoint; + } + } + iterator.next(); + } + + // dont forget the last and first point! + if(first != null) { + VLine line = new VLine(last[0], last[1], next[0], next[1]); + if(GeometryUtils.intersectLine(line, q1, q2)) { + currentIntersectionPoint = GeometryUtils.lineIntersectionPoint(new VLine(last[0], + last[1], next[0], next[1]), q1.getX(), q1.getY(), q2.getX(), q2.getY()); + + if (currentIntersectionPoint.distance(r) < currentMinDistance) { + currentMinDistance = currentIntersectionPoint.distance(r); + resultPoint = currentIntersectionPoint; + } + } + } + + return Optional.ofNullable(resultPoint); + } + @Override public boolean equals(Object obj) { if (this == obj) { diff --git a/VadereUtils/src/org/vadere/util/geometry/shapes/VRectangle.java b/VadereUtils/src/org/vadere/util/geometry/shapes/VRectangle.java index 71c4020ec..f6eeb0325 100644 --- a/VadereUtils/src/org/vadere/util/geometry/shapes/VRectangle.java +++ b/VadereUtils/src/org/vadere/util/geometry/shapes/VRectangle.java @@ -3,6 +3,7 @@ package org.vadere.util.geometry.shapes; import java.awt.geom.Rectangle2D; import java.util.Arrays; import java.util.List; +import java.util.Optional; import org.vadere.util.geometry.GeometryUtils; @@ -67,6 +68,23 @@ public class VRectangle extends Rectangle2D.Double implements VShape { return result; } + @Override + public Optional getClosestIntersectionPoint(VPoint q1, VPoint q2, VPoint r) { + double minDinstance = java.lang.Double.MAX_VALUE; + VPoint intersectionPoint = null; + for(VLine line : getLines()) { + if(GeometryUtils.intersectLineSegment(line, q1, q2)) { + VPoint tmpIntersectionPoint = GeometryUtils.lineIntersectionPoint(line, q1.getX(), q1.getY(), q2.getX(), q2.getY()); + double distance = tmpIntersectionPoint.distance(r); + if(distance < minDinstance) { + minDinstance = distance; + intersectionPoint = tmpIntersectionPoint; + } + } + } + return Optional.ofNullable(intersectionPoint); + } + public VLine[] getLines() { VLine[] result = new VLine[4]; diff --git a/VadereUtils/src/org/vadere/util/geometry/shapes/VRing.java b/VadereUtils/src/org/vadere/util/geometry/shapes/VRing.java index cfa0d9627..aec90c6b0 100644 --- a/VadereUtils/src/org/vadere/util/geometry/shapes/VRing.java +++ b/VadereUtils/src/org/vadere/util/geometry/shapes/VRing.java @@ -7,6 +7,7 @@ import java.awt.geom.Point2D; import java.awt.geom.Rectangle2D; import java.util.ArrayList; import java.util.List; +import java.util.Optional; import org.vadere.util.geometry.shapes.ShapeType; @@ -158,6 +159,24 @@ public class VRing implements VShape { throw new UnsupportedOperationException("method is not implemented jet."); } + @Override + public Optional getClosestIntersectionPoint(VPoint q1, VPoint q2, VPoint r) { + VCircle circle1 = new VCircle(center, radiusInnerCircle); + VCircle circle2 = new VCircle(center, radiusOuterCircle); + Optional optionalVPoint1 = circle1.getClosestIntersectionPoint(q1, q2, r); + Optional optionalVPoint2 = circle2.getClosestIntersectionPoint(q1, q2, r); + + if(!optionalVPoint1.isPresent()) { + return optionalVPoint2; + } else if(!optionalVPoint2.isPresent()) { + return optionalVPoint1; + } else if(optionalVPoint1.get().distance(r) < optionalVPoint2.get().distance(r)) { + return optionalVPoint1; + } else { + return optionalVPoint2; + } + } + @Override public boolean contains(IPoint point) { double distanceFromCenterToPoint = center.distance(point); diff --git a/VadereUtils/src/org/vadere/util/geometry/shapes/VShape.java b/VadereUtils/src/org/vadere/util/geometry/shapes/VShape.java index af8d439fd..16862665b 100644 --- a/VadereUtils/src/org/vadere/util/geometry/shapes/VShape.java +++ b/VadereUtils/src/org/vadere/util/geometry/shapes/VShape.java @@ -6,6 +6,7 @@ import java.awt.geom.Path2D; import java.awt.geom.Rectangle2D; import java.util.ArrayList; import java.util.List; +import java.util.Optional; import org.vadere.util.geometry.shapes.ShapeType; @@ -17,6 +18,8 @@ public interface VShape extends Shape, Cloneable { VPoint closestPoint(IPoint point); + Optional getClosestIntersectionPoint(VPoint q1, VPoint q2, VPoint r); + boolean contains(IPoint point); VShape translate(final IPoint vector); diff --git a/VadereUtils/src/org/vadere/util/math/GoldenSectionSearch.java b/VadereUtils/src/org/vadere/util/math/GoldenSectionSearch.java new file mode 100644 index 000000000..346f89374 --- /dev/null +++ b/VadereUtils/src/org/vadere/util/math/GoldenSectionSearch.java @@ -0,0 +1,57 @@ +package org.vadere.util.math; + +import org.jetbrains.annotations.NotNull; + +import java.util.function.Function; + +/** + * Recursive implementation of the Golden section search algorithm. + * + * @author Benedikt Zoennchen + */ +public class GoldenSectionSearch { + private static final double invphi = (Math.sqrt(5.0)-1)/2.0; + private static final double invphi2 = (3-Math.sqrt(5.0))/2.0; + + /** + * Computes a subinterval [a1;b1] of [a;b] such that the minimum of f is contained in [a1;b1]. + * The tolerance tol controls the accuracy, that is the size of the subinterval. + * This method uses recursion. + * + * @param f the function to be minimized + * @param a defines the interval. Assumption: a < b + * @param b defines the interval. Assumption: b > a + * @param tol controls the accuracy, i.e. b1-a1 <= tol + * + * @return a subinterval [a1;b1] containing the minimum of f + */ + public static double[] gss(@NotNull final Function f, final double a, final double b, final double tol) { + return gss(f, a, b, tol,b - a,true,0,0,true,0,0); + } + + private static double[] gss(@NotNull Function f, + final double a, final double b, final double tol, + double h, boolean noC, double c, double fc, + boolean noD, double d, double fd) { + if (Math.abs(h) <= tol) { + return new double[] { a, b }; + } + + if (noC) { + c = a + invphi2 * h; + fc = f.apply(c); + } + + if (noD) { + d = a + invphi * h; + fd = f.apply(d); + } + + if (fc < fd) { + return gss(f, a, d, tol,h * invphi,true,0,0,false, c, fc); + } else { + return gss(f, c, b, tol,h * invphi,false, d, fd,true,0,0); + } + } +} + -- 2.26.2