Add BehaviouralHeuristicsModel.

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

+package org.vadere.simulator.models.bhm;
+
+import java.util.Comparator;
+import java.util.LinkedList;
+import java.util.List;
+import java.util.PriorityQueue;
+import java.util.Random;
+
+import org.vadere.simulator.models.MainModel;
+import org.vadere.simulator.models.Model;
+import org.vadere.state.attributes.Attributes;
+import org.vadere.state.attributes.models.AttributesBHM;
+import org.vadere.state.attributes.scenario.AttributesAgent;
+import org.vadere.state.scenario.DynamicElement;
+import org.vadere.state.scenario.Pedestrian;
+import org.vadere.state.scenario.Target;
+import org.vadere.state.scenario.Topography;
+import org.vadere.util.geometry.shapes.VPoint;
+
+public class BehaviouralHeuristicsModel implements MainModel {
+
+	/**
+	 * Compares the time of the next possible move.
+	 */
+	private class ComparatorPedestrianBHM implements Comparator<PedestrianBHM> {
+
+		@Override
+		public int compare(PedestrianBHM ped1, PedestrianBHM ped2) {
+			if (ped1.getTimeOfNextStep() < ped2.getTimeOfNextStep()) {
+				return -1;
+			} else {
+				return 1;
+			}
+		}
+	}
+	
+	private List<Model> models = new LinkedList<>();
+
+	private AttributesBHM attributesBHM;
+	private AttributesAgent attributesPedestrian;
+	private Random random;
+	private Topography topography;
+	private double lastSimTimeInSec;
+	private int pedestrianIdCounter;
+	private PriorityQueue<PedestrianBHM> pedestrianEventsQueue;
+
+	public BehaviouralHeuristicsModel() {
+		this.pedestrianIdCounter = 0;
+		this.pedestrianEventsQueue = new PriorityQueue<>(100, new ComparatorPedestrianBHM());
+	}
+
+	@Override
+	public void initialize(List<Attributes> modelAttributesList, Topography topography,
+			AttributesAgent attributesPedestrian, Random random) {
+		this.attributesBHM = Model.findAttributes(modelAttributesList, AttributesBHM.class);
+		this.attributesPedestrian = attributesPedestrian;
+		this.topography = topography;
+		this.random = random;
+		this.models.add(this);
+	}
+
+	@Override
+	public <T extends DynamicElement> PedestrianBHM createElement(VPoint position, int id, Class<T> type) {
+		if (!Pedestrian.class.isAssignableFrom(type))
+			throw new IllegalArgumentException("BHM cannot initialize " + type.getCanonicalName());
+
+		pedestrianIdCounter++;
+		AttributesAgent pedAttributes = new AttributesAgent(
+				this.attributesPedestrian, id > 0 ? id : pedestrianIdCounter);
+
+		PedestrianBHM pedestrian = new PedestrianBHM(topography, pedAttributes, attributesBHM, random);
+
+		pedestrian.setPosition(position);
+		this.pedestrianEventsQueue.add(pedestrian);
+		return pedestrian;
+	}
+
+	@Override
+	public void preLoop(final double simTimeInSec) {
+		this.lastSimTimeInSec = simTimeInSec;
+	}
+
+
+	@Override
+	public void postLoop(double simTimeInSec) {}
+
+	@Override
+	public void update(final double simTimeInSec) {
+
+		// event driven update
+		if (!pedestrianEventsQueue.isEmpty()) {
+			while (pedestrianEventsQueue.peek().getTimeOfNextStep() < simTimeInSec) {
+				PedestrianBHM ped = pedestrianEventsQueue.poll();
+
+				if (ped.hasNextTarget()) {
+
+					ped.update(simTimeInSec);
+
+					Target target = topography.getTarget(ped.getNextTargetId());
+
+					if (!(target.getShape().contains(ped.getPosition()) && target.isAbsorbing())) {
+						pedestrianEventsQueue.add(ped);
+					}
+				}
+
+				if (pedestrianEventsQueue.isEmpty()) {
+					break;
+				}
+			}
+		}
+	}
+
+	@Override
+	public List<Model> getSubmodels() {
+		return models;
+	}
+
+}

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

+package org.vadere.simulator.models.bhm;
+
+import org.vadere.util.geometry.shapes.VPoint;
+
+public interface DirectionAddend {
+	public VPoint getDirectionAddend();
+}

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

+package org.vadere.simulator.models.bhm;
+
+import java.util.List;
+
+import org.apache.log4j.LogManager;
+import org.apache.log4j.Logger;
+import org.vadere.state.attributes.models.AttributesBHM;
+import org.vadere.state.scenario.Obstacle;
+import org.vadere.util.geometry.shapes.VPoint;
+
+public class DirectionAddendObstacle implements DirectionAddend {
+
+	private static Logger logger = LogManager.getLogger(DirectionAddendObstacle.class);
+
+	private final AttributesBHM attributesBHM;
+	private final PedestrianBHM me;
+
+	public DirectionAddendObstacle(PedestrianBHM me) {
+		this.me = me;
+		this.attributesBHM = me.getAttributesBHM();
+	}
+
+	@Override
+	public VPoint getDirectionAddend() {
+		return getTargetObstacleDirection();
+	}
+
+	public VPoint getTargetObstacleDirection() {
+
+		VPoint result = VPoint.ZERO;
+
+		List<Obstacle> closeObstacles = me.detectObstacleProximity(me.getPosition(),
+				me.getRadius() + attributesBHM.getObstacleRepulsionReach());
+
+		double normFactor = attributesBHM.getObstacleRepulsionMaxWeight() /
+				sumOfObstacleWeights(closeObstacles);
+
+		for (Obstacle obstacle : closeObstacles) {
+			double weight = weightObstacleDistance(obstacle.getShape().distance(me.getPosition()) + me.getRadius());
+
+			if (weight > 0.001 && normFactor > 0.001) {
+
+				weight = weight * weight * normFactor;
+
+				VPoint direction = me.getPosition().subtract(
+						obstacle.getShape().closestPoint(me.getPosition())).norm();
+
+				result = result.add(direction.scalarMultiply(weight));
+			}
+		}
+
+		return result;
+	}
+
+	private double sumOfObstacleWeights(List<Obstacle> closeObstacles) {
+		double result = 0;
+
+		for (Obstacle obstacle : closeObstacles) {
+			result = result + weightObstacleDistance(obstacle.getShape().distance(me.getPosition()) + me.getRadius());
+		}
+
+		return result;
+	}
+
+	private double weightObstacleDistance(double distance) {
+
+		double result = 0;
+
+		if (distance < attributesBHM.getObstacleRepulsionReach()) {
+			result = attributesBHM.getObstacleRepulsionReach() - distance;
+			result = result / attributesBHM.getObstacleRepulsionReach();
+		}
+
+		return result;
+	}
+}

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

+package org.vadere.simulator.models.bhm;
+
+import org.vadere.util.geometry.shapes.VPoint;
+
+public interface Navigation {
+	public VPoint getNavigationPosition();
+}

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

+package org.vadere.simulator.models.bhm;
+
+import java.util.LinkedList;
+import java.util.List;
+import java.util.Random;
+
+import org.apache.log4j.LogManager;
+import org.apache.log4j.Logger;
+import org.vadere.state.attributes.models.AttributesBHM;
+import org.vadere.state.scenario.Pedestrian;
+import org.vadere.state.scenario.Topography;
+import org.vadere.util.geometry.shapes.VPoint;
+import org.vadere.util.geometry.shapes.VShape;
+
+public class NavigationCluster implements Navigation {
+
+	private static Logger logger = LogManager.getLogger(NavigationCluster.class);
+
+	private final Topography topography;
+	private final AttributesBHM attributesBHM;
+	private final PedestrianBHM me;
+	private final NavigationProximity proximityNavigation;
+
+	public NavigationCluster(PedestrianBHM me, Topography topography, Random random) {
+		this.me = me;
+		this.topography = topography;
+		this.attributesBHM = me.getAttributesBHM();
+		this.proximityNavigation = new NavigationProximity(me, random);
+	}
+
+	@Override
+	public VPoint getNavigationPosition() {
+
+		VPoint result;
+
+		Pedestrian futureCollision = me.findCollisionPedestrian(me.computeMovementProjection(), false);
+
+		if (futureCollision != null) {
+			result = evadeCollision(futureCollision);
+		} else {
+			result = me.computeTargetStep();
+		}
+
+		// no good, stay where you are
+		if (me.collidesWithPedestrian(result, 0) || me.collidesWithObstacle(result)) {
+			// result = me.getPosition();
+			result = proximityNavigation.getNavigationPosition();
+		}
+
+		return result;
+	}
+
+	private VPoint evadeCollision(final Pedestrian collisionPed) {
+
+		VPoint result = null;
+
+		List<Pedestrian> cluster = determineCluster(collisionPed);
+		cluster.remove(collisionPed);
+
+		Pedestrian avoidPedestrianMin = collisionPed;
+		Pedestrian avoidPedestrianMax = collisionPed;
+
+		List<VPoint> evasionPoints = getRelativeEvasionPoints(collisionPed);
+
+		// pedestrians are in collision state
+		if (evasionPoints == null) {
+			logger.warn("Collision with pedestrian " + collisionPed.getId() +
+					". Pedestrian " + me.getId() + " stepped away.");
+
+			return me.stepAwayFromCollision(collisionPed);
+		}
+
+		double angleToTarget1 = UtilsBHM.angleTo(me.getTargetDirection(), evasionPoints.get(0).norm());
+		double angleToTarget2 = UtilsBHM.angleTo(me.getTargetDirection(), evasionPoints.get(1).norm());
+
+		double extremeMinAngle, extremeMaxAngle;
+		VPoint extremeMinEvasionPoint, extremeMaxEvasionPoint;
+
+		if (angleToTarget1 < angleToTarget2) {
+			extremeMinAngle = angleToTarget1;
+			extremeMaxAngle = angleToTarget2;
+			extremeMinEvasionPoint = evasionPoints.get(0);
+			extremeMaxEvasionPoint = evasionPoints.get(1);
+		} else {
+			extremeMinAngle = angleToTarget2;
+			extremeMaxAngle = angleToTarget1;
+			extremeMinEvasionPoint = evasionPoints.get(1);
+			extremeMaxEvasionPoint = evasionPoints.get(0);
+		}
+
+		for (Pedestrian ped : cluster) {
+			evasionPoints = getRelativeEvasionPoints(ped);
+
+			// pedestrians are in collision state
+			if (evasionPoints == null) {
+				logger.warn("Collision with pedestrian " + ped.getId() +
+						". Pedestrian " + me.getId() + " stepped away.");
+
+				return me.stepAwayFromCollision(ped);
+			}
+
+			angleToTarget1 = UtilsBHM.angleTo(me.getTargetDirection(), evasionPoints.get(0).norm());
+			angleToTarget2 = UtilsBHM.angleTo(me.getTargetDirection(), evasionPoints.get(1).norm());
+
+			double angleToTargetThisMin, angleToTargetThisMax;
+			VPoint evasionPointMin, evasionPointMax;
+
+			// only the greater angle is a candidate for a new extreme value
+			if (angleToTarget1 < angleToTarget2) {
+				angleToTargetThisMin = angleToTarget1;
+				angleToTargetThisMax = angleToTarget2;
+				evasionPointMin = evasionPoints.get(0);
+				evasionPointMax = evasionPoints.get(1);
+			} else {
+				angleToTargetThisMin = angleToTarget2;
+				angleToTargetThisMax = angleToTarget1;
+				evasionPointMin = evasionPoints.get(1);
+				evasionPointMax = evasionPoints.get(0);
+			}
+
+			if (angleToTargetThisMin < extremeMinAngle) {
+				extremeMinAngle = angleToTargetThisMin;
+				extremeMinEvasionPoint = evasionPointMin;
+				avoidPedestrianMin = ped;
+			}
+			if (angleToTargetThisMax > extremeMaxAngle) {
+				extremeMaxAngle = angleToTargetThisMax;
+				extremeMaxEvasionPoint = evasionPointMax;
+				avoidPedestrianMax = ped;
+			}
+		}
+
+		result = selectClusterDetourStep(extremeMinEvasionPoint, extremeMaxEvasionPoint);
+
+		// DEBUG
+		// logger.info("Pedestrians from cluster to avoid are " + avoidPedestrianMin.getId() + " and
+		// " + avoidPedestrianMax.getId());
+
+		return result;
+	}
+
+	private VPoint selectClusterDetourStep(VPoint evasionPoint1, VPoint evasionPoint2) {
+
+		VPoint result;
+
+		VPoint evasionStep1 = evasionPoint1.norm().scalarMultiply(me.getStepLength()).add(me.getPosition());
+		VPoint evasionStep2 = evasionPoint2.norm().scalarMultiply(me.getStepLength()).add(me.getPosition());
+
+
+		// in case of collision return other
+		if (me.collidesWithObstacle(evasionStep1)) {
+
+			if (me.collidesWithObstacle(evasionStep2)) {
+				// both positions are colliding, fall back to current position
+				return me.getPosition();
+			} else {
+				return evasionStep2;
+			}
+		} else if (me.collidesWithObstacle(evasionStep2)) {
+			return evasionStep1;
+		}
+
+
+		VShape target = topography.getTarget(me.getNextTargetId()).getShape();
+
+		double lastDirectionAngle1 = UtilsBHM.angle(me.getTargetDirection(), evasionPoint1);
+		double lastDirectionAngle2 = UtilsBHM.angle(me.getTargetDirection(), evasionPoint2);
+
+		// compute walking distance to target through evasion points
+		double detour1 = target.distance(me.getPosition().add(evasionPoint1))
+				+ evasionPoint1.distanceToOrigin();
+		double detour2 = target.distance(me.getPosition().add(evasionPoint2))
+				+ evasionPoint2.distanceToOrigin();
+
+
+		// avoid walking backwards
+		if (lastDirectionAngle1 > attributesBHM.getBackwardsAngle()) {
+
+			if (lastDirectionAngle2 > attributesBHM.getBackwardsAngle())
+				if (detour1 < detour2) {
+					result = evasionStep1;
+				} else {
+					result = evasionStep2;
+				}
+			else {
+				result = evasionStep2;
+			}
+		} else if (lastDirectionAngle2 > attributesBHM.getBackwardsAngle()) {
+			result = evasionStep1;
+		} else {
+			// choose shorter detour to target
+			if (detour1 < detour2) {
+				result = evasionStep1;
+			} else {
+				result = evasionStep2;
+			}
+		}
+
+		return result;
+	}
+
+	private List<VPoint> getRelativeEvasionPoints(final Pedestrian collisionPed) {
+
+		List<VPoint> result;
+
+		VPoint relativeThisPosition = me.getPosition().subtract(collisionPed.getPosition());
+		double radius = me.getRadius() + collisionPed.getRadius() + UtilsBHM.DOUBLE_EPSILON;
+
+		// pedestrians are in collision state
+		if (relativeThisPosition.distanceToOrigin() < radius) {
+			result = null;
+
+		} else {
+			List<VPoint> tangentialPoints = UtilsBHM.getTangentialPoints(relativeThisPosition, radius);
+
+			// DEBUG
+			if (Double.isNaN(tangentialPoints.get(0).x) || Double.isNaN(tangentialPoints.get(0).y) ||
+					Double.isNaN(tangentialPoints.get(1).x) || Double.isNaN(tangentialPoints.get(1).y)) {
+
+				logger.error("Tangential point NaN for pedestrian " + collisionPed.getId() + ".");
+			}
+
+			result = UtilsBHM.getRelativeEvasionPointFromTangential(
+					relativeThisPosition, tangentialPoints);
+		}
+
+		return result;
+	}
+
+	private List<Pedestrian> determineCluster(Pedestrian collisionPed) {
+
+		List<Pedestrian> result = new LinkedList<>();
+
+		LinkedList<Pedestrian> contained = new LinkedList<>();
+		List<Pedestrian> notContained;
+		List<Pedestrian> notContainedNext = new LinkedList<>();
+
+
+		for (Pedestrian other : topography.getElements(Pedestrian.class)) {
+
+			// select pedestrians ahead
+			if (UtilsBHM.angleBetweenTarget(me, other) < attributesBHM.getBackwardsAngle()) {
+
+				// skip this and collision pedestrian (the latter is first to be investigated)
+				if (other.getId() != me.getId() && other.getId() != collisionPed.getId()) {
+					notContainedNext.add(other);
+				}
+			}
+		}
+
+		Pedestrian containedNext = collisionPed;
+
+		while (containedNext != null) {
+
+			notContained = notContainedNext;
+			notContainedNext = new LinkedList<>();
+			result.add(containedNext);
+
+			// iterate through all pedestrians outside of the cluster
+			for (Pedestrian pedNotContained : notContained) {
+
+				// add pedestrian to cluster, who is closer to the cluster than this pedestrian's
+				// diameter
+				if (containedNext.getPosition().distance(pedNotContained.getPosition()) < containedNext.getRadius()
+						+ pedNotContained.getRadius() +
+						me.getRadius() * 2 + attributesBHM.getDistanceToKeep()) {
+
+					if (attributesBHM.isOnlyEvadeContraFlow()) {
+
+						double angleBetween = UtilsBHM.angleBetweenTargetDirection(me, pedNotContained);
+
+						if (angleBetween > attributesBHM.getOnlyEvadeContraFlowAngle()) {
+							contained.add(pedNotContained);
+						}
+					} else {
+						contained.add(pedNotContained);
+					}
+				} else {
+					notContainedNext.add(pedNotContained);
+				}
+			}
+
+			if (contained.isEmpty()) {
+				containedNext = null;
+			} else {
+				containedNext = contained.pop();
+			}
+		}
+
+		return result;
+	}
+}

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

