Add BiomechanicsModel.

VadereSimulator/src/org/vadere/simulator/models/bmm/BiomechanicsModel.java

+package org.vadere.simulator.models.bmm;
+
+import java.util.LinkedList;
+import java.util.List;
+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.models.AttributesBMM;
+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.shapes.VPoint;
+
+/**
+ * 
+ * MainModel READY!
+ * 
+ *
+ */
+public class BiomechanicsModel implements MainModel {
+
+	private List<Model> models = new LinkedList<>();
+
+	private AttributesBMM attributesBMM;
+	private AttributesBHM attributesBHM;
+	private AttributesAgent attributesPedestrian;
+	private Random random;
+	private Topography topography;
+	private int pedestrianIdCounter;
+	private List<PedestrianBMM> pedestriansBMM;
+	protected double lastSimTimeInSec;
+
+	public BiomechanicsModel() {
+		this.pedestrianIdCounter = 0;
+		this.pedestriansBMM = new LinkedList<>();
+	}
+
+	@Override
+	public void initialize(List<Attributes> modelAttributesList, Topography topography,
+			AttributesAgent attributesPedestrian, Random random) {
+		this.attributesBHM = Model.findAttributes(modelAttributesList, AttributesBHM.class);
+		this.attributesBMM = Model.findAttributes(modelAttributesList, AttributesBMM.class);
+		this.attributesPedestrian = attributesPedestrian;
+		this.topography = topography;
+		this.random = random;
+		this.models.add(this);
+	}
+
+	@Override
+	public <T extends DynamicElement> PedestrianBMM createElement(VPoint position, int id, Class<T> type) {
+		if (!Pedestrian.class.isAssignableFrom(type))
+			throw new IllegalArgumentException("BMM cannot initialize " + type.getCanonicalName());
+
+		pedestrianIdCounter++;
+		AttributesAgent pedAttributes = new AttributesAgent(
+				this.attributesPedestrian, id > 0 ? id : pedestrianIdCounter);
+
+		PedestrianBMM pedestrian = new PedestrianBMM(position, topography, pedAttributes,
+				attributesBMM, attributesBHM, random);
+
+		this.pedestriansBMM.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) {
+		double deltaTime = simTimeInSec - lastSimTimeInSec;
+
+		for (PedestrianBMM agent : pedestriansBMM) {
+			agent.update(simTimeInSec, deltaTime);
+		}
+
+		for (PedestrianBMM agent : pedestriansBMM) {
+			agent.move(simTimeInSec, deltaTime);
+		}
+
+		for (PedestrianBMM agent : pedestriansBMM) {
+			agent.reverseCollisions();
+		}
+
+		this.lastSimTimeInSec = simTimeInSec;
+	}
+
+	@Override
+	public List<Model> getSubmodels() {
+		return models;
+	}
+
+}

VadereSimulator/src/org/vadere/simulator/models/bmm/PedestrianBMM.java

