Add ReynoldsSteeringModel (not testet yet)

VadereSimulator/src/org/vadere/simulator/models/reynolds/PedestrianReynolds.java

+package org.vadere.simulator.models.reynolds;
+
+import java.util.Random;
+
+import org.vadere.state.attributes.scenario.AttributesAgent;
+import org.vadere.state.scenario.Pedestrian;
+import org.vadere.util.geometry.Vector2D;
+import org.vadere.util.geometry.shapes.VPoint;
+
+public class PedestrianReynolds extends Pedestrian {
+
+	private Vector2D lastMovement;
+	private double startTime;
+
+	public PedestrianReynolds(AttributesAgent attributesPedestrian, Random random) {
+		super(attributesPedestrian, random);
+
+		this.lastMovement = new Vector2D(0, 0);
+		this.startTime = -1;
+	}
+
+	public VPoint getLastMovement() {
+		return lastMovement;
+	}
+
+	public double getStartTime() {
+		return startTime;
+	}
+
+	public void move(double simTime, Vector2D mov) {
+		lastMovement = mov;
+
+		if (startTime < 0) {
+			startTime = simTime;
+		}
+
+		this.setPosition(this.getPosition().add(mov));
+	}
+
+}

VadereSimulator/src/org/vadere/simulator/models/reynolds/ReynoldsSteeringModel.java

+package org.vadere.simulator.models.reynolds;
+
+import java.util.Collection;
+import java.util.Collections;
+import java.util.Iterator;
+import java.util.List;
+import java.util.Random;
+
+import org.vadere.simulator.models.MainModel;
+import org.vadere.simulator.models.Model;
+import org.vadere.simulator.models.reynolds.behaviour.CollisionAvoidance;
+import org.vadere.simulator.models.reynolds.behaviour.Containment;
+import org.vadere.simulator.models.reynolds.behaviour.Seek;
+import org.vadere.simulator.models.reynolds.behaviour.Separation;
+import org.vadere.simulator.models.reynolds.behaviour.WallAvoidance;
+import org.vadere.simulator.models.reynolds.behaviour.Wander;
+import org.vadere.state.attributes.Attributes;
+import org.vadere.state.attributes.models.AttributesReynolds;
+import org.vadere.state.attributes.scenario.AttributesAgent;
+import org.vadere.state.scenario.DynamicElement;
+import org.vadere.state.scenario.Pedestrian;
+import org.vadere.state.scenario.Topography;
+import org.vadere.util.geometry.Vector2D;
+import org.vadere.util.geometry.shapes.VPoint;
+
+public class ReynoldsSteeringModel implements MainModel {
+
+	private AttributesReynolds attributesReynolds;
+	private AttributesAgent attributesPedestrian;
+	private Random random;
+	private Topography topography;
+	private int pedestrianIdCounter;
+
+	private Seek bSeek;
+	private Separation bSeparation;
+	private Containment bContainment;
+	private CollisionAvoidance bCollisionAvoidance;
+	private WallAvoidance bWallAvoidance;
+	private Wander bWander;
+	private List<Model> submodels;
+
+	public ReynoldsSteeringModel() {
+		this.pedestrianIdCounter = 0;
+		this.bSeek = new Seek(this);
+		this.bSeparation = new Separation(this);
+		this.bContainment = new Containment(this);
+		this.bCollisionAvoidance = new CollisionAvoidance(this);
+		this.bWallAvoidance = new WallAvoidance(this);
+		this.bWander = new Wander(this);
+	}
+
+	@Override
+	public void initialize(List<Attributes> modelAttributesList, Topography topography,
+			AttributesAgent attributesPedestrian, Random random) {
+
+		this.attributesReynolds = Model.findAttributes(modelAttributesList, AttributesReynolds.class);
+		this.attributesPedestrian = attributesPedestrian;
+		this.topography = topography;
+		this.random = random;
+
+		submodels = Collections.singletonList(this);
+
+	}
+
+	@Override
+	public void preLoop(final double simTimeInSec) {}
+
+	@Override
+	public void postLoop(final double simTimeInSec) {}
+
+	@Override
+	public void update(final double simTimeInSec) {
+		Collection<Pedestrian> pedestrians = topography.getElements(Pedestrian.class);
+		Iterator<Pedestrian> it = pedestrians.iterator();
+		double maxSpeed = 3;
+
+		for (; it.hasNext();) {
+			PedestrianReynolds ped = (PedestrianReynolds) it.next();
+			Vector2D mov = new Vector2D(0, 0);
+
+			mov = mov.add(bSeek.nextStep(simTimeInSec, mov, ped));
+			mov = mov.add(bWander.nextStep(simTimeInSec, mov, ped));
+			mov = mov.add(bCollisionAvoidance.nextStep(simTimeInSec, mov, ped));
+			mov = mov.add(bWallAvoidance.nextStep(simTimeInSec, mov, ped));
+			mov = mov.add(bSeparation.nextStep(simTimeInSec, mov, ped));
+			mov = mov.add(bContainment.nextStep(simTimeInSec, mov, ped));
+
+			// if movement is faster than max speed,
+			// no normal movement is available, skip this turn.
+			if (mov.getLength() > maxSpeed) {
+				mov = new Vector2D(0, 0);
+			}
+
+			ped.move(simTimeInSec, mov);
+		}
+	}
+
+	public Topography getScenario() {
+		return this.topography;
+	}
+
+	public AttributesAgent getAttributesPedestrian() {
+		return this.attributesPedestrian;
+	}
+
+	public AttributesReynolds getAttributesReynolds() {
+		return this.attributesReynolds;
+	}
+
+	@Override
+	public <T extends DynamicElement> Pedestrian createElement(VPoint position, int id, Class<T> type) {
+		if (!Pedestrian.class.isAssignableFrom(type))
+			throw new IllegalArgumentException("RSM cannot initialize " + type.getCanonicalName());
+
+		this.pedestrianIdCounter++;
+		AttributesAgent pedAttributes = new AttributesAgent(
+				attributesPedestrian, id > 0 ? id : pedestrianIdCounter);
+		Pedestrian result = new PedestrianReynolds(pedAttributes, random);
+		result.setPosition(position);
+		return result;
+	}
+
+	@Override
+	public List<Model> getSubmodels() {
+		return submodels;
+	}
+
+}

