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",
"description" : "",
"release" : "0.6",
"release" : "0.7",
"processWriters" : {
"files" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.TimestepPedestrianIdOutputFile",
......@@ -54,7 +54,8 @@
"pedestrianOverlapProcessorId" : 3
}
} ],
"isTimestamped" : true
"isTimestamped" : true,
"isWriteMetaData" : false
},
"scenario" : {
"mainModel" : "org.vadere.simulator.models.bhm.BehaviouralHeuristicsModel",
......@@ -101,8 +102,8 @@
"obstacleGridPenalty" : 0.1,
"targetAttractionStrength" : 1.0,
"timeCostAttributes" : {
"standardDeviation" : 0.7,
"type" : "OBSTACLES",
"standardDeviation" : 0.2,
"type" : "UNIT",
"obstacleDensityWeight" : 3.5,
"pedestrianSameTargetDensityWeight" : 3.5,
"pedestrianOtherTargetDensityWeight" : 3.5,
......@@ -174,6 +175,7 @@
"startingWithRedLight" : false,
"nextSpeed" : -1.0
} ],
"absorbingAreas" : [ ],
"sources" : [ {
"id" : -1,
"shape" : {
......@@ -205,7 +207,9 @@
"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.6",
"release" : "0.7",
"processWriters" : {
"files" : [ {
"type" : "org.vadere.simulator.projects.dataprocessing.outputfile.TimestepPedestrianIdOutputFile",
......@@ -54,7 +54,8 @@
"pedestrianOverlapProcessorId" : 3
}
} ],
"isTimestamped" : true
"isTimestamped" : true,
"isWriteMetaData" : false
},
"scenario" : {
"mainModel" : "org.vadere.simulator.models.bhm.BehaviouralHeuristicsModel",
......@@ -68,9 +69,9 @@
"navigationFollower" : false,
"directionWallDistance" : false,
"tangentialEvasion" : true,
"sidewaysEvasion" : false,
"sidewaysEvasion" : true,
"onlyEvadeContraFlow" : false,
"makeSmallSteps" : false,
"makeSmallSteps" : true,
"followerProximityNavigation" : true,
"differentBehaviour" : false,
"differentEvasionBehaviourPercentage" : [ ],
......@@ -103,7 +104,7 @@
"timeCostAttributes" : {
"standardDeviation" : 0.7,
"type" : "OBSTACLES",
"obstacleDensityWeight" : 3.5,
"obstacleDensityWeight" : 1.0,
"pedestrianSameTargetDensityWeight" : 3.5,
"pedestrianOtherTargetDensityWeight" : 3.5,
"pedestrianWeight" : 3.5,
......@@ -138,19 +139,37 @@
},
"obstacles" : [ {
"shape" : {
"x" : 9.0,
"y" : 21.0,
"width" : 26.1,
"height" : 20.0,
"x" : -2.5,
"y" : 43.5,
"width" : 23.9,
"height" : 3.6,
"type" : "RECTANGLE"
},
"id" : -1
}, {
"shape" : {
"x" : -2.5,
"y" : 43.5,
"width" : 23.9,
"height" : 3.6,
"x" : 2.4,
"y" : 39.9,
"width" : 32.2,
"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"
},
"id" : -1
......@@ -174,6 +193,7 @@
"startingWithRedLight" : false,
"nextSpeed" : -1.0
} ],
"absorbingAreas" : [ ],
"sources" : [ {
"id" : -1,
"shape" : {
......@@ -205,7 +225,9 @@
"maximumSpeed" : 2.2,
"acceleration" : 2.0
},
"teleporter" : null,
"attributesCar" : null
}
},
"eventInfos" : [ ]
}
}
\ No newline at end of file
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.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;
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;
......@@ -60,7 +61,7 @@ public class PedestrianBHM extends Pedestrian {
}
public PedestrianBHM(Topography topography, AttributesAgent attributesPedestrian,
AttributesBHM attributesBHM, Random random, IPotentialFieldTarget potentialFieldTarget) {
AttributesBHM attributesBHM, Random random, @Nullable IPotentialFieldTarget potentialFieldTarget) {
super(attributesPedestrian, random);
this.potentialFieldTarget = potentialFieldTarget;
this.random = random;
......@@ -108,6 +109,10 @@ public class PedestrianBHM extends Pedestrian {
setEvasionStrategy();
}
private boolean isPotentialFieldInUse() {
return potentialFieldTarget != null;
}
private void setEvasionStrategy() {
if (attributesBHM.isSwitchBehaviour()) {
......@@ -270,7 +275,7 @@ public class PedestrianBHM extends Pedestrian {
}
for (DirectionAddend da : directionAddends) {
targetDirection = targetDirection.add(da.getDirectionAddend());
targetDirection = targetDirection.add(da.getDirectionAddend(targetDirection));
}
if(targetDirection.distanceToOrigin() < GeometryUtils.DOUBLE_EPS) {
......@@ -441,6 +446,22 @@ public class PedestrianBHM extends Pedestrian {
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...
......
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<Pedestrian> convexHull(final List<Pedestrian> cluster) {
List<Pedestrian> 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<Pedestrian> convexHull(@NotNull final List<Pedestrian> 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<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 sum = 0;
......
......@@ -17,6 +17,7 @@ import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.VShape;
import org.vadere.util.geometry.shapes.Vector2D;
import org.vadere.util.logging.Logger;
import org.vadere.util.math.MathUtil;
import java.util.HashMap;
import java.util.List;
......@@ -104,9 +105,11 @@ public class PotentialFieldTarget implements IPotentialFieldTarget {
@Override
public Vector2D getTargetPotentialGradient(VPoint pos, Agent ped) {
double potential = getPotential(pos, ped);
double eps = 0.001;
double dGradPX = (getPotential(pos.add(new VPoint(eps, 0)), ped) - potential) / eps;
double dGradPY = (getPotential(pos.add(new VPoint(0, eps)), ped) - potential) / eps;
// 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);
}
......
......@@ -18,6 +18,7 @@ public class AttributesFloorField extends Attributes {
private double potentialFieldResolution = 0.1;
private double obstacleGridPenalty = 0.1;
private double targetAttractionStrength = 1.0;
//private double
private AttributesTimeCost timeCostAttributes;
......
......@@ -15,6 +15,9 @@ public class MathUtil {
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) {
double result = radian;
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