Currently job artifacts in CI/CD pipelines on LRZ GitLab never expire. Starting from Wed 26.1.2022 the default expiration time will be 30 days (GitLab default). Currently existing artifacts in already completed jobs will not be affected by the change. The latest artifacts for all jobs in the latest successful pipelines will be kept. More information: https://gitlab.lrz.de/help/user/admin_area/settings/continuous_integration.html#default-artifacts-expiration

Commit d099754c authored by Michael Seitz's avatar Michael Seitz
Browse files

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