VadereSimulator/src/org/vadere/simulator/models/reynolds/behaviour/CollisionAvoidance.java

+package org.vadere.simulator.models.reynolds.behaviour;
+
+import java.util.Collection;
+import java.util.Iterator;
+
+import org.vadere.simulator.models.reynolds.PedestrianReynolds;
+import org.vadere.simulator.models.reynolds.ReynoldsSteeringModel;
+import org.vadere.state.scenario.Pedestrian;
+import org.vadere.util.geometry.Vector2D;
+
+/**
+ * Take the current movement and shorten it, if a collision would occur.
+ * 
+ */
+public class CollisionAvoidance {
+
+	private ReynoldsSteeringModel model;
+
+	public CollisionAvoidance(ReynoldsSteeringModel model) {
+		this.model = model;
+	}
+
+	public Vector2D nextStep(double simTime, Vector2D currentMov, PedestrianReynolds self) {
+		double radius = self.getAttributes().getRadius();
+
+		Collection<Pedestrian> peds = model.getScenario().getElements(Pedestrian.class);
+		Iterator<Pedestrian> it = peds.iterator();
+		Pedestrian p;
+
+		Vector2D toNeighbor;
+		Vector2D proj;
+		Vector2D norm;
+
+		// Only avoid collisions, which would occur in front of us.
+		Vector2D pos = new Vector2D(self.getPosition().add(currentMov.normalize(radius)));
+		Vector2D mov = currentMov.sub(currentMov.normalize(radius));
+
+		while (it.hasNext()) {
+			p = it.next();
+			if (p.getId() == self.getId()) {
+				continue;
+			}
+
+			// get vector from pedestrian to neighbor
+			toNeighbor = new Vector2D(p.getPosition().subtract(pos));
+
+			// skip neighbor farer away than current movement
+			if (toNeighbor.getLength() > mov.getLength()) {
+				continue;
+			}
+
+			// project toNeighbor to current movement
+			proj = mov.multiply(
+					(mov.x * toNeighbor.x + mov.y * toNeighbor.y)
+							/ (Math.pow(mov.x, 2) + Math.pow(mov.y, 2)));
+
+			// if projection has different leading sign than current movement, our neighbor is
+			// behind us.
+			if ((proj.x < 0 && mov.x > 0 || proj.x > 0 && mov.x < 0) ||
+					(proj.y < 0 && mov.y > 0 || proj.y > 0 && mov.y < 0)) {
+				continue;
+			}
+
+			// get normal from neighbor to current movement
+			// if it is more than twice the pedestrian radius, the neighbor
+			// is not in our way, continue
+			norm = toNeighbor.sub(proj);
+			if (norm.getLength() > 2 * radius) {
+				continue;
+			}
+
+			// if projection is shorter than twice the ped radius, our neighbor is too
+			// close, we have to skip our current movement
+			if (proj.getLength() < 2 * radius) {
+				return mov.multiply(-1);
+			}
+
+			// return difference between new and old movement as our correction vector
+			return proj.sub(mov);
+		}
+
+		// No correction needed
+		return new Vector2D(0, 0);
+	}
+
+}

