diff --git a/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_displaced_ok.scenario b/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_displaced_ok.scenario new file mode 100644 index 0000000000000000000000000000000000000000..2a3cb56ca74fc2d66c0b76748130060ea4f76347 --- /dev/null +++ b/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_displaced_ok.scenario @@ -0,0 +1,194 @@ +{ + "name" : "floor_field_navigation_test_displaced_ok", + "description" : "", + "release" : "0.9", + "processWriters" : { + "files" : [ { + "type" : "org.vadere.simulator.projects.dataprocessing.outputfile.TimestepPedestrianIdOutputFile", + "filename" : "postvis.trajectories", + "processors" : [ 1 ] + } ], + "processors" : [ { + "type" : "org.vadere.simulator.projects.dataprocessing.processor.PedestrianPositionProcessor", + "id" : 1 + } ], + "isTimestamped" : true, + "isWriteMetaData" : false + }, + "scenario" : { + "mainModel" : "org.vadere.simulator.models.bhm.BehaviouralHeuristicsModel", + "attributesModel" : { + "org.vadere.state.attributes.models.AttributesBHM" : { + "stepLengthIntercept" : 0.4625, + "stepLengthSlopeSpeed" : 0.2345, + "stepLengthSD" : 0.036, + "stepLengthDeviation" : true, + "navigationCluster" : false, + "navigationFollower" : false, + "directionWallDistance" : false, + "tangentialEvasion" : true, + "sidewaysEvasion" : false, + "onlyEvadeContraFlow" : false, + "makeSmallSteps" : false, + "followerProximityNavigation" : false, + "differentBehaviour" : false, + "differentEvasionBehaviourPercentage" : [ ], + "varyingBehaviour" : false, + "adaptiveBehaviourDensity" : false, + "adaptiveBehaviourStepsRemained" : [ ], + "switchBehaviour" : false, + "evasionDetourThreshold" : 0.1, + "onlyEvadeContraFlowAngle" : 2.0943951023931953, + "followerAngleMovement" : 1.5707963267948966, + "followerAnglePosition" : 1.5707963267948966, + "followerDistance" : 10.0, + "smallStepResolution" : 5, + "plannedStepsAhead" : 5, + "obstacleRepulsionReach" : 0.5, + "obstacleRepulsionMaxWeight" : 6.0, + "distanceToKeep" : 0.5, + "backwardsAngle" : 1.5707963267948966, + "reconsiderOldTargets" : false, + "targetThresholdX" : 1.7976931348623157E308, + "targetThresholdY" : 1.7976931348623157E308, + "spaceToKeep" : 0.01, + "stepAwayFromCollisions" : false + }, + "org.vadere.state.attributes.models.AttributesFloorField" : { + "createMethod" : "HIGH_ACCURACY_FAST_MARCHING", + "potentialFieldResolution" : 0.1, + "obstacleGridPenalty" : 0.1, + "targetAttractionStrength" : 1.0, + "timeCostAttributes" : { + "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", + "width" : 0.4, + "height" : 5.0 + } + } + }, + "attributesSimulation" : { + "finishTime" : 500.0, + "simTimeStepLength" : 0.4, + "realTimeSimTimeRatio" : 0.0, + "writeSimulationData" : false, + "visualizationEnabled" : true, + "printFPS" : false, + "digitsPerCoordinate" : 2, + "useFixedSeed" : true, + "fixedSeed" : 1, + "simulationSeed" : 1 + }, + "topography" : { + "attributes" : { + "bounds" : { + "x" : 66.0, + "y" : 56.0, + "width" : 35.0, + "height" : 60.0 + }, + "boundingBoxWidth" : 0.5, + "bounded" : true + }, + "obstacles" : [ { + "shape" : { + "x" : 63.5, + "y" : 99.5, + "width" : 23.9, + "height" : 3.6, + "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 + } ], + "measurementAreas" : [ { + "shape" : { + "x" : 10.0, + "y" : 32.0, + "width" : 15.0, + "height" : 8.0, + "type" : "RECTANGLE" + }, + "id" : 1 + } ], + "stairs" : [ ], + "targets" : [ { + "id" : 1, + "absorbing" : true, + "shape" : { + "x" : 67.0, + "y" : 107.0, + "width" : 15.0, + "height" : 5.0, + "type" : "RECTANGLE" + }, + "waitingTime" : 0.0, + "waitingTimeYellowPhase" : 0.0, + "parallelWaiters" : 0, + "individualWaiting" : true, + "deletionDistance" : 0.1, + "startingWithRedLight" : false, + "nextSpeed" : -1.0 + } ], + "absorbingAreas" : [ ], + "sources" : [ { + "id" : -1, + "shape" : { + "x" : 76.0, + "y" : 61.0, + "width" : 15.0, + "height" : 9.0, + "type" : "RECTANGLE" + }, + "interSpawnTimeDistribution" : "org.vadere.state.scenario.ConstantDistribution", + "distributionParameters" : [ 1.0 ], + "spawnNumber" : 500, + "maxSpawnNumberTotal" : -1, + "startTime" : 0.0, + "endTime" : 0.0, + "spawnAtRandomPositions" : true, + "useFreeSpaceOnly" : true, + "targetIds" : [ 1 ], + "groupSizeDistribution" : [ 1.0 ], + "dynamicElementType" : "PEDESTRIAN" + } ], + "dynamicElements" : [ ], + "attributesPedestrian" : { + "radius" : 0.195, + "densityDependentSpeed" : false, + "speedDistributionMean" : 1.34, + "speedDistributionStandardDeviation" : 0.26, + "minimumSpeed" : 0.5, + "maximumSpeed" : 2.2, + "acceleration" : 2.0 + }, + "teleporter" : null, + "attributesCar" : null + }, + "eventInfos" : [ ] + } +} \ No newline at end of file diff --git a/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_ok.scenario b/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_ok.scenario new file mode 100644 index 0000000000000000000000000000000000000000..8cdeeb097240fee563119e7d8e133134404d4834 --- /dev/null +++ b/VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_ok.scenario @@ -0,0 +1,203 @@ +{ + "name" : "floor_field_navigation_test_ok", + "description" : "", + "release" : "0.9", + "processWriters" : { + "files" : [ { + "type" : "org.vadere.simulator.projects.dataprocessing.outputfile.TimestepPedestrianIdOutputFile", + "filename" : "postvis.trajectories", + "processors" : [ 1 ] + } ], + "processors" : [ { + "type" : "org.vadere.simulator.projects.dataprocessing.processor.PedestrianPositionProcessor", + "id" : 1 + } ], + "isTimestamped" : true, + "isWriteMetaData" : false + }, + "scenario" : { + "mainModel" : "org.vadere.simulator.models.bhm.BehaviouralHeuristicsModel", + "attributesModel" : { + "org.vadere.state.attributes.models.AttributesBHM" : { + "stepLengthIntercept" : 0.4625, + "stepLengthSlopeSpeed" : 0.2345, + "stepLengthSD" : 0.036, + "stepLengthDeviation" : true, + "navigationCluster" : false, + "navigationFollower" : false, + "directionWallDistance" : false, + "tangentialEvasion" : false, + "sidewaysEvasion" : false, + "onlyEvadeContraFlow" : false, + "makeSmallSteps" : false, + "followerProximityNavigation" : false, + "differentBehaviour" : false, + "differentEvasionBehaviourPercentage" : [ ], + "varyingBehaviour" : false, + "adaptiveBehaviourDensity" : false, + "adaptiveBehaviourStepsRemained" : [ ], + "switchBehaviour" : false, + "evasionDetourThreshold" : 0.1, + "onlyEvadeContraFlowAngle" : 2.0943951023931953, + "followerAngleMovement" : 1.5707963267948966, + "followerAnglePosition" : 1.5707963267948966, + "followerDistance" : 10.0, + "smallStepResolution" : 5, + "plannedStepsAhead" : 5, + "obstacleRepulsionReach" : 0.5, + "obstacleRepulsionMaxWeight" : 6.0, + "distanceToKeep" : 0.5, + "backwardsAngle" : 1.5707963267948966, + "reconsiderOldTargets" : false, + "targetThresholdX" : 1.7976931348623157E308, + "targetThresholdY" : 1.7976931348623157E308, + "spaceToKeep" : 0.01, + "stepAwayFromCollisions" : false + }, + "org.vadere.state.attributes.models.AttributesFloorField" : { + "createMethod" : "HIGH_ACCURACY_FAST_MARCHING", + "potentialFieldResolution" : 0.1, + "obstacleGridPenalty" : 0.1, + "targetAttractionStrength" : 1.0, + "timeCostAttributes" : { + "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", + "width" : 0.4, + "height" : 5.0 + } + } + }, + "attributesSimulation" : { + "finishTime" : 500.0, + "simTimeStepLength" : 0.4, + "realTimeSimTimeRatio" : 0.0, + "writeSimulationData" : true, + "visualizationEnabled" : true, + "printFPS" : false, + "digitsPerCoordinate" : 2, + "useFixedSeed" : true, + "fixedSeed" : 1, + "simulationSeed" : 1 + }, + "topography" : { + "attributes" : { + "bounds" : { + "x" : 0.0, + "y" : 0.0, + "width" : 35.0, + "height" : 60.0 + }, + "boundingBoxWidth" : 0.5, + "bounded" : true + }, + "obstacles" : [ { + "shape" : { + "x" : -2.6, + "y" : 44.3, + "width" : 23.9, + "height" : 3.6, + "type" : "RECTANGLE" + }, + "id" : -1 + }, { + "shape" : { + "x" : 2.4, + "y" : 39.9, + "width" : 32.2, + "height" : 2.4, + "type" : "RECTANGLE" + }, + "id" : -1 + }, { + "shape" : { + "x" : 0.3, + "y" : 36.5, + "width" : 29.9, + "height" : 1.9, + "type" : "RECTANGLE" + }, + "id" : -1 + }, { + "shape" : { + "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 + } ], + "measurementAreas" : [ ], + "stairs" : [ ], + "targets" : [ { + "id" : 1, + "absorbing" : true, + "shape" : { + "x" : 10.0, + "y" : 51.0, + "width" : 15.0, + "height" : 5.0, + "type" : "RECTANGLE" + }, + "waitingTime" : 0.0, + "waitingTimeYellowPhase" : 0.0, + "parallelWaiters" : 0, + "individualWaiting" : true, + "deletionDistance" : 0.1, + "startingWithRedLight" : false, + "nextSpeed" : -1.0 + } ], + "absorbingAreas" : [ ], + "sources" : [ { + "id" : -1, + "shape" : { + "x" : 10.0, + "y" : 5.0, + "width" : 15.0, + "height" : 9.0, + "type" : "RECTANGLE" + }, + "interSpawnTimeDistribution" : "org.vadere.state.scenario.ConstantDistribution", + "distributionParameters" : [ 1.0 ], + "spawnNumber" : 100, + "maxSpawnNumberTotal" : -1, + "startTime" : 0.0, + "endTime" : 0.0, + "spawnAtRandomPositions" : true, + "useFreeSpaceOnly" : true, + "targetIds" : [ 1 ], + "groupSizeDistribution" : [ 1.0 ], + "dynamicElementType" : "PEDESTRIAN" + } ], + "dynamicElements" : [ ], + "attributesPedestrian" : { + "radius" : 0.195, + "densityDependentSpeed" : false, + "speedDistributionMean" : 1.34, + "speedDistributionStandardDeviation" : 0.26, + "minimumSpeed" : 0.5, + "maximumSpeed" : 2.2, + "acceleration" : 2.0 + }, + "teleporter" : null, + "attributesCar" : null + }, + "eventInfos" : [ ] + } +} \ No newline at end of file diff --git a/VadereModelTests/TestBHM/vadere.project b/VadereModelTests/TestBHM/vadere.project new file mode 100644 index 0000000000000000000000000000000000000000..0c7e880c5f7a680a4341a49ec9a70178e48ab664 --- /dev/null +++ b/VadereModelTests/TestBHM/vadere.project @@ -0,0 +1 @@ +TEST OSM \ No newline at end of file diff --git a/VadereSimulator/src/org/vadere/simulator/control/Simulation.java b/VadereSimulator/src/org/vadere/simulator/control/Simulation.java index acb407546e77f97b2cb34d2c35d0af5e7989b5a0..826500e7f341001f864a154884e98e6b7e1753c0 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 a327249736e5ffe87a69c87df69f23d3a63ef25f..5252b846cc6234c13e5af3749cf54e91c8b6b3f8 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/bhm/BehaviouralHeuristicsModel.java +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/BehaviouralHeuristicsModel.java @@ -10,8 +10,14 @@ import org.jetbrains.annotations.NotNull; import org.vadere.annotation.factories.models.ModelClass; import org.vadere.simulator.models.MainModel; import org.vadere.simulator.models.Model; +import org.vadere.simulator.models.osm.PedestrianOSM; +import org.vadere.simulator.models.potential.fields.IPotentialFieldTarget; +import org.vadere.simulator.models.potential.fields.IPotentialFieldTargetGrid; +import org.vadere.simulator.models.potential.fields.PotentialFieldTargetGrid; import org.vadere.state.attributes.Attributes; +import org.vadere.state.attributes.exceptions.AttributesNotFoundException; import org.vadere.state.attributes.models.AttributesBHM; +import org.vadere.state.attributes.models.AttributesFloorField; import org.vadere.state.attributes.scenario.AttributesAgent; import org.vadere.state.scenario.DynamicElement; import org.vadere.state.scenario.Pedestrian; @@ -24,6 +30,8 @@ import org.vadere.util.geometry.shapes.VShape; @ModelClass(isMainModel = true) public class BehaviouralHeuristicsModel implements MainModel { + private IPotentialFieldTarget potentialFieldTarget; + /** * Compares the time of the next possible move. */ @@ -52,9 +60,22 @@ 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) { + + try { + potentialFieldTarget = IPotentialFieldTargetGrid.createPotentialField( + modelAttributesList, topography, attributesPedestrian, PotentialFieldTargetGrid.class.getCanonicalName()); + this.models.add(potentialFieldTarget); + } catch (AttributesNotFoundException e) { + potentialFieldTarget = null; + } + this.attributesBHM = Model.findAttributes(modelAttributesList, AttributesBHM.class); this.attributesPedestrian = attributesPedestrian; this.topography = topography; @@ -77,7 +98,7 @@ public class BehaviouralHeuristicsModel implements MainModel { } private PedestrianBHM createElement(VPoint position, @NotNull final AttributesAgent pedAttributes) { - PedestrianBHM pedestrian = new PedestrianBHM(topography, pedAttributes, attributesBHM, random); + PedestrianBHM pedestrian = new PedestrianBHM(topography, pedAttributes, attributesBHM, random, potentialFieldTarget); pedestrian.setPosition(position); return pedestrian; } diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddend.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddend.java index 35162cda49a49a20f45b2e18f025f1d4f3829695..3bea6deb887c632719f31b4f312369e67fb725fe 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddend.java +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddend.java @@ -1,7 +1,8 @@ package org.vadere.simulator.models.bhm; +import org.jetbrains.annotations.NotNull; import org.vadere.util.geometry.shapes.VPoint; public interface DirectionAddend { - public VPoint getDirectionAddend(); + public VPoint getDirectionAddend(@NotNull final VPoint targetDirection); } diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacle.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacle.java index b41624de37b7a4e96707ea84b6484b36b8176148..a93850c62835bfef163c03bd6e957db1c85b1bef 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacle.java +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacle.java @@ -1,5 +1,6 @@ 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.shapes.VPoint; @@ -20,7 +21,7 @@ public class DirectionAddendObstacle implements DirectionAddend { } @Override - public VPoint getDirectionAddend() { + public VPoint getDirectionAddend(@NotNull final VPoint targetDirection) { return getTargetObstacleDirection(); } diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacleTargetPotential.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacleTargetPotential.java new file mode 100644 index 0000000000000000000000000000000000000000..57e64b11d8682dce8a1a615905fa2cf521f3d3a1 --- /dev/null +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacleTargetPotential.java @@ -0,0 +1,105 @@ +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; + private final PedestrianBHM me; + + public DirectionAddendObstacleTargetPotential(PedestrianBHM me) { + this.me = me; + this.attributesBHM = me.getAttributesBHM(); + } + + @Override + public VPoint getDirectionAddend(@NotNull final VPoint targetDirection) { + VPoint addend = VPoint.ZERO; + + 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 75bae708adc9cf505d561cc0c8c94312a41f3c95..1f24485b39e0b9e52c5e564a713ee2ca1664061d 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,22 @@ 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()); + }*/ result = me.getPosition(); me.action = 0; // LOGGING diff --git a/VadereSimulator/src/org/vadere/simulator/models/bhm/PedestrianBHM.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/PedestrianBHM.java index afcd15666314431301e268f0fd0d7ede0554f97f..7c8a9f464b279479e643485cd44002a7fc9f3935 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/bhm/PedestrianBHM.java +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/PedestrianBHM.java @@ -1,5 +1,14 @@ package org.vadere.simulator.models.bhm; +import java.util.Collection; +import java.util.LinkedList; +import java.util.List; +import java.util.Optional; +import java.util.Random; + +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; +import org.vadere.simulator.models.potential.fields.IPotentialFieldTarget; import org.vadere.state.attributes.models.AttributesBHM; import org.vadere.state.attributes.scenario.AttributesAgent; import org.vadere.state.events.exceptions.UnsupportedEventException; @@ -12,24 +21,19 @@ import org.vadere.state.scenario.Pedestrian; 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.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; -import java.util.Collection; -import java.util.LinkedList; -import java.util.List; -import java.util.Random; - public class PedestrianBHM extends Pedestrian { private static Logger logger = Logger.getLogger(PedestrianBHM.class); - private final Random random; + private final transient Random random; private final AttributesBHM attributesBHM; - private final Topography topography; + private final transient Topography topography; private final double stepLength; @@ -39,19 +43,26 @@ public class PedestrianBHM extends Pedestrian { private VPoint lastPosition; private VPoint targetDirection; - private final Navigation navigation; - private final List directionAddends; + private final transient Navigation navigation; + private final transient List directionAddends; protected int action; private boolean evadesTangentially; 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) { - super(attributesPedestrian, random); + AttributesBHM attributesBHM, Random random) { + this(topography, attributesPedestrian, attributesBHM, random, null); + } + public PedestrianBHM(Topography topography, AttributesAgent attributesPedestrian, + AttributesBHM attributesBHM, Random random, @Nullable IPotentialFieldTarget potentialFieldTarget) { + super(attributesPedestrian, random); + this.potentialFieldTarget = potentialFieldTarget; this.random = random; this.attributesBHM = attributesBHM; this.topography = topography; @@ -93,8 +104,21 @@ 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() { + return potentialFieldTarget != null; } private void setEvasionStrategy() { @@ -164,7 +188,7 @@ public class PedestrianBHM extends Pedestrian { } else { throw new UnsupportedEventException(mostImportantEvent, this.getClass()); } - + getFootSteps().add(new FootStep(position, getPosition(), timeOfNextStep, timeOfNextStep + durationNextStep)); } @@ -222,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()); } /** @@ -240,16 +264,20 @@ public class PedestrianBHM extends Pedestrian { } if (hasNextTarget()) { - VShape targetShape = topography.getTarget(getNextTargetId()).getShape(); - if (!targetShape.contains(getPosition())) { - VPoint targetPoint = targetShape.closestPoint(getPosition()); - targetDirection = targetPoint.subtract(getPosition()).norm(); + Target target = topography.getTarget(getNextTargetId()); + if (!target.getShape().contains(getPosition())) { + + targetDirection = targetDirectionStrategy.getTargetDirection(target); for (DirectionAddend da : directionAddends) { - targetDirection = targetDirection.add(da.getDirectionAddend()); + targetDirection = targetDirection.add(da.getDirectionAddend(targetDirection)); } - if (!targetDirection.equals(VPoint.ZERO)) { + //TODO: if this happens it might cause problems dependent on the heuristics choose. + if(targetDirection.distanceToOrigin() < GeometryUtils.DOUBLE_EPS) { + targetDirection = VPoint.ZERO; + } + else { targetDirection = targetDirection.norm(); } } @@ -326,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()) - @@ -363,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; @@ -373,13 +404,13 @@ public class PedestrianBHM extends Pedestrian { /** * This does not check collisions on the path, just collisions with position! */ - List detectObstacleProximity(VPoint position, double proximity) { + List detectObstacleProximity(@NotNull VPoint position, double proximity) { Collection obstacles = topography.getObstacles(); List result = new LinkedList<>(); for (Obstacle obstacle : obstacles) { - if (obstacle.getShape().distance(position) < proximity) { + if (obstacle.getShape().distance(position) < proximity) { result.add(obstacle); } } @@ -387,6 +418,23 @@ public class PedestrianBHM extends Pedestrian { return result; } + Optional detectClosestObstacleProximity(@NotNull final VPoint position, double proximity) { + + Collection obstacles = topography.getObstacles(); + Obstacle obs = null; + double minDistance = Double.MAX_VALUE; + + for (Obstacle obstacle : obstacles) { + double distance = obstacle.getShape().distance(position); + if (distance < proximity && distance < minDistance) { + obs = obstacle; + minDistance = distance; + } + } + return Optional.ofNullable(obs); + } + + // Java nuisance... @@ -435,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 0000000000000000000000000000000000000000..53b0ecc6e5319d008b5a4c8bb83ec8e6665c5104 --- /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 0000000000000000000000000000000000000000..3a1ea6836c310a8248bb2f3822767dd8195d7415 --- /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 0000000000000000000000000000000000000000..88e9a9977faf28627f4373ae454843d6e0371a41 --- /dev/null +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/TargetDirectionEuclidean.java @@ -0,0 +1,24 @@ +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) { + VPoint position = pedestrianBHM.getPosition(); + return target.getShape().closestPoint(pedestrianBHM.getPosition()).subtract(position).norm(); + } +} 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 0000000000000000000000000000000000000000..9a410641b2abeba2ba28ba40d89ace727ec14998 --- /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 0000000000000000000000000000000000000000..4d97846362242523d7c7f0366f894fac058f98f6 --- /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 0000000000000000000000000000000000000000..62867fc2d007ef8de053e8cf7288f080c466fc6c --- /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/bhm/UtilsBHM.java b/VadereSimulator/src/org/vadere/simulator/models/bhm/UtilsBHM.java index e6494bf18d206d4263052d2bdc3ad3c1a7208612..5fc6d21b5ca245debddf5f0118f08e18260694f0 100644 --- a/VadereSimulator/src/org/vadere/simulator/models/bhm/UtilsBHM.java +++ b/VadereSimulator/src/org/vadere/simulator/models/bhm/UtilsBHM.java @@ -1,14 +1,20 @@ package org.vadere.simulator.models.bhm; +import org.jetbrains.annotations.NotNull; import org.vadere.state.scenario.Pedestrian; +import org.vadere.util.geometry.GrahamScan; import org.vadere.util.geometry.shapes.VLine; import org.vadere.util.geometry.shapes.VPoint; +import org.vadere.util.geometry.shapes.VPolygon; import org.vadere.util.logging.Logger; import java.util.ArrayList; +import java.util.HashSet; import java.util.LinkedList; import java.util.List; import java.util.Random; +import java.util.Set; +import java.util.stream.Collectors; public class UtilsBHM { @@ -153,39 +159,32 @@ public class UtilsBHM { } /** - * QuickHull algorithm to determine convex hull of pedestrian cluster. - * + * Filters the list of pedestrians such that only pedestrians will remain which form + * the convex hull of all the pedestrians in the list (with repect to their position). + * + * @param cluster list of pedestrians + * + * @return pedestrians forming the convex hull */ - private static List convexHull(final List cluster) { - List result; - - // any three points form a convex hull - if (cluster.size() < 4) { - result = cluster; - } else { - result = new LinkedList<>(); - - Pedestrian xmin = cluster.get(0); - Pedestrian xmax = cluster.get(0); - - for (int i = 2; i < result.size(); i++) { - Pedestrian next = cluster.get(i); - if (next.getPosition().x < xmin.getPosition().x) { - xmin = next; - } else if (next.getPosition().x > xmin.getPosition().x) { - xmax = next; - } - } - - VLine initialLine = new VLine(xmin.getPosition(), xmax.getPosition()); - - // TODO [priority=medium] [task=feature] complete algorithm + private static List convexHull(@NotNull final List cluster) { + if(cluster.size() > 2) { + return cluster; } - throw new UnsupportedOperationException("this method is not fully implemented jet."); - // return result; + + GrahamScan grahamScan = new GrahamScan(cluster.stream().map(ped -> ped.getPosition()).collect(Collectors.toList())); + Set convexHull = new HashSet<>(grahamScan.getConvexHull()); + return cluster.stream().filter(ped -> convexHull.contains(ped.getPosition())).collect(Collectors.toList()); } - public static int randomChoice(List fractions, Random random) { + /** + * Given the fractions f1, f2, ..., fn this method returns 1 or 2 ... or n with probability + * f1 / sum, f2 / sum, ..., fn / sum for sum = f1 + f2 + ... + fn. + * + * @param fractions + * @param random + * @return a randomly chosen index in [0, fraction.size()) with probabilities depending on the fractions + */ + public static int randomChoice(@NotNull final List fractions, @NotNull final Random random) { double[] probabilities = new double[fractions.size()]; double sum = 0; 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 4b47bf85f5df805ba8bbd7d653484453245fd887..4ccb0362ac8feb3087980895b7f76e97501e3649 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,8 @@ 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; import java.util.List; @@ -103,7 +105,13 @@ public class PotentialFieldTarget implements IPotentialFieldTarget { @Override public Vector2D getTargetPotentialGradient(VPoint pos, Agent ped) { - throw new UnsupportedOperationException("gradient not yet implemented"); + //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; + double dGradPX = (getPotential(pos.add(new VPoint(eps, 0)), ped) - getPotential(pos.subtract(new VPoint(eps, 0)), ped)) / (2*eps); + double dGradPY = (getPotential(pos.add(new VPoint(0, eps)), ped) - getPotential(pos.subtract(new VPoint(0, eps)), ped)) / (2*eps); + return new Vector2D(dGradPX, dGradPY); } /** 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 f83f025cce8e6b6e9e203552a819028806c94e3e..a227285032b90cc1ad915eb8ddf94199c64625e8 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 d5125efe36546419fbf0cf6e49b4147d363b58d3..468207f4b11b41a405fe301f836a50b2e3456af8 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 17885d1afc2e7815fe60183433e26998517f01aa..ac66b3a9a8c69d038acd246e4b959691298a1ac0 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 0000000000000000000000000000000000000000..80248742792bb88fdb2c502a561f0ebbd1a1d14a --- /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 b8666fe8dff4b9926f9fd2377fd31fa1a56a35a4..148cf1c38968b97ffa60cda3726fe69ea9b6bb46 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 f7681e87d334b4c06d57c3e47fee1c027aeebda4..8ccdbabbba54a031026340cc00578ca6ff9d6b42 100644 --- a/VadereState/src/org/vadere/state/attributes/models/AttributesFloorField.java +++ b/VadereState/src/org/vadere/state/attributes/models/AttributesFloorField.java @@ -14,10 +14,16 @@ 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; private double targetAttractionStrength = 1.0; + //private double private AttributesTimeCost timeCostAttributes; diff --git a/VadereState/src/org/vadere/state/attributes/models/AttributesTimeCost.java b/VadereState/src/org/vadere/state/attributes/models/AttributesTimeCost.java index 3eb90169ea7eb464b36b4dd4d1541e79f8a4c3e3..2deb89d33c2566a410bbb093df3482d8f5d65648 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 0000000000000000000000000000000000000000..2720e4cad71fbeaee047217e039acc77d0954089 --- /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 c010d636b3bc1202c01afe193bfeb6a07fc0a424..a5bf5634cc673f06b69ca3da7e35a70935ba0ea2 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; @@ -180,13 +181,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<>(); @@ -219,6 +221,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: @@ -453,6 +464,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); @@ -532,6 +549,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). * @@ -1006,12 +1027,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) { @@ -1215,4 +1237,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 8cfc2f0128278c0620fc92a27d7edf0145646a39..f29632af4c49986cb61cb88c1ea11e1bfcc7466f 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 3e68b4b801516c2973f29591f1e51be57c16dd52..891043f2be81b18c6eb86f13660130338bcf21a3 100644 --- a/VadereUtils/src/org/vadere/util/geometry/shapes/VPolygon.java +++ b/VadereUtils/src/org/vadere/util/geometry/shapes/VPolygon.java @@ -10,6 +10,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; @@ -413,11 +414,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 @@ -470,6 +480,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 010616c42f624ad155d83c96d7bb3b202e07f7d4..6bb833d2a08ff36da09b267153d689b52392d71c 100644 --- a/VadereUtils/src/org/vadere/util/geometry/shapes/VRectangle.java +++ b/VadereUtils/src/org/vadere/util/geometry/shapes/VRectangle.java @@ -6,6 +6,7 @@ import java.awt.*; import java.awt.geom.Rectangle2D; import java.util.Arrays; import java.util.List; +import java.util.Optional; @SuppressWarnings("serial") /** @@ -68,6 +69,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 cfa0d96272cf84ac4cc4c6a9445c9d78a9274102..aec90c6b0364d7ff2dab206843c862b0e87bee8d 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 daf6ce921d5a5e2085e909c178cc126686f32efd..15a82689a166166990fb228ecbcbff8a4809a29b 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; /** * Geometric shape and position. @@ -18,6 +19,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 0000000000000000000000000000000000000000..346f89374c8dab1c17584292321c5d4bbcd2e12d --- /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); + } + } +} + diff --git a/VadereUtils/src/org/vadere/util/math/MathUtil.java b/VadereUtils/src/org/vadere/util/math/MathUtil.java index 859e759a468cc874ba5ad0d3a63cfbdccf7caef0..7a4611804c0544abd8b810a520dc6c233cbd68ee 100644 --- a/VadereUtils/src/org/vadere/util/math/MathUtil.java +++ b/VadereUtils/src/org/vadere/util/math/MathUtil.java @@ -15,6 +15,9 @@ public class MathUtil { private final static List neumannNeighborhood = getNeumannNeighborhood(new Point(0, 0)); + // epsilon for finite differences see https://en.wikipedia.org/wiki/Numerical_differentiation#Practical_considerations_using_floating_point_arithmetic + public static double EPSILON = Math.sqrt(Math.ulp(1.0)); + public static double toPositiveSmallestRadian(final double radian) { double result = radian; if(result < 0) { diff --git a/VadereUtils/tests/org/vadere/util/math/TestMachineEpsilon.java b/VadereUtils/tests/org/vadere/util/math/TestMachineEpsilon.java new file mode 100644 index 0000000000000000000000000000000000000000..f8d00975ef5d61e55e3c2e3f4374742e25d1b293 --- /dev/null +++ b/VadereUtils/tests/org/vadere/util/math/TestMachineEpsilon.java @@ -0,0 +1,33 @@ +package org.vadere.util.math; + +import org.junit.Test; + +/** + * @author Benedikt Zoennchen + */ +public class TestMachineEpsilon { + + @Test + public void testMachineEpsilon() { + assert Double.compare(Math.ulp(1.0), machineEpsilon(0.5)) == 0; + } + + public static double machineEpsilon(double eps) + { + // taking a floating type variable + double prev_epsilon = 0.0; + + // run until condition satisfy + while ((1+eps) != 1) + { + // copying value of epsilon + // into previous epsilon + prev_epsilon = eps; + + // dividing epsilon by 2 + eps /=2; + } + + return prev_epsilon; + } +}