Commit d71c4cd5 authored by BZoennchen's avatar BZoennchen

Merge branch 'dev/BHM' into 'master'

Dev/bhm

See merge request !63
parents dac80587 e631854e
Pipeline #114756 passed with stages
in 135 minutes and 8 seconds
{
"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
{
"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
TEST OSM
\ No newline at end of file
......@@ -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);
......
......@@ -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<Attributes> 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;
}
......
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);
}
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();
}
......
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<Obstacle> closeObstacles = me.detectClosestObstacleProximity(nextPosition, me.getRadius() + GeometryUtils.DOUBLE_EPS);
// if there is none, there is no need to change the target direction
if(closeObstacles.isPresent()) {
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<VPoint> closestIntersectionPoint = shape.getClosestIntersectionPoint(start, end, start);
// this should never happen!
if(!closestIntersectionPoint.isPresent()) {
return end;
}
closestPoint = closestIntersectionPoint.get();
} else {
closestPoint = shape.closestPoint(end);
}
return closestPoint;
}
}
package org.vadere.simulator.models.bhm;
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<Obstacle> collideObstacles, VPoint currentNextPosition) {
VPoint newNextPosition = currentNextPosition;
for(Obstacle obstacle : collideObstacles) {
newNextPosition = findCollisionFreePosition(obstacle, me.getPosition(), newNextPosition);
}
return newNextPosition;
}
private VPoint findCollisionFreePosition(@NotNull final Obstacle obstacle, VPoint start, VPoint end) {
VPoint direction = end.subtract(start);
VShape shape = obstacle.getShape();
boolean contains = shape.contains(end);
VPoint closestPoint;
if(contains) {
Optional<VPoint> closestIntersectionPoint = shape.getClosestIntersectionPoint(start, end, start);
// this should never happen!
if(!closestIntersectionPoint.isPresent()) {
return end;
}
closestPoint = closestIntersectionPoint.get();
} else {
closestPoint = shape.closestPoint(end);
}
double distance = contains ? -closestPoint.distance(end) : closestPoint.distance(end);
double diff = me.getRadius() - distance + 0.1;
assert diff > 0;
VPoint normal = end.subtract(closestPoint);
if(contains) {
normal = normal.scalarMultiply(-1.0);
}
VPoint q1 = end.add(normal.setMagnitude(diff));
VPoint q2 = q1.add(normal.rotate(Math.PI * 0.5));
VPoint newEnd = GeometryUtils.lineIntersectionPoint(q1, q2, start, end);
VPoint newDirection = newEnd.subtract(start);
// the new end generates a shorter step in the same direction?
if(newDirection.distanceToOrigin() < direction.distanceToOrigin() && direction.subtract(newDirection).distanceToOrigin() < direction.distanceToOrigin()) {
return newEnd;
} else {
return end;
}
//return newEnd;
}
@Override
public VPoint getNavigationPosition() {
me.action = 1; // LOGGING
VPoint result = me.computeTargetStep();
boolean targetDirection = true;
// this is a problem since