Commit d1d0a432 authored by BZoennchen's avatar BZoennchen

implementation of a new obstacle avoidance for the BHM, not ready jet.

parent 29f15b6f
{ {
"name" : "floor_field_navigation_test_displaced_ok", "name" : "floor_field_navigation_test_displaced_ok",
"description" : "", "description" : "",
"release" : "0.6", "release" : "0.7",
"processWriters" : { "processWriters" : {
"files" : [ { "files" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.TimestepPedestrianIdOutputFile", "type" : "org.vadere.simulator.projects.dataprocessing.outputfile.TimestepPedestrianIdOutputFile",
...@@ -54,7 +54,8 @@ ...@@ -54,7 +54,8 @@
"pedestrianOverlapProcessorId" : 3 "pedestrianOverlapProcessorId" : 3
} }
} ], } ],
"isTimestamped" : true "isTimestamped" : true,
"isWriteMetaData" : false
}, },
"scenario" : { "scenario" : {
"mainModel" : "org.vadere.simulator.models.bhm.BehaviouralHeuristicsModel", "mainModel" : "org.vadere.simulator.models.bhm.BehaviouralHeuristicsModel",
...@@ -101,8 +102,8 @@ ...@@ -101,8 +102,8 @@
"obstacleGridPenalty" : 0.1, "obstacleGridPenalty" : 0.1,
"targetAttractionStrength" : 1.0, "targetAttractionStrength" : 1.0,
"timeCostAttributes" : { "timeCostAttributes" : {
"standardDeviation" : 0.7, "standardDeviation" : 0.2,
"type" : "OBSTACLES", "type" : "UNIT",
"obstacleDensityWeight" : 3.5, "obstacleDensityWeight" : 3.5,
"pedestrianSameTargetDensityWeight" : 3.5, "pedestrianSameTargetDensityWeight" : 3.5,
"pedestrianOtherTargetDensityWeight" : 3.5, "pedestrianOtherTargetDensityWeight" : 3.5,
...@@ -174,6 +175,7 @@ ...@@ -174,6 +175,7 @@
"startingWithRedLight" : false, "startingWithRedLight" : false,
"nextSpeed" : -1.0 "nextSpeed" : -1.0
} ], } ],
"absorbingAreas" : [ ],
"sources" : [ { "sources" : [ {
"id" : -1, "id" : -1,
"shape" : { "shape" : {
...@@ -205,7 +207,9 @@ ...@@ -205,7 +207,9 @@
"maximumSpeed" : 2.2, "maximumSpeed" : 2.2,
"acceleration" : 2.0 "acceleration" : 2.0
}, },
"teleporter" : null,
"attributesCar" : null "attributesCar" : null
} },
"eventInfos" : [ ]
} }
} }
\ No newline at end of file
{ {
"name" : "floor_field_navigation_test_ok", "name" : "floor_field_navigation_test_ok",
"description" : "", "description" : "",
"release" : "0.6", "release" : "0.7",
"processWriters" : { "processWriters" : {
"files" : [ { "files" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.TimestepPedestrianIdOutputFile", "type" : "org.vadere.simulator.projects.dataprocessing.outputfile.TimestepPedestrianIdOutputFile",
...@@ -54,7 +54,8 @@ ...@@ -54,7 +54,8 @@
"pedestrianOverlapProcessorId" : 3 "pedestrianOverlapProcessorId" : 3
} }
} ], } ],
"isTimestamped" : true "isTimestamped" : true,
"isWriteMetaData" : false
}, },
"scenario" : { "scenario" : {
"mainModel" : "org.vadere.simulator.models.bhm.BehaviouralHeuristicsModel", "mainModel" : "org.vadere.simulator.models.bhm.BehaviouralHeuristicsModel",
...@@ -68,9 +69,9 @@ ...@@ -68,9 +69,9 @@
"navigationFollower" : false, "navigationFollower" : false,
"directionWallDistance" : false, "directionWallDistance" : false,
"tangentialEvasion" : true, "tangentialEvasion" : true,
"sidewaysEvasion" : false, "sidewaysEvasion" : true,
"onlyEvadeContraFlow" : false, "onlyEvadeContraFlow" : false,
"makeSmallSteps" : false, "makeSmallSteps" : true,
"followerProximityNavigation" : true, "followerProximityNavigation" : true,
"differentBehaviour" : false, "differentBehaviour" : false,
"differentEvasionBehaviourPercentage" : [ ], "differentEvasionBehaviourPercentage" : [ ],
...@@ -103,7 +104,7 @@ ...@@ -103,7 +104,7 @@
"timeCostAttributes" : { "timeCostAttributes" : {
"standardDeviation" : 0.7, "standardDeviation" : 0.7,
"type" : "OBSTACLES", "type" : "OBSTACLES",
"obstacleDensityWeight" : 3.5, "obstacleDensityWeight" : 1.0,
"pedestrianSameTargetDensityWeight" : 3.5, "pedestrianSameTargetDensityWeight" : 3.5,
"pedestrianOtherTargetDensityWeight" : 3.5, "pedestrianOtherTargetDensityWeight" : 3.5,
"pedestrianWeight" : 3.5, "pedestrianWeight" : 3.5,
...@@ -138,19 +139,37 @@ ...@@ -138,19 +139,37 @@
}, },
"obstacles" : [ { "obstacles" : [ {
"shape" : { "shape" : {
"x" : 9.0, "x" : -2.5,
"y" : 21.0, "y" : 43.5,
"width" : 26.1, "width" : 23.9,
"height" : 20.0, "height" : 3.6,
"type" : "RECTANGLE" "type" : "RECTANGLE"
}, },
"id" : -1 "id" : -1
}, { }, {
"shape" : { "shape" : {
"x" : -2.5, "x" : 2.4,
"y" : 43.5, "y" : 39.9,
"width" : 23.9, "width" : 32.2,
"height" : 3.6, "height" : 2.4,
"type" : "RECTANGLE"
},
"id" : -1
}, {
"shape" : {
"x" : 0.4,
"y" : 36.7,
"width" : 29.9,
"height" : 1.9,
"type" : "RECTANGLE"
},
"id" : -1
}, {
"shape" : {
"x" : 1.6,
"y" : 33.0,
"width" : 33.4,
"height" : 1.9,
"type" : "RECTANGLE" "type" : "RECTANGLE"
}, },
"id" : -1 "id" : -1
...@@ -174,6 +193,7 @@ ...@@ -174,6 +193,7 @@
"startingWithRedLight" : false, "startingWithRedLight" : false,
"nextSpeed" : -1.0 "nextSpeed" : -1.0
} ], } ],
"absorbingAreas" : [ ],
"sources" : [ { "sources" : [ {
"id" : -1, "id" : -1,
"shape" : { "shape" : {
...@@ -205,7 +225,9 @@ ...@@ -205,7 +225,9 @@
"maximumSpeed" : 2.2, "maximumSpeed" : 2.2,
"acceleration" : 2.0 "acceleration" : 2.0
}, },
"teleporter" : null,
"attributesCar" : null "attributesCar" : null
} },
"eventInfos" : [ ]
} }
} }
\ No newline at end of file
package org.vadere.simulator.models.bhm; package org.vadere.simulator.models.bhm;
import org.jetbrains.annotations.NotNull;
import org.vadere.util.geometry.shapes.VPoint; import org.vadere.util.geometry.shapes.VPoint;
public interface DirectionAddend { public interface DirectionAddend {
public VPoint getDirectionAddend(); public VPoint getDirectionAddend(@NotNull final VPoint targetDirection);
} }
package org.vadere.simulator.models.bhm; package org.vadere.simulator.models.bhm;
import org.jetbrains.annotations.NotNull;
import org.vadere.state.attributes.models.AttributesBHM; import org.vadere.state.attributes.models.AttributesBHM;
import org.vadere.state.scenario.Obstacle; import org.vadere.state.scenario.Obstacle;
import org.vadere.util.geometry.shapes.VPoint; import org.vadere.util.geometry.shapes.VPoint;
...@@ -20,7 +21,7 @@ public class DirectionAddendObstacle implements DirectionAddend { ...@@ -20,7 +21,7 @@ public class DirectionAddendObstacle implements DirectionAddend {
} }
@Override @Override
public VPoint getDirectionAddend() { public VPoint getDirectionAddend(@NotNull final VPoint targetDirection) {
return getTargetObstacleDirection(); 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.shapes.VPoint;
import java.util.List;
import java.util.Optional;
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;
Optional<Obstacle> closeObstacles = me.detectClosestObstacleProximity(me.getPosition(), me.getRadius());
if(closeObstacles.isPresent()) {
Obstacle obstacle = closeObstacles.get();
VPoint closestPoint = obstacle.getShape().closestPoint(me.getPosition());
}
return addend;
}
}
...@@ -3,6 +3,7 @@ package org.vadere.simulator.models.bhm; ...@@ -3,6 +3,7 @@ package org.vadere.simulator.models.bhm;
import java.util.Collection; import java.util.Collection;
import java.util.LinkedList; import java.util.LinkedList;
import java.util.List; import java.util.List;
import java.util.Optional;
import java.util.Random; import java.util.Random;
import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.NotNull;
...@@ -60,7 +61,7 @@ public class PedestrianBHM extends Pedestrian { ...@@ -60,7 +61,7 @@ public class PedestrianBHM extends Pedestrian {
} }
public PedestrianBHM(Topography topography, AttributesAgent attributesPedestrian, public PedestrianBHM(Topography topography, AttributesAgent attributesPedestrian,
AttributesBHM attributesBHM, Random random, IPotentialFieldTarget potentialFieldTarget) { AttributesBHM attributesBHM, Random random, @Nullable IPotentialFieldTarget potentialFieldTarget) {
super(attributesPedestrian, random); super(attributesPedestrian, random);
this.potentialFieldTarget = potentialFieldTarget; this.potentialFieldTarget = potentialFieldTarget;
this.random = random; this.random = random;
...@@ -108,6 +109,10 @@ public class PedestrianBHM extends Pedestrian { ...@@ -108,6 +109,10 @@ public class PedestrianBHM extends Pedestrian {
setEvasionStrategy(); setEvasionStrategy();
} }
private boolean isPotentialFieldInUse() {
return potentialFieldTarget != null;
}
private void setEvasionStrategy() { private void setEvasionStrategy() {
if (attributesBHM.isSwitchBehaviour()) { if (attributesBHM.isSwitchBehaviour()) {
...@@ -270,7 +275,7 @@ public class PedestrianBHM extends Pedestrian { ...@@ -270,7 +275,7 @@ public class PedestrianBHM extends Pedestrian {
} }
for (DirectionAddend da : directionAddends) { for (DirectionAddend da : directionAddends) {
targetDirection = targetDirection.add(da.getDirectionAddend()); targetDirection = targetDirection.add(da.getDirectionAddend(targetDirection));
} }
if(targetDirection.distanceToOrigin() < GeometryUtils.DOUBLE_EPS) { if(targetDirection.distanceToOrigin() < GeometryUtils.DOUBLE_EPS) {
...@@ -441,6 +446,22 @@ public class PedestrianBHM extends Pedestrian { ...@@ -441,6 +446,22 @@ public class PedestrianBHM extends Pedestrian {
return result; return result;
} }
Optional<Obstacle> detectClosestObstacleProximity(@NotNull final VPoint position, double proximity) {
Collection<Obstacle> 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;
}
}
return Optional.ofNullable(obs);
}
// Java nuisance... // Java nuisance...
......
package org.vadere.simulator.models.bhm; package org.vadere.simulator.models.bhm;
import org.jetbrains.annotations.NotNull;
import org.vadere.state.scenario.Pedestrian; 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.VLine;
import org.vadere.util.geometry.shapes.VPoint; import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.VPolygon;
import org.vadere.util.logging.Logger; import org.vadere.util.logging.Logger;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.HashSet;
import java.util.LinkedList; import java.util.LinkedList;
import java.util.List; import java.util.List;
import java.util.Random; import java.util.Random;
import java.util.Set;
import java.util.stream.Collectors;
public class UtilsBHM { public class UtilsBHM {
...@@ -153,39 +159,32 @@ 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<Pedestrian> convexHull(final List<Pedestrian> cluster) { private static List<Pedestrian> convexHull(@NotNull final List<Pedestrian> cluster) {
List<Pedestrian> result; if(cluster.size() > 2) {
return cluster;
// 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
} }
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<VPoint> convexHull = new HashSet<>(grahamScan.getConvexHull());
return cluster.stream().filter(ped -> convexHull.contains(ped.getPosition())).collect(Collectors.toList());
} }
public static int randomChoice(List<Double> 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<Double> fractions, @NotNull final Random random) {
double[] probabilities = new double[fractions.size()]; double[] probabilities = new double[fractions.size()];
double sum = 0; double sum = 0;
......
...@@ -17,6 +17,7 @@ import org.vadere.util.geometry.shapes.VPoint; ...@@ -17,6 +17,7 @@ import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.VShape; import org.vadere.util.geometry.shapes.VShape;
import org.vadere.util.geometry.shapes.Vector2D; import org.vadere.util.geometry.shapes.Vector2D;
import org.vadere.util.logging.Logger; import org.vadere.util.logging.Logger;
import org.vadere.util.math.MathUtil;
import java.util.HashMap; import java.util.HashMap;
import java.util.List; import java.util.List;
...@@ -104,9 +105,11 @@ public class PotentialFieldTarget implements IPotentialFieldTarget { ...@@ -104,9 +105,11 @@ public class PotentialFieldTarget implements IPotentialFieldTarget {
@Override @Override
public Vector2D getTargetPotentialGradient(VPoint pos, Agent ped) { public Vector2D getTargetPotentialGradient(VPoint pos, Agent ped) {
double potential = getPotential(pos, ped); double potential = getPotential(pos, ped);
double eps = 0.001; // according to https://en.wikipedia.org/wiki/Numerical_differentiation#Practical_considerations_using_floating_point_arithmetic
double dGradPX = (getPotential(pos.add(new VPoint(eps, 0)), ped) - potential) / eps; double eps = Math.max(pos.x, pos.y) * MathUtil.EPSILON;
double dGradPY = (getPotential(pos.add(new VPoint(0, eps)), ped) - potential) / eps; //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); return new Vector2D(dGradPX, dGradPY);
} }
......
...@@ -18,6 +18,7 @@ public class AttributesFloorField extends Attributes { ...@@ -18,6 +18,7 @@ public class AttributesFloorField extends Attributes {
private double potentialFieldResolution = 0.1; private double potentialFieldResolution = 0.1;
private double obstacleGridPenalty = 0.1; private double obstacleGridPenalty = 0.1;
private double targetAttractionStrength = 1.0; private double targetAttractionStrength = 1.0;
//private double
private AttributesTimeCost timeCostAttributes; private AttributesTimeCost timeCostAttributes;
......
...@@ -15,6 +15,9 @@ public class MathUtil { ...@@ -15,6 +15,9 @@ public class MathUtil {
private final static List<Point> neumannNeighborhood = getNeumannNeighborhood(new Point(0, 0)); private final static List<Point> 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) { public static double toPositiveSmallestRadian(final double radian) {
double result = radian; double result = radian;
if(result < 0) { if(result < 0) {
......
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;
}
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment