Merge branch 'dev/BHM' into 'master'

Dev/bhm

See merge request !63

VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_displaced_ok.scenario

+{
+  "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

VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_ok.scenario

+{
+  "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

VadereModelTests/TestBHM/vadere.project

+TEST OSM
\ No newline at end of file

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);

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<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;
 	}

VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddend.java

 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);
 }

VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacle.java

 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();
 	}
 

VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacleTargetPotential.java

+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;
+	}
+}

VadereSimulator/src/org/vadere/simulator/models/bhm/NavigationProximity.java

 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?