Commit 6c2cad9b authored by Felix Dietrich's avatar Felix Dietrich
Browse files

Add model ovm/OptimalVelocityModel.java

parent de9267c2
...@@ -6,8 +6,16 @@ import java.util.Iterator; ...@@ -6,8 +6,16 @@ import java.util.Iterator;
import java.util.LinkedList; import java.util.LinkedList;
import org.vadere.state.attributes.scenario.AttributesAgent; import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.attributes.scenario.AttributesCar;
import org.vadere.state.attributes.scenario.AttributesTopography; import org.vadere.state.attributes.scenario.AttributesTopography;
import org.vadere.state.scenario.*; import org.vadere.state.scenario.Obstacle;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.ScenarioElement;
import org.vadere.state.scenario.Source;
import org.vadere.state.scenario.Stairs;
import org.vadere.state.scenario.Target;
import org.vadere.state.scenario.Teleporter;
import org.vadere.state.scenario.Topography;
import org.vadere.util.geometry.shapes.VPoint; import org.vadere.util.geometry.shapes.VPoint;
/** /**
...@@ -33,6 +41,7 @@ public class TopographyBuilder implements Iterable<ScenarioElement> { ...@@ -33,6 +41,7 @@ public class TopographyBuilder implements Iterable<ScenarioElement> {
private LinkedList<ScenarioElement> topographyElements; private LinkedList<ScenarioElement> topographyElements;
private AttributesTopography attributes; private AttributesTopography attributes;
private AttributesAgent attributesPedestrian; private AttributesAgent attributesPedestrian;
private AttributesCar attributesCar;
/** /**
* Default-Constructor that initialize an empty TopographyBuilder. * Default-Constructor that initialize an empty TopographyBuilder.
...@@ -69,6 +78,7 @@ public class TopographyBuilder implements Iterable<ScenarioElement> { ...@@ -69,6 +78,7 @@ public class TopographyBuilder implements Iterable<ScenarioElement> {
} }
attributes = topography.getAttributes(); attributes = topography.getAttributes();
attributesPedestrian = topography.getAttributesPedestrian(); attributesPedestrian = topography.getAttributesPedestrian();
attributesCar = topography.getAttributesCar();
topographyElements = new LinkedList<>(); topographyElements = new LinkedList<>();
topographyElements.addAll(obstacles); topographyElements.addAll(obstacles);
topographyElements.addAll(stairs); topographyElements.addAll(stairs);
...@@ -104,7 +114,7 @@ public class TopographyBuilder implements Iterable<ScenarioElement> { ...@@ -104,7 +114,7 @@ public class TopographyBuilder implements Iterable<ScenarioElement> {
} }
public Topography build() { public Topography build() {
Topography topography = new Topography(attributes, attributesPedestrian); Topography topography = new Topography(attributes, attributesPedestrian, attributesCar);
for (Obstacle obstacle : obstacles) for (Obstacle obstacle : obstacles)
topography.addObstacle(obstacle); topography.addObstacle(obstacle);
......
/**
* This class manages the Equations of the Optimal Velocity Model and computes the
* derivative-equations
*
*/
package org.vadere.simulator.models.ovm;
import java.text.DecimalFormat;
import java.util.ArrayList;
import java.util.Collection;
import java.util.List;
import java.util.Random;
import java.util.concurrent.ExecutionException;
import org.vadere.simulator.models.ode.AbstractModelEquations;
import org.vadere.simulator.models.ode.ODEModel;
import org.vadere.state.attributes.models.AttributesODEIntegrator;
import org.vadere.state.attributes.models.AttributesOVM;
import org.vadere.state.scenario.Car;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Target;
import org.vadere.util.geometry.Vector2D;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.parallel.AParallelWorker;
import org.vadere.util.parallel.CountableParallelWorker;
import org.vadere.util.parallel.IAsyncComputable;
import org.vadere.util.parallel.AParallelWorker.Work;
public class OVMEquations extends AbstractModelEquations<Car> implements IAsyncComputable {
private AttributesOVM attributesOVM;
private double sensitivity;
private double sightDistance;
private double sightDistanceFactor;
private boolean pedestrianInteraction;
private final Random rand = new Random(1); // needed to create ghost cars for a time instant.
/**
* Optimal Velocity Function in the form of the original paper
*
* @param x_n Xn
* @param x_n_1 Xn-1
* @return
*/
public double ovFunction(double x_n, double x_n_1, double speed) {
double dX = Math.max(0, x_n_1 - x_n);
return speed / 2 * (Math.tanh((8 * dX) / (3.6 * speed) - 2) + Math.tanh(2));
}
@Override
public void computeDerivatives(double t, double[] x, double[] xdot) {
// fetch attributes of the model
sensitivity = attributesOVM.getSensitivity();
sightDistance = attributesOVM.getSightDistance();
sightDistanceFactor = attributesOVM.getSightDistanceFactor();
// update position of all dynamic elements
ODEModel.updateElementPositions(Car.class, t, topography, this, x);
// create workers for each pedestrian
List<AParallelWorker<Double[]>> workers = new ArrayList<AParallelWorker<Double[]>>();
// loop over all persons and compute the next step
int personCounter = 0; // used for arrays, not identical to personID!
for (final Car car : elements) {
AParallelWorker<Double[]> w = new CountableParallelWorker<Double[]>(
personCounter, new Work<Double[]>() {
private int ID;
@Override
public Double[] call() throws Exception {
computeSingleCarParallel(car, getWorkerID(), t, x, xdot);
return null;
}
@Override
public void setID(int ID) {
this.ID = ID;
}
@Override
public int getWorkerID() {
return this.ID;
}
});
personCounter++;
workers.add(w);
w.start();
}
// finish the work
for (AParallelWorker<Double[]> worker : workers) {
try {
worker.finish();
} catch (ExecutionException e) {
e.printStackTrace();
} catch (InterruptedException e) {
// Necessary in order to tell Simulation the thread has been
// interrupted.
// TODO [priority=medium] [task=refactoring] Hack, other solution?
Thread.currentThread().interrupt();
break;
}
}
}
private void computeSingleCarParallel(
Car currentCar, int carIdInArray, double t,
double[] x, double[] xdot) {
int fCI = -1;
Car nearestCar = null;
List<Car> neighbors = topography.getSpatialMap(Car.class).getObjects(currentCar.getPosition(), sightDistance);
if (topography.hasTeleporter()) {
Vector2D tpoint = topography.getTeleporter().getTeleporterShift();
List<Car> ghostCars =
topography.getSpatialMap(Car.class).getObjects(currentCar.getPosition().add(tpoint), sightDistance);
for (Car ghost : ghostCars) {
Car repositionedGhost = new Car(ghost.getAttributes(), rand);
repositionedGhost.setPosition(
ghost.getPosition().add(topography.getTeleporter().getTeleporterShift().multiply(-1)));
neighbors.add(repositionedGhost);
}
}
for (int j = 0; j < neighbors.size(); j++) {
Car car = neighbors.get(j);
if (nearestCar == null) {
if (car != currentCar && isInFront(car, currentCar)) {
nearestCar = car;
fCI = j;
}
} else {
if (car != currentCar &&
isInFront(car, currentCar) &&
isInFront(nearestCar, car)) {
nearestCar = car;
fCI = j;
}
}
}
// Check Pedestrians if pedestrianIteraction is turned on
if (pedestrianInteraction) {
// TODO [priority=medium] [task=bugfix] [Error?]
}
// evaluate the front car with respect to a limited sight distance
/*
* List<Car> neighbors =
* topography.getSpatialMap(Car.class).getObjects(actualCar.getPosition(), sightDistance);
* for(Car car:neighbors){
* if (nearestCar == null){
* nearestCar=car;
* } else {
* if (car.getPosition().getX() > actualCar.getPosition().getX()
* && car.getPosition().getX() < nearestCar.getPosition().getX()){
* nearestCar=car;
* }
* }
* }
*/
computeSingleCar(currentCar, nearestCar, t, x, xdot, carIdInArray, fCI);
}
private boolean isInFront(Car car, Car currentCar) {
if (this.attributesOVM.isIgnoreOtherCars())
return false;
if (currentCar.hasNextTarget() && car.hasNextTarget()) {
// TODO [priority=low] [task=feature] check that the cars actually stop when another
// car is in front
if (currentCar.getNextTargetId() != car.getNextTargetId()) {
return false;
}
Target target = topography.getTarget(currentCar.getNextTargetId());
if (target == null) {
System.out.println(currentCar.getNextTargetId());
}
double distanceToThis = target.getShape().distance(currentCar.getPosition());
double distanceToOther = target.getShape().distance(car.getPosition());
return distanceToOther < distanceToThis;
} else {
return car.getPosition().getX() >= currentCar.getPosition().getX();
}
}
/**
* Computation of speed and position of a single car for one time step
*
* @param currentCar
* @param frontCar
* @param t
* @param x
* @param xdot
* @param index
*/
private void computeSingleCar(Car currentCar, Car frontCar, double t, double[] x, double[] xdot, int index,
int fCI) {
double[] position = new double[2];
double[] speed = new double[2];
double[] position2 = new double[2];
getPosition(index, x, position2);
getVelocity(index, x, speed);
VPoint myPos = new VPoint(position2[0], position2[1]);
Vector2D myVelocity = new Vector2D(speed[0], speed[1]);
double mySpeed = myVelocity.x; // only the x coordinate is set. Do NOT use "length" here, otherwise negative speeds occur.
// velocity is just passed through, but rotated so that the vector points in target
// direction
if (currentCar.hasNextTarget()) {
position[0] = myVelocity.x;
position[1] = myVelocity.y;
}
// also change the direction of the acceleration so that the velocity for each car is
// correctly set.
if (currentCar.hasNextTarget()) {
Target target = topography.getTarget(currentCar.getNextTargetId());
VPoint closest = new Vector2D(target.getShape().getBounds2D().getCenterX(),
target.getShape().getBounds2D().getCenterY());
double localspeed = mySpeed;
Vector2D direction = new Vector2D(closest.add(myPos.scalarMultiply(-1))).normalize(localspeed);
position[0] = direction.x;
position[1] = direction.y;
}
if (frontCar == null) {
double newTargetX = sightDistance * sightDistanceFactor;
if (currentCar.hasNextTarget()) // only one target left (?)
{
Target target = topography.getTarget(currentCar.getNextTargetId());
if (target == null) {
System.out.println(currentCar.getNextTargetId());
}
if (target.getShape().distance(myPos) < sightDistance) {
newTargetX = myPos.distance(new VPoint(target.getShape().getBounds2D().getCenterX(),
target.getShape().getBounds2D().getCenterY()));
}
}
speed[0] = sensitivity * ovFunction(0.0, newTargetX, currentCar.getFreeFlowSpeed()) - sensitivity * mySpeed;
} else {
speed[0] = sensitivity * ovFunction(0.0,
myPos.distance(frontCar.getPosition()) - currentCar.getAttributes().getLength() * 2,
currentCar.getFreeFlowSpeed()) - sensitivity * myVelocity.getLength();
}
speed[1] = 0;
setPosition(index, xdot, position);
setVelocity(index, xdot, speed);
}
@Override
/**
* Dimension per Car
* 4 parameters: position (x,y), velocity (x,y)
*/
protected int dimensionPerPerson() {
return 4;
}
public void setVelocity(int personID, double[] solution, double[] velocity) {
solution[personID * dimensionPerPerson() + 2] = velocity[0];
solution[personID * dimensionPerPerson() + 3] = velocity[1];
}
@Override
public void getVelocity(int personID, double[] solution, double[] velocity) {
velocity[0] = solution[personID * dimensionPerPerson() + 2];
velocity[1] = solution[personID * dimensionPerPerson() + 3];
}
public void setModelAttributes(AttributesOVM attributes) {
this.attributesOVM = attributes;
}
public void setPedestrianInteraction(boolean pedestrianInteraction) {
this.pedestrianInteraction = pedestrianInteraction;
}
}
/**
* This class implements the optimal velocity model for car traffic
*
*/
package org.vadere.simulator.models.ovm;
import java.util.Collection;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Random;
import org.vadere.simulator.control.ActiveCallback;
import org.vadere.simulator.models.Model;
import org.vadere.simulator.models.ode.IntegratorFactory;
import org.vadere.simulator.models.ode.ODEModel;
import org.vadere.state.attributes.Attributes;
import org.vadere.state.attributes.models.AttributesOVM;
import org.vadere.state.attributes.scenario.AttributesAgent;
import org.vadere.state.attributes.scenario.AttributesCar;
import org.vadere.state.scenario.Agent;
import org.vadere.state.scenario.Car;
import org.vadere.state.scenario.DynamicElement;
import org.vadere.state.scenario.Pedestrian;
import org.vadere.state.scenario.Topography;
import org.vadere.util.data.Table;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.parallel.ParallelWorkerUtil;
public class OptimalVelocityModel extends ODEModel<Car, AttributesCar> {
private int carIdCounter = 10000000; // TODO [priority=low] [task=refactoring] hack, think about another way of separating car IDs and pedestrian IDs.
private AttributesOVM attributesOVM;
private OVMEquations ovmEquations;
private List<ActiveCallback> activeCallbacks;
/**
* Constructor for OptimalVelocityModel used in the ModelCreator
*
* @param scenario
* @param ovmEquations
* @param attributesOVM
* @param elementAttributes
* @param random
*/
@Deprecated
public OptimalVelocityModel(Topography scenario,
OVMEquations ovmEquations,
AttributesOVM attributesOVM,
AttributesCar elementAttributes,
boolean pedestrianInteraction,
Random random) {
super(Car.class, scenario,
IntegratorFactory.createFirstOrderIntegrator(attributesOVM.getAttributesODEIntegrator()),
ovmEquations,
elementAttributes,
random);
this.attributesOVM = attributesOVM;
this.ovmEquations = ovmEquations;
ovmEquations.setPedestrianInteraction(pedestrianInteraction);
ovmEquations.setModelAttributes(attributesOVM);
ovmEquations.setGradients(null, null, null, scenario);
}
public OptimalVelocityModel() { }
@Override
public void initialize(List<Attributes> modelAttributesList, Topography topography,
AttributesAgent attributesPedestrian, Random random) {
this.attributesOVM = Model.findAttributes(modelAttributesList, AttributesOVM.class);
AttributesCar elementAttributes = topography.getAttributesCar();// Model.findAttributes(modelAttributesList, AttributesCar.class);
this.ovmEquations = new OVMEquations();
super.initializeODEModel(Car.class,
IntegratorFactory.createFirstOrderIntegrator(
attributesOVM.getAttributesODEIntegrator()),
ovmEquations, elementAttributes, topography, random);
ovmEquations.setPedestrianInteraction(false);
ovmEquations.setModelAttributes(attributesOVM);
ovmEquations.setGradients(null, null, null, topography);
activeCallbacks = Collections.singletonList(this);
}
@Override
/**
* Creates a single car with given attributes
*
* @param store
* @return single Car-Object
*/
public <T extends DynamicElement> Agent createElement(VPoint position, int id, Class<T> type) {
if (!Car.class.isAssignableFrom(type))
throw new IllegalArgumentException("OVM cannot initialize " + type.getCanonicalName());
carIdCounter++;
AttributesCar carAttributes = new AttributesCar(elementAttributes, id > 0 ? id : carIdCounter);
Car result = new Car(carAttributes, random);
result.setPosition(position);
// result.setVelocity(result.getCarAttrributes().getDirection());
return result;
}
@Override
public void preLoop(final double simTimeInSec) {
super.preLoop(simTimeInSec);
// setup thread pool if it is not setup already
int WORKERS_COUNT = 16;// pedestrians.keySet().size();
ParallelWorkerUtil.setup(WORKERS_COUNT);
}
@Override
public void postLoop(final double simTimeInSec) {
super.postLoop(simTimeInSec);
}
@Override
public void update(final double simTimeInSec) {
// Get all cars in the topography
Collection<Car> cars = topography.getElements(Car.class);
// Give cars to the OVMEquations-Object
ovmEquations.setElements(cars);
super.update(simTimeInSec);
}
/**
* @return the attributesOVM
*/
public AttributesOVM getAttributesOVM() {
return attributesOVM;
}
@Override
public List<ActiveCallback> getActiveCallbacks() {
return activeCallbacks;
}
}
...@@ -191,7 +191,7 @@ public abstract class JsonConverter { ...@@ -191,7 +191,7 @@ public abstract class JsonConverter {
private static class TopographyStore { private static class TopographyStore {
AttributesTopography attributes = new AttributesTopography(); AttributesTopography attributes = new AttributesTopography();
AttributesAgent attributesPedestrian = new AttributesAgent(); AttributesAgent attributesPedestrian = new AttributesAgent();
AttributesCar attributesCar; AttributesCar attributesCar = new AttributesCar();
Collection<AttributesObstacle> obstacles = new LinkedList<>(); Collection<AttributesObstacle> obstacles = new LinkedList<>();
Collection<AttributesStairs> stairs = new LinkedList<>(); Collection<AttributesStairs> stairs = new LinkedList<>();
Collection<AttributesTarget> targets = new LinkedList<>(); Collection<AttributesTarget> targets = new LinkedList<>();
......
...@@ -58,13 +58,9 @@ public class Topography { ...@@ -58,13 +58,9 @@ public class Topography {
public Topography(AttributesTopography attributes, AttributesAgent attributesPedestrian, public Topography(AttributesTopography attributes, AttributesAgent attributesPedestrian,
AttributesCar attributesCar) { AttributesCar attributesCar) {
this(attributes, attributesPedestrian);
this.attributesCar = attributesCar;
}
public Topography(AttributesTopography attributes, AttributesAgent attributesPedestrian) {
this.attributes = attributes; this.attributes = attributes;
this.attributesPedestrian = attributesPedestrian; this.attributesPedestrian = attributesPedestrian;
this.attributesCar = attributesCar;
this.obstacles = new LinkedList<>(); this.obstacles = new LinkedList<>();
this.stairs = new LinkedList<>(); this.stairs = new LinkedList<>();
this.sources = new LinkedList<>(); this.sources = new LinkedList<>();
...@@ -81,7 +77,7 @@ public class Topography { ...@@ -81,7 +77,7 @@ public class Topography {
* Creates an empty scenario where bounds and finishTime are empty / zero. * Creates an empty scenario where bounds and finishTime are empty / zero.
*/ */
public Topography() { public Topography() {
this(new AttributesTopography(), new AttributesAgent()); this(new AttributesTopography(), new AttributesAgent(), new AttributesCar());
} }
public Rectangle2D.Double getBounds() { public Rectangle2D.Double getBounds() {
...@@ -303,8 +299,7 @@ public class Topography { ...@@ -303,8 +299,7 @@ public class Topography {
*/ */
@Override @Override
public Topography clone() { public Topography clone() {
Topography s = new Topography(this.attributes, this.attributesPedestrian); Topography s = new Topography(this.attributes, this.attributesPedestrian, this.attributesCar);
s.attributesCar = this.attributesCar;
for (Obstacle obstacle : this.getObstacles()) { for (Obstacle obstacle : this.getObstacles()) {
if (this.boundaryObstacles.contains(obstacle)) if (this.boundaryObstacles.contains(obstacle))
......
Supports Markdown
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