Commit 83568196 authored by hm-seitz4's avatar hm-seitz4

Add BiomechanicsModel.

parent d099754c
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;
}
}
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;
}
}
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