Commit d099754c authored by hm-seitz4's avatar hm-seitz4

Add BehaviouralHeuristicsModel.

parent 0b345001
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;
}
}
package org.vadere.simulator.models.bhm;
import org.vadere.util.geometry.shapes.VPoint;
public interface DirectionAddend {
public VPoint getDirectionAddend();
}
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;
}
}
package org.vadere.simulator.models.bhm;
import org.vadere.util.geometry.shapes.VPoint;
public interface Navigation {
public VPoint getNavigationPosition();
}
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;
}
}
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;
}
private Pedestrian selectPedestrianToFollow() {