Add model ovm/OptimalVelocityModel.java

VadereGui/src/org/vadere/gui/topographycreator/model/TopographyBuilder.java

@@ -6,8 +6,16 @@ import java.util.Iterator;
 import java.util.LinkedList;
 
 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.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;
 
 /**
@@ -33,6 +41,7 @@ public class TopographyBuilder implements Iterable<ScenarioElement> {
 	private LinkedList<ScenarioElement> topographyElements;
 	private AttributesTopography attributes;
 	private AttributesAgent attributesPedestrian;
+	private AttributesCar attributesCar;
 
 	/**
 	 * Default-Constructor that initialize an empty TopographyBuilder.
@@ -69,6 +78,7 @@ public class TopographyBuilder implements Iterable<ScenarioElement> {
 		}
 		attributes = topography.getAttributes();
 		attributesPedestrian = topography.getAttributesPedestrian();
+		attributesCar = topography.getAttributesCar();
 		topographyElements = new LinkedList<>();
 		topographyElements.addAll(obstacles);
 		topographyElements.addAll(stairs);
@@ -104,7 +114,7 @@ public class TopographyBuilder implements Iterable<ScenarioElement> {
 	}
 
 	public Topography build() {
-		Topography topography = new Topography(attributes, attributesPedestrian);
+		Topography topography = new Topography(attributes, attributesPedestrian, attributesCar);
 
 		for (Obstacle obstacle : obstacles)
 			topography.addObstacle(obstacle);

VadereSimulator/src/org/vadere/simulator/models/ovm/OVMEquations.java

+/**
+ * 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;
+
+	}
+
+}

VadereSimulator/src/org/vadere/simulator/models/ovm/OptimalVelocityModel.java

+/**
+ * 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;
+	}
+
+}

VadereSimulator/src/org/vadere/simulator/projects/io/JsonConverter.java

@@ -191,7 +191,7 @@ public abstract class JsonConverter {
 	private static class TopographyStore {
 		AttributesTopography attributes = new AttributesTopography();
 		AttributesAgent attributesPedestrian = new AttributesAgent();
-		AttributesCar attributesCar;
+		AttributesCar attributesCar = new AttributesCar();
 		Collection<AttributesObstacle> obstacles = new LinkedList<>();
 		Collection<AttributesStairs> stairs = new LinkedList<>();
 		Collection<AttributesTarget> targets = new LinkedList<>();

VadereState/src/org/vadere/state/scenario/Topography.java

@@ -58,13 +58,9 @@ public class Topography {
 
 	public Topography(AttributesTopography attributes, AttributesAgent attributesPedestrian,
 			AttributesCar attributesCar) {
-		this(attributes, attributesPedestrian);
-		this.attributesCar = attributesCar;
-	}
-
-	public Topography(AttributesTopography attributes, AttributesAgent attributesPedestrian) {
 		this.attributes = attributes;
 		this.attributesPedestrian = attributesPedestrian;
+		this.attributesCar = attributesCar;
 		this.obstacles = new LinkedList<>();
 		this.stairs = new LinkedList<>();
 		this.sources = new LinkedList<>();
@@ -81,7 +77,7 @@ public class Topography {
 	 * Creates an empty scenario where bounds and finishTime are empty / zero.
 	 */
 	public Topography() {
-		this(new AttributesTopography(), new AttributesAgent());
+		this(new AttributesTopography(), new AttributesAgent(), new AttributesCar());
 	}
 
 	public Rectangle2D.Double getBounds() {
@@ -303,8 +299,7 @@ public class Topography {
 	 */
 	@Override
 	public Topography clone() {
-		Topography s = new Topography(this.attributes, this.attributesPedestrian);
-		s.attributesCar = this.attributesCar;
+		Topography s = new Topography(this.attributes, this.attributesPedestrian, this.attributesCar);
 
 		for (Obstacle obstacle : this.getObstacles()) {
 			if (this.boundaryObstacles.contains(obstacle))