Skip to content

V
vadere
Commit cd65cfb9 authored by Benedikt Zoennchen's avatar Benedikt Zoennchen
Browse files
Options

new robust computation of the target direction for the BHM which is enabled if... 

new robust computation of the target direction for the BHM which is enabled if a target potential is available. New TimeCostFunction to compute a target potential repulsive at obstacles.
parent d1d0a432
Hide whitespace changes
Inline Side-by-side
Showing with 912 additions and 141 deletions
VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_displaced_ok.scenario
View file @ cd65cfb9
......@@ -64,7 +64,7 @@
"stepLengthIntercept" : 0.4625,
"stepLengthSlopeSpeed" : 0.2345,
"stepLengthSD" : 0.036,
"stepLengthDeviation" : false,
"stepLengthDeviation" : true,
"navigationCluster" : false,
"navigationFollower" : false,
"directionWallDistance" : false,
......@@ -72,7 +72,7 @@
"sidewaysEvasion" : false,
"onlyEvadeContraFlow" : false,
"makeSmallSteps" : false,
"followerProximityNavigation" : true,
"followerProximityNavigation" : false,
"differentBehaviour" : false,
"differentEvasionBehaviourPercentage" : [ ],
"varyingBehaviour" : false,
......@@ -86,8 +86,8 @@
"followerDistance" : 10.0,
"smallStepResolution" : 5,
"plannedStepsAhead" : 5,
"obstacleRepulsionReach" : 1.0,
"obstacleRepulsionMaxWeight" : 0.5,
"obstacleRepulsionReach" : 0.5,
"obstacleRepulsionMaxWeight" : 6.0,
"distanceToKeep" : 0.5,
"backwardsAngle" : 1.5707963267948966,
"reconsiderOldTargets" : false,
......@@ -103,22 +103,24 @@
"targetAttractionStrength" : 1.0,
"timeCostAttributes" : {
"standardDeviation" : 0.2,
"type" : "UNIT",
"obstacleDensityWeight" : 3.5,
"type" : "DISTANCE_TO_OBSTACLES",
"obstacleDensityWeight" : 1.0,
"pedestrianSameTargetDensityWeight" : 3.5,
"pedestrianOtherTargetDensityWeight" : 3.5,
"pedestrianWeight" : 3.5,
"queueWidthLoading" : 1.0,
"pedestrianDynamicWeight" : 6.0,
"loadingType" : "CONSTANT"
"loadingType" : "CONSTANT",
"width" : 0.4,
"height" : 5.0
}
}
},
"attributesSimulation" : {
"finishTime" : 200.0,
"finishTime" : 500.0,
"simTimeStepLength" : 0.4,
"realTimeSimTimeRatio" : 0.0,
"writeSimulationData" : true,
"writeSimulationData" : false,
"visualizationEnabled" : true,
"printFPS" : false,
"digitsPerCoordinate" : 2,
......@@ -138,15 +140,6 @@
"bounded" : true
},
"obstacles" : [ {
"shape" : {
"x" : 75.0,
"y" : 77.0,
"width" : 26.1,
"height" : 20.0,
"type" : "RECTANGLE"
},
"id" : -1
}, {
"shape" : {
"x" : 63.5,
"y" : 99.5,
......@@ -155,13 +148,31 @@
"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
} ],
"stairs" : [ ],
"targets" : [ {
"id" : 1,
"absorbing" : true,
"shape" : {
"x" : 76.0,
"x" : 67.0,
"y" : 107.0,
"width" : 15.0,
"height" : 5.0,
......@@ -187,7 +198,7 @@
},
"interSpawnTimeDistribution" : "org.vadere.state.scenario.ConstantDistribution",
"distributionParameters" : [ 1.0 ],
"spawnNumber" : 100,
"spawnNumber" : 500,
"maxSpawnNumberTotal" : -1,
"startTime" : 0.0,
"endTime" : 0.0,
......
VadereModelTests/TestBHM/scenarios/floor_field_navigation_test_ok.scenario
View file @ cd65cfb9
......@@ -64,15 +64,15 @@
"stepLengthIntercept" : 0.4625,
"stepLengthSlopeSpeed" : 0.2345,
"stepLengthSD" : 0.036,
"stepLengthDeviation" : false,
"stepLengthDeviation" : true,
"navigationCluster" : false,
"navigationFollower" : false,
"directionWallDistance" : false,
"tangentialEvasion" : true,
"sidewaysEvasion" : true,
"tangentialEvasion" : false,
"sidewaysEvasion" : false,
"onlyEvadeContraFlow" : false,
"makeSmallSteps" : true,
"followerProximityNavigation" : true,
"makeSmallSteps" : false,
"followerProximityNavigation" : false,
"differentBehaviour" : false,
"differentEvasionBehaviourPercentage" : [ ],
"varyingBehaviour" : false,
......@@ -86,8 +86,8 @@
"followerDistance" : 10.0,
"smallStepResolution" : 5,
"plannedStepsAhead" : 5,
"obstacleRepulsionReach" : 1.0,
"obstacleRepulsionMaxWeight" : 0.5,
"obstacleRepulsionReach" : 0.5,
"obstacleRepulsionMaxWeight" : 6.0,
"distanceToKeep" : 0.5,
"backwardsAngle" : 1.5707963267948966,
"reconsiderOldTargets" : false,
......@@ -102,22 +102,24 @@
"obstacleGridPenalty" : 0.1,
"targetAttractionStrength" : 1.0,
"timeCostAttributes" : {
"standardDeviation" : 0.7,
"type" : "OBSTACLES",
"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"
"loadingType" : "CONSTANT",
"width" : 0.4,
"height" : 5.0
}
}
},
"attributesSimulation" : {
"finishTime" : 200.0,
"simTimeStepLength" : 0.4,
"realTimeSimTimeRatio" : 0.0,
"realTimeSimTimeRatio" : 0.2,
"writeSimulationData" : true,
"visualizationEnabled" : true,
"printFPS" : false,
......@@ -139,8 +141,8 @@
},
"obstacles" : [ {
"shape" : {
"x" : -2.5,
"y" : 43.5,
"x" : -2.6,
"y" : 44.3,
"width" : 23.9,
"height" : 3.6,
"type" : "RECTANGLE"
......@@ -157,8 +159,8 @@
"id" : -1
}, {
"shape" : {
"x" : 0.4,
"y" : 36.7,
"x" : 0.3,
"y" : 36.5,
"width" : 29.9,
"height" : 1.9,
"type" : "RECTANGLE"
......@@ -166,13 +168,22 @@
"id" : -1
}, {
"shape" : {
"x" : 1.6,
"y" : 33.0,
"width" : 33.4,
"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
} ],
"stairs" : [ ],
"targets" : [ {
......
VadereSimulator/src/org/vadere/simulator/control/Simulation.java
View file @ cd65cfb9
......@@ -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
View file @ cd65cfb9
......@@ -62,6 +62,10 @@ 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) {
......
VadereSimulator/src/org/vadere/simulator/models/bhm/DirectionAddendObstacleTargetPotential.java
View file @ cd65cfb9
......@@ -3,11 +3,21 @@ 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;
......@@ -22,15 +32,74 @@ public class DirectionAddendObstacleTargetPotential implements DirectionAddend {
public VPoint getDirectionAddend(@NotNull final VPoint targetDirection) {
VPoint addend = VPoint.ZERO;
Optional<Obstacle> closeObstacles = me.detectClosestObstacleProximity(me.getPosition(), me.getRadius());
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
View file @ cd65cfb9
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 the ped will never move!
List<Obstacle> 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,18 @@ 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());
......
VadereSimulator/src/org/vadere/simulator/models/bhm/PedestrianBHM.java
View file @ cd65cfb9
......@@ -22,10 +22,8 @@ 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.VCircle;
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;
......@@ -46,7 +44,7 @@ public class PedestrianBHM extends Pedestrian {
private VPoint targetDirection;
private final transient Navigation navigation;
private final List<DirectionAddend> directionAddends;
private final transient List<DirectionAddend> directionAddends;
protected int action;
......@@ -54,6 +52,7 @@ public class PedestrianBHM extends Pedestrian {
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) {
......@@ -105,8 +104,17 @@ 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() {
......@@ -238,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());
}
/**
......@@ -256,28 +264,16 @@ public class PedestrianBHM extends Pedestrian {
}
if (hasNextTarget()) {
VShape targetShape = topography.getTarget(getNextTargetId()).getShape();
if (!targetShape.contains(getPosition())) {
Target target = topography.getTarget(getNextTargetId());
if (!target.getShape().contains(getPosition())) {
// use just euklid direction to the target
if(potentialFieldTarget == null) {
VPoint targetPoint = targetShape.closestPoint(getPosition());
targetDirection = targetPoint.subtract(getPosition()).norm();
}
else {
Vector2D vec = new Vector2D(computeTargetDirectionByGradient());
if(vec.getLength() < GeometryUtils.DOUBLE_EPS) {
targetDirection = VPoint.ZERO;
}
else {
targetDirection = vec.norm();
}
}
targetDirection = targetDirectionStrategy.getTargetDirection(target);
for (DirectionAddend da : directionAddends) {
targetDirection = targetDirection.add(da.getDirectionAddend(targetDirection));
}
//TODO: if this happens it might cause problems dependent on the heuristics choose.
if(targetDirection.distanceToOrigin() < GeometryUtils.DOUBLE_EPS) {
targetDirection = VPoint.ZERO;
}
......@@ -288,33 +284,6 @@ public class PedestrianBHM extends Pedestrian {
}
}
private VPoint computeTargetDirectionByGradient() {
return potentialFieldTarget.getTargetPotentialGradient(getPosition(), this).multiply(-1.0);
}
private VPoint computeTargetDirectionByOptimaization() {
Vector2D gradient = potentialFieldTarget.getTargetPotentialGradient(getPosition(), this).multiply(-1.0);
double angle = GeometryUtils.angleTo(gradient, new VPoint(1, 0));
List<VPoint> possibleNextPositions = GeometryUtils.getDiscDiscretizationPoints(
random,
false,
new VCircle(getPosition(), stepLength),
1,
15,
angle,
2*Math.PI);
VPoint nextOptimalPos = possibleNextPositions.stream()
.filter(p -> !collidesWithObstacle(p))
.min(
(p1, p2) -> Double.compare(potentialFieldTarget.getPotential(p1, this),
potentialFieldTarget.getPotential(p2, this))
).get();
return nextOptimalPos.subtract(getPosition());
}
/**
* Set the last target if beyond a certain threshold.
*/
......@@ -385,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()) -
......@@ -422,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;
......@@ -438,7 +410,7 @@ public class PedestrianBHM extends Pedestrian {
List<Obstacle> result = new LinkedList<>();
for (Obstacle obstacle : obstacles) {
if (obstacle.getShape().distance(position) < proximity) {
if (obstacle.getShape().distance(position) < proximity) {
result.add(obstacle);
}
}
......@@ -456,6 +428,7 @@ public class PedestrianBHM extends Pedestrian {
double distance = obstacle.getShape().distance(position);
if (distance < proximity && distance < minDistance) {
obs = obstacle;
minDistance = distance;
}
}
return Optional.ofNullable(obs);
......@@ -510,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());