+package org.vadere.simulator.models.bhm;
+
+import java.util.LinkedList;
+import java.util.List;
+import java.util.Random;
+
+import org.apache.log4j.LogManager;
+import org.apache.log4j.Logger;
+import org.vadere.state.attributes.models.AttributesBHM;
+import org.vadere.state.scenario.Pedestrian;
+import org.vadere.state.scenario.Topography;
+import org.vadere.util.geometry.shapes.VPoint;
+
+public class NavigationFollower implements Navigation {
+
+	private static Logger logger = LogManager.getLogger(NavigationFollower.class);
+
+	private final Topography topography;
+	private final AttributesBHM attributesBHM;
+	private final PedestrianBHM me;
+	private final NavigationProximity proximityNavigation;
+
+	public NavigationFollower(PedestrianBHM me, Topography topography, Random random) {
+		this.me = me;
+		this.topography = topography;
+		this.attributesBHM = me.getAttributesBHM();
+		this.proximityNavigation = new NavigationProximity(me, random);
+	}
+
+	@Override
+	public VPoint getNavigationPosition() {
+		VPoint result;
+
+		Pedestrian futureCollision = me.findCollisionPedestrian(me.computeMovementProjection(), false);
+
+		if (futureCollision != null) {
+			result = selectFollowingPosition();
+
+			if (result == null) {
+
+				result = proximityNavigation.getNavigationPosition();
+
+			} else if (me.collidesWithObstacle(result) || me.collidesWithPedestrianOnPath(result)) {
+
+				if (attributesBHM.isFollowerProximityNavigation()) {
+					result = proximityNavigation.getNavigationPosition();
+				} else {
+					result = me.getPosition();
+				}
+			}
+		} else {
+
+			result = proximityNavigation.getNavigationPosition();
+		}
+
+		return result;
+	}
+
+	private VPoint selectFollowingPosition() {
+		VPoint result = null;
+		Pedestrian pedestrianToFollow = selectPedestrianToFollow();
+
+		if (pedestrianToFollow != null) {
+			VPoint followDirection = pedestrianToFollow.getPosition().subtract(me.getPosition()).norm();
+			result = followDirection.scalarMultiply(me.getStepLength()).add(me.getPosition());
+		}
+
+		return result;
+	}