VadereSimulator/src/org/vadere/simulator/models/reynolds/behaviour/Containment.java

+package org.vadere.simulator.models.reynolds.behaviour;
+
+import java.util.Iterator;
+import java.util.List;
+
+import org.vadere.simulator.models.reynolds.PedestrianReynolds;
+import org.vadere.simulator.models.reynolds.ReynoldsSteeringModel;
+import org.vadere.state.scenario.Obstacle;
+import org.vadere.util.geometry.Vector2D;
+
+public class Containment {
+
+	private ReynoldsSteeringModel model;
+
+	public Containment(ReynoldsSteeringModel model) {
+		this.model = model;
+	}
+
+	public Vector2D nextStep(double simTime, Vector2D currentMov, PedestrianReynolds self) {
+		double radius = self.getAttributes().getRadius();
+
+		List<Obstacle> obstacles = model.getScenario().getObstacles();
+		Iterator<Obstacle> it = obstacles.iterator();
+		Obstacle o;
+
+		Vector2D mov = new Vector2D(0, 0);
+		Vector2D nextPos = new Vector2D(self.getPosition()).add(currentMov);
+
+		// iterate over every obstacle o(n^2)
+		while (it.hasNext()) {
+			o = it.next();
+
+			// if we are already inside an obstacle, try to escape by negating the current movement
+			// this will also work by negating the last movement, maybe better imo
+			if (o.getShape().contains(self.getPosition())) {
+				return currentMov.multiply(-2);
+			}
+
+			// get distance from itself to the closest point of the obstacle
+			Vector2D dist = new Vector2D(o.getShape().closestPoint(self.getPosition())).sub(self.getPosition());
+
+			// get distance from the next position to the closest point of the obstacle
+			Vector2D nextDist = new Vector2D(o.getShape().closestPoint(nextPos)).sub(nextPos);
+
+			// if the pedestrian does not end in the obstacle with his next movement, ignore
+			// obstacle
+			if (!o.getShape().contains(nextPos) && nextDist.getLength() > 1.05 * radius) {
+				continue;
+			}
+
+			// project movement onto distance from position to obstacle
+			Vector2D proj = dist.multiply(
+					(dist.x * currentMov.x + dist.y * currentMov.y)
+							/ (Math.pow(dist.x, 2) + Math.pow(dist.y, 2)));
+
+			// calculate correction vector to avoid walking into the obstacle
+			if (dist.getLength() < proj.getLength()) {
+				mov = dist.sub(proj);
+				mov = mov.normalize(mov.getLength() + 1.05 * radius);
+			} else if (dist.getLength() > proj.getLength()) {
+				mov = proj.sub(dist);
+				mov = mov.normalize(mov.getLength() + 1.05 * radius);
+			} else {
+				mov = dist.multiply(-1).normalize(1.05 * radius);
+			}
+
+			return mov;
+		}
+
+		return new Vector2D(0, 0);
+	}
+
+}

VadereSimulator/src/org/vadere/simulator/models/reynolds/behaviour/Seek.java

+package org.vadere.simulator.models.reynolds.behaviour;
+
+import org.vadere.simulator.models.reynolds.PedestrianReynolds;
+import org.vadere.simulator.models.reynolds.ReynoldsSteeringModel;
+import org.vadere.util.geometry.Vector2D;
+import org.vadere.util.geometry.shapes.VPoint;
+
+public class Seek {
+
+	private ReynoldsSteeringModel model;
+
+	public Seek(ReynoldsSteeringModel model) {
+		this.model = model;
+	}
+
+	public Vector2D nextStep(double simTime, Vector2D currentMov, PedestrianReynolds ped) {
+		double maxSpeed = ped.getAttributes().getSpeedDistributionMean();
+		double simTimeStepLength = 0.4; // TODO [priority=low] [task=refactoring] get this attribute from AttributePedestrians
+
+		VPoint pos = ped.getPosition();
+		// VPoint target = model.getScenario().getTargets().get(0).getShape().closestPoint(pos);
+		VPoint target = model.getScenario().getTarget(ped.getTargets().getFirst()).getShape().closestPoint(pos);
+		Vector2D toTarget = new Vector2D(target.subtract(pos));
+		Vector2D mov = toTarget.clone();
+
+		// Cap to max speed
+		mov = mov.normalize(maxSpeed);
+
+		// Accelerate
+		double startTime = ped.getStartTime();
+		double pastTime = startTime < 0 ? 0 : simTime - ped.getStartTime();
+		if (pastTime < simTimeStepLength + simTimeStepLength * 0.1) {
+			mov.normalize(maxSpeed / 2);
+		}
+
+		// Arrive
+		double stepsUntilTarget = 3;
+		double distToTarget = toTarget.getLength();
+		double slowed;
+		if (distToTarget < maxSpeed * stepsUntilTarget) {
+			slowed = distToTarget / stepsUntilTarget + maxSpeed * 0.1;
+			if (mov.getLength() > slowed) {
+				mov = mov.normalize(slowed);
+			}
+		}
+
+		return mov;
+	}
+
+}

VadereSimulator/src/org/vadere/simulator/models/reynolds/behaviour/Separation.java

+package org.vadere.simulator.models.reynolds.behaviour;
+
+import java.util.Collection;
+import java.util.Iterator;
+
+import org.vadere.simulator.models.reynolds.PedestrianReynolds;
+import org.vadere.simulator.models.reynolds.ReynoldsSteeringModel;
+import org.vadere.state.scenario.Pedestrian;
+import org.vadere.util.geometry.Vector2D;
+
+public class Separation {
+
+	private ReynoldsSteeringModel model;
+
+	public Separation(ReynoldsSteeringModel model) {
+		this.model = model;
+	}
+
+	public Vector2D nextStep(double simTime, Vector2D currentMov, PedestrianReynolds self) {
+		double radius = self.getAttributes().getRadius();
+
+		Collection<Pedestrian> peds = model.getScenario().getElements(Pedestrian.class);
+		Iterator<Pedestrian> it = peds.iterator();
+		Pedestrian p;
+
+		Vector2D mov = new Vector2D(0, 0);
+		Vector2D nextPos = new Vector2D(self.getPosition()).add(currentMov);
+
+		// iterate over every neighbor, o(n^2)
+		while (it.hasNext()) {
+			p = it.next();
+
+			// skip itself
+			if (p.getId() == self.getId()) {
+				continue;
+			}
+
+			// distance to the neighbor, from the future position
+			Vector2D dist = new Vector2D(p.getPosition().subtract(nextPos));
+
+			// if neighbor is too far every, ignore him
+			if (dist.getLength() >= 2 * radius) {
+				continue;
+			}
+
+			// calculate correction vector, to avoid collision
+			dist = dist.normalize(2.1 * radius - dist.getLength()).multiply(-1);
+
+			// add to movement
+			nextPos = nextPos.add(dist);
+			mov = mov.add(dist);
+		}
+
+		// This will iterate over every neighbor only once and will thus not be able to find
+		// the best, available spot for the next movement. This behavior is based on the
+		// assumption, that the decisions of the direction of the human movement are often
+		// not perfect and if the agent does not find a good spot in a decent amount of time,
+		// it will wait (skip the turn) and try again.
+
+		// If still in collision, negate current movement and stand still
+		it = peds.iterator();
+		while (it.hasNext()) {
+			p = it.next();
+			if (p.getId() == self.getId()) {
+				continue;
+			}
+
+			Vector2D dist = new Vector2D(p.getPosition().subtract(nextPos));
+			if (dist.getLength() < 2 * radius) {
+				return currentMov.multiply(-1);
+			}
+		}
+
+		return mov;
+	}
+
+}

VadereSimulator/src/org/vadere/simulator/models/reynolds/behaviour/WallAvoidance.java

+package org.vadere.simulator.models.reynolds.behaviour;
+
+import org.vadere.simulator.models.reynolds.PedestrianReynolds;
+import org.vadere.simulator.models.reynolds.ReynoldsSteeringModel;
+import org.vadere.util.geometry.Vector2D;
+
+public class WallAvoidance {
+
+	private ReynoldsSteeringModel model;
+
+	public WallAvoidance(ReynoldsSteeringModel model) {
+		this.model = model;
+	}
+
+	public Vector2D nextStep(double simTime, Vector2D currentMov, PedestrianReynolds ped) {
+		return new Vector2D(0, 0);
+	}
+
+}

VadereSimulator/src/org/vadere/simulator/models/reynolds/behaviour/Wander.java

+package org.vadere.simulator.models.reynolds.behaviour;
+
+import org.vadere.simulator.models.reynolds.PedestrianReynolds;
+import org.vadere.simulator.models.reynolds.ReynoldsSteeringModel;
+import org.vadere.util.geometry.Vector2D;
+
+public class Wander {
+
+	private ReynoldsSteeringModel model;
+
+	public Wander(ReynoldsSteeringModel model) {
+		this.model = model;
+	}
+
+	public Vector2D nextStep(double simTime, Vector2D currentMov, PedestrianReynolds ped) {
+		return new Vector2D(0, 0);
+	}
+
+}