+package org.vadere.simulator.models.bmm;
+
+import java.util.Random;
+
+import org.vadere.simulator.models.bhm.PedestrianBHM;
+import org.vadere.state.attributes.models.AttributesBHM;
+import org.vadere.state.attributes.models.AttributesBMM;
+import org.vadere.state.attributes.scenario.AttributesAgent;
+import org.vadere.state.scenario.Pedestrian;
+import org.vadere.state.scenario.Topography;
+import org.vadere.util.geometry.GeometryUtils;
+import org.vadere.util.geometry.Vector2D;
+import org.vadere.util.geometry.shapes.VPoint;
+
+public class PedestrianBMM extends Pedestrian {
+
+	private final Topography topography;
+	private final AttributesBMM attributesBMM;
+	private final PedestrianBHM pedestrianBHM;
+
+	private VPoint nextStepPosition;
+	private VPoint lastMotionPosition;
+	private VPoint nextMotionPosition;
+	private VPoint nextVelocity;
+	private VPoint nextAcceleration;
+	private double stepSum;
+
+	private boolean isInitialized;
+
+	public PedestrianBMM(VPoint position, Topography topography, AttributesAgent attributesPedestrian,
+			AttributesBMM attributesBMM, AttributesBHM attributesBHM, Random random) {
+
+		super(attributesPedestrian, random);
+		super.setPosition(position);
+
+		this.topography = topography;
+		this.attributesBMM = attributesBMM;
+		this.pedestrianBHM = new PedestrianBHM(topography, attributesPedestrian, attributesBHM, random);
+		this.isInitialized = false;
+	}
+
+	void intialize() {
+		pedestrianBHM.setTargets(getTargets());
+		reset();
+		this.isInitialized = true;
+	}
+
+	void reset() {
+		this.nextStepPosition = getPosition();
+		this.lastMotionPosition = getPosition();
+		this.nextMotionPosition = getPosition();
+		this.nextVelocity = new VPoint(0, 0);
+		this.nextAcceleration = new VPoint(0, 0);
+		this.stepSum = 0;
+	}
+
+	void update(double simTime, double deltaTime) {
+
+		if (!isInitialized) {
+			intialize();
+			updateStepPosition(simTime);
+		} else if (!attributesBMM.isStepwiseDecisions() ||
+				pedestrianBHM.getStepLength() < stepSum ||
+				getVelocity().distanceToOrigin() < GeometryUtils.DOUBLE_EPS) {
+			updateStepPosition(simTime);
+		}
+
+		updateAcceleration(simTime, deltaTime);
+		updateVelocity(simTime, deltaTime);
+		updatePosition(simTime, deltaTime);
+	}
+
+	void updatePosition(double simTime, double deltaTime) {
+		this.nextMotionPosition = getPosition().add(nextVelocity.scalarMultiply(deltaTime));
+
+		// if next position collides with wall or other pedestrian, remain at current position
+		if (pedestrianBHM.collidesWithObstacle(nextMotionPosition) ||
+				pedestrianBHM.collidesWithPedestrian(nextMotionPosition, 0)) {
+			this.nextMotionPosition = getPosition();
+		}
+	}
+
+	void updateVelocity(double simTime, double deltaTime) {
+		this.nextVelocity = getVelocity().add(nextAcceleration.scalarMultiply(deltaTime));
+	}
+
+	void updateAcceleration(double simTime, double deltaTime) {
+
+		VPoint preferredVelocity = nextStepPosition.subtract(getPosition()).normZeroSafe();
+		preferredVelocity = preferredVelocity.scalarMultiply(getFreeFlowSpeed());
+
+		VPoint acceleration = preferredVelocity.subtract(getVelocity());
+
+		this.nextAcceleration = acceleration.scalarMultiply(attributesBMM.getAcceleration());
+	}
+
+	void move(double simTime, double deltaTime) {
+		VPoint currentPosition = getPosition();
+
+		this.lastMotionPosition = currentPosition;
+		setPosition(nextMotionPosition);
+
+		Vector2D motionStep = new Vector2D(nextMotionPosition.x - currentPosition.x,
+				nextMotionPosition.y - currentPosition.y);
+
+		if (deltaTime < GeometryUtils.DOUBLE_EPS) {
+			setVelocity(new Vector2D(0, 0));
+		} else {
+			// compute velocity by forward difference
+			setVelocity(motionStep.multiply(1.0 / deltaTime));
+		}
+
+		this.stepSum = stepSum + motionStep.distanceToOrigin();
+	}
+
+	public void reverseCollisions() {
+		// if next position collides with wall or other pedestrian, remain at current position
+		if (pedestrianBHM.collidesWithObstacle(nextMotionPosition) ||
+				pedestrianBHM.collidesWithPedestrian(nextMotionPosition, 0)) {
+			setPosition(lastMotionPosition);
+			reset();
+		}
+	}
+
+	private void updateStepPosition(double simTime) {
+		pedestrianBHM.setPosition(this.getPosition());
+		pedestrianBHM.update(simTime);
+		this.nextStepPosition = pedestrianBHM.getPosition();
+		this.stepSum = 0;
+	}
+
+
+}