Commit 9ad2f983 authored by Jakob Schöttl's avatar Jakob Schöttl

Add ReynoldsSteeringModel (not testet yet)

parent 09caf4a1
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));
}
}
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;
}
}
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);
}
}
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);
}
}
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;
}
}
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;
}
}
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);
}
}
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);
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment