change pedestrian position processor to more accurate position using FootStep (interpolated)

VadereSimulator/src/org/vadere/simulator/models/bhm/PedestrianBHM.java

@@ -189,7 +189,7 @@ public class PedestrianBHM extends Pedestrian {
 			throw new UnsupportedEventException(mostImportantEvent, this.getClass());
 		}
 
-		getTrajectory().add(new FootStep(position, getPosition(), timeOfNextStep, timeOfNextStep + durationNextStep));
+		addFootStepToTrajectory(new FootStep(position, getPosition(), timeOfNextStep, timeOfNextStep + durationNextStep));
 	}
 
 	/**

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

@@ -110,7 +110,7 @@ public class BiomechanicsModel implements MainModel {
 		for(int i = 0; i < pedestriansBMM.size(); i++) {
 			PedestrianBMM agent = pedestriansBMM.get(i);
 			agent.clearFootSteps();
-			agent.getTrajectory().add(new FootStep(positions.get(i), agent.getPosition(), lastSimTimeInSec, simTimeInSec));
+			agent.addFootStepToTrajectory(new FootStep(positions.get(i), agent.getPosition(), lastSimTimeInSec, simTimeInSec));
 		}
 
 		this.lastSimTimeInSec = simTimeInSec;

VadereSimulator/src/org/vadere/simulator/models/ode/ODEModel.java

@@ -128,7 +128,7 @@ public abstract class ODEModel<T extends DynamicElement, TAttributes extends Att
 				if (element.getType() == ScenarioElementType.PEDESTRIAN) {
 					Pedestrian pedestrian = (Pedestrian)element;
 					pedestrian.clearFootSteps();
-					pedestrian.getTrajectory().add(new FootStep(positions.get(i), pedestrian.getPosition(), lastSimTimeInSec, simTimeInSec));
+					pedestrian.addFootStepToTrajectory(new FootStep(positions.get(i), pedestrian.getPosition(), lastSimTimeInSec, simTimeInSec));
 				}
 			}
 		}

VadereSimulator/src/org/vadere/simulator/models/osm/OSMBehaviorController.java

@@ -78,7 +78,7 @@ public class OSMBehaviorController {
 
         // strides and foot steps have no influence on the simulation itself, i.e. they are saved to analyse trajectories
         pedestrian.getStrides().add(Pair.of(currentPosition.distance(nextPosition), stepStartTime));
-        pedestrian.getTrajectory().add(new FootStep(currentPosition, nextPosition, stepStartTime, stepEndTime));
+        pedestrian.addFootStepToTrajectory(new FootStep(currentPosition, nextPosition, stepStartTime, stepEndTime));
     }
 
     public void wait(PedestrianOSM pedestrian) {

VadereSimulator/src/org/vadere/simulator/models/osm/updateScheme/UpdateSchemeOSM.java

@@ -134,7 +134,7 @@ public interface UpdateSchemeOSM extends DynamicElementRemoveListener<Pedestrian
 		 * strides and foot steps have no influence on the simulation itself, i.e. they are saved to analyse trajectories
 		 */
 		pedestrian.getStrides().add(Pair.of(currentPosition.distance(nextPosition), timeOfNextStep));
-		pedestrian.getTrajectory().add(new FootStep(currentPosition, nextPosition, timeOfNextStep, entTimeOfStep));
+		pedestrian.addFootStepToTrajectory(new FootStep(currentPosition, nextPosition, timeOfNextStep, entTimeOfStep));
 	}
 
 }

VadereSimulator/src/org/vadere/simulator/models/reynolds/PedestrianReynolds.java

@@ -44,7 +44,7 @@ public class PedestrianReynolds extends Pedestrian {
 		setPosition(newPosition);
 		clearFootSteps();
 		// TODO: the first footstep starts at the wrong time!
-		getTrajectory().add(new FootStep(oldPosition, newPosition, lastSimTimeInSec, simTime));
+		addFootStepToTrajectory(new FootStep(oldPosition, newPosition, lastSimTimeInSec, simTime));
 		lastSimTimeInSec = simTime;
 	}
 

VadereSimulator/src/org/vadere/simulator/projects/dataprocessing/processor/PedestrianDensityProcessor.java

@@ -24,10 +24,11 @@ public abstract class PedestrianDensityProcessor extends DataProcessor<TimestepP
 	@Override
 	public void doUpdate(final SimulationState state) {
 		this.pedPosProc.update(state);
+		double simTime = state.getSimTimeInSec();
 
-		state.getTopography().getElements(Pedestrian.class).stream()
-				.forEach(ped -> this.putValue(new TimestepPedestrianIdKey(state.getStep(), ped.getId()),
-						this.densAlg.getDensity(ped.getPosition(), state)));
+		state.getTopography().getElements(Pedestrian.class).stream().
+				forEach(ped -> this.putValue(new TimestepPedestrianIdKey(state.getStep(), ped.getId()),
+						this.densAlg.getDensity(ped.getInterpolatedFootStepPosition(simTime), state)));
 	}
 
 	@Override

VadereSimulator/src/org/vadere/simulator/projects/dataprocessing/processor/PedestrianPositionProcessor.java

@@ -7,8 +7,10 @@ import org.vadere.simulator.projects.dataprocessing.datakey.PedestrianIdKey;
 import org.vadere.simulator.projects.dataprocessing.datakey.TimestepKey;
 import org.vadere.simulator.projects.dataprocessing.datakey.TimestepPedestrianIdKey;
 import org.vadere.state.scenario.Pedestrian;
+import org.vadere.state.simulation.FootStep;
 import org.vadere.util.geometry.shapes.VPoint;
 
+import java.util.Collection;
 import java.util.Map;
 import java.util.stream.Collectors;
 
@@ -30,9 +32,15 @@ public class PedestrianPositionProcessor extends DataProcessor<TimestepPedestria
 
 	@Override
 	protected void doUpdate(final SimulationState state) {
+		// This does not work currently, bcause of the mocking in the tests.
+		// Collection<Pedestrian> pedestrians = state.getTopography().getPedestrianDynamicElements().getElements();
+		Collection<Pedestrian> pedestrians = state.getTopography().getElements(Pedestrian.class);
 		Integer timeStep = state.getStep();
-		for (Pedestrian p : state.getTopography().getElements(Pedestrian.class)) {
-			this.putValue(new TimestepPedestrianIdKey(timeStep, p.getId()), p.getPosition());
+		double simTime = state.getSimTimeInSec();
+
+		for (Pedestrian pedestrian : pedestrians){
+			VPoint interpolatedPoint = pedestrian.getInterpolatedFootStepPosition(simTime);
+			this.putValue(new TimestepPedestrianIdKey(timeStep, pedestrian.getId()), interpolatedPoint);
 		}
 	}
 

VadereSimulator/src/org/vadere/simulator/projects/dataprocessing/processor/PointDensityCountingAlgorithm.java

@@ -21,11 +21,15 @@ public class PointDensityCountingAlgorithm extends PointDensityAlgorithm {
 	}
 
 	@Override
-	public double getDensity(VPoint pos, SimulationState state) {
+	public double getDensity(VPoint densityEvalPosition, SimulationState state) {
 		int numberOfPedsInCircle = 0;
 
+		double simTimeSeconds = state.getSimTimeInSec();
+
 		for (Pedestrian ped : state.getTopography().getElements(Pedestrian.class)) {
-			if (pos.distance(ped.getPosition()) < this.radius) {
+			VPoint pedestrianPosition = ped.getInterpolatedFootStepPosition(simTimeSeconds);
+
+			if (densityEvalPosition.distance(pedestrianPosition) < this.radius) {
 				numberOfPedsInCircle++;
 			}
 		}

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

@@ -288,7 +288,7 @@ public class TrajectoryReader {
 		if(stridesIndex != NOT_SET_COLUMN_INDEX_IDENTIFIER) {
 			FootStep[] footSteps = StateJsonConverter.deserializeObjectFromJson(rowTokens[stridesIndex], FootStep[].class);
 			for(FootStep footStep : footSteps) {
-				ped.getTrajectory().add(footStep);
+				ped.addFootStepToTrajectory(footStep);
 			}
 		}
 

VadereSimulator/tests/org/vadere/simulator/projects/dataprocessing/processor/AreaSpeedProcessorTestEnv.java

@@ -7,6 +7,8 @@ import org.vadere.state.attributes.processor.AttributesAreaSpeedProcessor;
 import org.vadere.state.attributes.scenario.AttributesMeasurementArea;
 import org.vadere.state.scenario.MeasurementArea;
 import org.vadere.state.scenario.Pedestrian;
+import org.vadere.state.simulation.FootStep;
+import org.vadere.state.simulation.VTrajectory;
 import org.vadere.util.geometry.shapes.VPoint;
 import org.vadere.util.geometry.shapes.VRectangle;
 
@@ -49,9 +51,19 @@ public class AreaSpeedProcessorTestEnv extends ProcessorTestEnv<TimestepKey, Dou
 		addSimState(new SimulationStateMock(1) {
 			@Override
 			public void mockIt() {
-				b.clear().add(1, new VPoint(1.0, 0.0))
-						.add(2, new VPoint(0.0, 0.0))
-						.add(3, new VPoint(7.0, 4.0));    //not in area
+
+				VTrajectory traj1 = new VTrajectory();
+				VTrajectory traj2 = new VTrajectory();
+				VTrajectory traj3 = new VTrajectory();
+
+				//trajectoryID
+				traj1.add(new FootStep(new VPoint(1,0), new VPoint(1,0), 0, 1));
+				traj2.add(new FootStep(new VPoint(0,0), new VPoint(0,0), 0, 1));
+				traj3.add(new FootStep(new VPoint(7,4), new VPoint(7,4), 0, 1));
+
+				b.clear().add(1, traj1)
+						.add(2, traj2)
+						.add(3, traj3);    //not in area
 
 				when(state.getTopography().getElements(Pedestrian.class)).thenReturn(b.getList());
 				when(state.getSimTimeInSec()).thenReturn(0.0);
@@ -64,9 +76,18 @@ public class AreaSpeedProcessorTestEnv extends ProcessorTestEnv<TimestepKey, Dou
 		addSimState(new SimulationStateMock(2) {
 			@Override
 			public void mockIt() {
-				b.clear().add(1, new VPoint(1.0, 1.0)) //dist = 1.0
-						.add(2, new VPoint(3.0, 4.0))    //dist = 5.0
-						.add(3, new VPoint(8.0, 4.0));    //not in area
+
+				VTrajectory traj1 = new VTrajectory();
+				VTrajectory traj2 = new VTrajectory();
+				VTrajectory traj3 = new VTrajectory();
+
+				traj1.add(new FootStep(new VPoint(1,1), new VPoint(1,1), 1, 2.0));
+				traj2.add(new FootStep(new VPoint(3,4), new VPoint(3,4), 1, 2.0));
+				traj3.add(new FootStep(new VPoint(8,4), new VPoint(8,4), 1, 2.0));
+
+				b.clear().add(1, traj1)   //dist = 1.0
+						.add(2, traj2)    //dist = 5.0
+						.add(3, traj3);   //not in area
 
 				when(state.getTopography().getElements(Pedestrian.class)).thenReturn(b.getList());
 				double simTime = 1.0;
@@ -82,9 +103,18 @@ public class AreaSpeedProcessorTestEnv extends ProcessorTestEnv<TimestepKey, Dou
 		addSimState(new SimulationStateMock(3) {
 			@Override
 			public void mockIt() {
-				b.clear().add(1, new VPoint(3.0, 1.0)) //dist = 2.0
-						.add(2, new VPoint(5.0, 4.0))    //not in area
-						.add(3, new VPoint(8.0, 8.0));    //not in area
+
+				VTrajectory traj1 = new VTrajectory();
+				VTrajectory traj2 = new VTrajectory();
+				VTrajectory traj3 = new VTrajectory();
+
+				traj1.add(new FootStep(new VPoint(3,1), new VPoint(3,1), 1.0, 2.0));
+				traj2.add(new FootStep(new VPoint(5,4), new VPoint(5,4), 1.0, 2.0));
+				traj3.add(new FootStep(new VPoint(8,8), new VPoint(8,8), 1.0, 2.0));
+
+				b.clear().add(1, traj1)   //dist = 2.0
+						.add(2, traj2)    //not in area
+						.add(3, traj3);   //not in area
 
 				when(state.getTopography().getElements(Pedestrian.class)).thenReturn(b.getList());
 				double simTime = 2.0;
@@ -100,9 +130,18 @@ public class AreaSpeedProcessorTestEnv extends ProcessorTestEnv<TimestepKey, Dou
 		addSimState(new SimulationStateMock(4) {
 			@Override
 			public void mockIt() {
-				b.clear().add(1, new VPoint(6.0, 1.0)) // not in area
-						.add(2, new VPoint(7.0, 4.0))    // not in area
-						.add(3, new VPoint(9.0, 8.0));    // not in area
+
+				VTrajectory traj1 = new VTrajectory();
+				VTrajectory traj2 = new VTrajectory();
+				VTrajectory traj3 = new VTrajectory();
+
+				traj1.add(new FootStep(new VPoint(6,1), new VPoint(6,1), 2, 3.0));
+				traj2.add(new FootStep(new VPoint(7,4), new VPoint(7,4), 2, 3.0));
+				traj3.add(new FootStep(new VPoint(9,8), new VPoint(9,8), 2, 3.0));
+
+				b.clear().add(1, traj1)   // not in area
+						.add(2, traj2)    // not in area
+						.add(3, traj3);   // not in area
 
 				when(state.getTopography().getElements(Pedestrian.class)).thenReturn(b.getList());
 				double simTime = 3.0;

VadereSimulator/tests/org/vadere/simulator/projects/dataprocessing/processor/PedestrianDensityCountingProcessorTestEnv.java

@@ -5,6 +5,8 @@ import org.vadere.simulator.projects.dataprocessing.datakey.TimestepPedestrianId
 import org.vadere.simulator.utils.PedestrianListBuilder;
 import org.vadere.state.attributes.processor.AttributesPedestrianDensityCountingProcessor;
 import org.vadere.state.scenario.Pedestrian;
+import org.vadere.state.simulation.FootStep;
+import org.vadere.state.simulation.VTrajectory;
 import org.vadere.util.geometry.shapes.VPoint;
 
 import java.util.ArrayList;
@@ -43,13 +45,23 @@ public class PedestrianDensityCountingProcessorTestEnv extends ProcessorTestEnv<
 			@Override
 			public void mockIt() {
 
-				b.clear().add(1, new VPoint(1.0, 1.0));
-				b.add(2, new VPoint(1.2, 1.2));
-				b.add(3, new VPoint(1.4, 1.4));
+				VTrajectory traj1 = new VTrajectory().add(new FootStep(new VPoint(1,1), new VPoint(1,1), 0,0));
+				VTrajectory traj2 = new VTrajectory().add(new FootStep(new VPoint(1.2,1.2), new VPoint(1.2,1.2), 0,0));
+
 				VPoint p = new VPoint(1.4, 1.4);
-				p = p.addPrecise(new VPoint(Math.sqrt(radius) - 0.001, Math.sqrt(radius) - 0.001));
-				b.add(4, p);
-				b.add(5, new VPoint(10.0, 10.0));
+				VPoint pPrecise = p.clone().addPrecise(new VPoint(Math.sqrt(radius) - 0.001, Math.sqrt(radius) - 0.001));
+
+				VTrajectory traj3 = new VTrajectory().add(new FootStep(p, p, 0,0));
+				VTrajectory traj4 = new VTrajectory().add(new FootStep(pPrecise, pPrecise, 0,0));
+				VTrajectory traj5 = new VTrajectory().add(new FootStep(new VPoint(10,10), new VPoint(10,10), 0,0));
+
+
+				b.clear().add(1, traj1);
+				b.add(2, traj2);
+				b.add(3, traj3);
+				b.add(4, traj4);
+				b.add(5, traj5);
+
 				Mockito.when(state.getTopography().getElements(Pedestrian.class)).thenReturn(b.getList());
 
 				int step = state.getStep();

VadereSimulator/tests/org/vadere/simulator/projects/dataprocessing/processor/PedestrianFlowProcessorTestEnv.java

@@ -6,6 +6,8 @@ import org.vadere.simulator.utils.PedestrianListBuilder;
 import org.vadere.state.attributes.processor.AttributesPedestrianDensityCountingProcessor;
 import org.vadere.state.attributes.processor.AttributesPedestrianFlowProcessor;
 import org.vadere.state.scenario.Pedestrian;
+import org.vadere.state.simulation.FootStep;
+import org.vadere.state.simulation.VTrajectory;
 import org.vadere.util.geometry.shapes.VPoint;
 
 import java.util.ArrayList;
@@ -49,13 +51,23 @@ public class PedestrianFlowProcessorTestEnv extends ProcessorTestEnv<TimestepPed
 		addSimState(new SimulationStateMock(1) {
 			@Override
 			public void mockIt() {
-				b.clear().add(1, new VPoint(1.0, 1.0));
-				b.add(2, new VPoint(1.2, 1.2));
-				b.add(3, new VPoint(1.4, 1.4));
+
+				VTrajectory traj1 = new VTrajectory().add(new FootStep(new VPoint(1,1), new VPoint(1,1), 0,0));
+				VTrajectory traj2 = new VTrajectory().add(new FootStep(new VPoint(1.2,1.2), new VPoint(1.2,1.2), 0,0));
+
 				VPoint p = new VPoint(1.4, 1.4);
-				p = p.addPrecise(new VPoint(Math.sqrt(radius) - 0.001, Math.sqrt(radius) - 0.001));
-				b.add(4, p);
-				b.add(5, new VPoint(10.0, 10.0));
+				VPoint pPrecise = p.clone().addPrecise(new VPoint(Math.sqrt(radius) - 0.001, Math.sqrt(radius) - 0.001));
+
+				VTrajectory traj3 = new VTrajectory().add(new FootStep(p, p, 0,0));
+				VTrajectory traj4 = new VTrajectory().add(new FootStep(pPrecise, pPrecise, 0,0));
+				VTrajectory traj5 = new VTrajectory().add(new FootStep(new VPoint(10,10), new VPoint(10,10), 0,0));
+
+
+				b.clear().add(1, traj1);
+				b.add(2, traj2);
+				b.add(3, traj3);
+				b.add(4, traj4);
+				b.add(5, traj5);
 
 				Mockito.when(state.getTopography().getElements(Pedestrian.class)).thenReturn(b.getList());
 				Mockito.when(state.getSimTimeInSec()).thenReturn(0.0);
@@ -76,14 +88,38 @@ public class PedestrianFlowProcessorTestEnv extends ProcessorTestEnv<TimestepPed
 		addSimState(new SimulationStateMock(2) {
 			@Override
 			public void mockIt() {
+
 				VPoint mov = new VPoint(3.0, 4.0); //dist = 5 time = 1 -> 5ms^-1
-				b.clear().add(1, new VPoint(1.0, 1.0).addPrecise(mov));
-				b.add(2, new VPoint(1.2, 1.2).addPrecise(mov));
-				b.add(3, new VPoint(1.4, 1.4).addPrecise(mov));
+
+				VTrajectory traj1 = new VTrajectory().add(
+						new FootStep(
+								new VPoint(1,1).addPrecise(mov),
+								new VPoint(1,1).addPrecise(mov), 1,1));
+
+				VTrajectory traj2 = new VTrajectory().add(
+						new FootStep(
+								new VPoint(1.2,1.2).addPrecise(mov),
+								new VPoint(1.2,1.2).addPrecise(mov), 1,1));
+
 				VPoint p = new VPoint(1.4, 1.4);
-				p = p.addPrecise(new VPoint(Math.sqrt(radius) - 0.001, Math.sqrt(radius) - 0.001));
-				b.add(4, p.addPrecise(mov));
-				b.add(5, new VPoint(10.0, 10.0).addPrecise(mov));
+				VPoint pPrecise = p.clone().addPrecise(
+						new VPoint(Math.sqrt(radius) - 0.001, Math.sqrt(radius) - 0.001));
+
+				VTrajectory traj3 = new VTrajectory().add(
+						new FootStep(p.addPrecise(mov), p.addPrecise(mov), 1,1));
+
+				VTrajectory traj4 = new VTrajectory().add(
+						new FootStep(pPrecise.addPrecise(mov), pPrecise.addPrecise(mov), 1,1));
+
+				VTrajectory traj5 = new VTrajectory().add(
+						new FootStep(new VPoint(10,10).addPrecise(mov), new VPoint(10,10).addPrecise(mov), 1,1));
+
+
+				b.clear().add(1, traj1);
+				b.add(2, traj2);
+				b.add(3, traj3);
+				b.add(4, traj4);
+				b.add(5, traj5);
 
 				Mockito.when(state.getTopography().getElements(Pedestrian.class)).thenReturn(b.getList());
 				Mockito.when(state.getSimTimeInSec()).thenReturn(1.0);

VadereSimulator/tests/org/vadere/simulator/projects/dataprocessing/processor/PedestrianLastPositionProcessorTestEnv.java

@@ -4,6 +4,8 @@ import org.vadere.simulator.projects.dataprocessing.datakey.PedestrianIdKey;
 import org.vadere.simulator.utils.PedestrianListBuilder;
 import org.vadere.state.attributes.processor.AttributesPedestrianLastPositionProcessor;
 import org.vadere.state.scenario.Pedestrian;
+import org.vadere.state.simulation.FootStep;
+import org.vadere.state.simulation.VTrajectory;
 import org.vadere.util.geometry.shapes.VPoint;
 
 import java.util.ArrayList;
@@ -36,9 +38,13 @@ public class PedestrianLastPositionProcessorTestEnv extends ProcessorTestEnv<Ped
 			@Override
 			public void mockIt() {
 
-				b.clear().add(1, new VPoint(1.0, 1.2))
-						.add(3, new VPoint(4.45, 1.2))
-						.add(5, new VPoint(3.546, 7.2342));
+				VTrajectory traj1 = new VTrajectory().add(new FootStep(new VPoint(1.0, 1.2), new VPoint(1.0, 1.2), 0,0));
+				VTrajectory traj3 = new VTrajectory().add(new FootStep(new VPoint(4.45, 1.2), new VPoint(4.45, 1.2), 0,0));
+				VTrajectory traj5 = new VTrajectory().add(new FootStep(new VPoint(3.546, 7.2342), new VPoint(3.546, 7.2342), 0,0));
+
+				b.clear().add(1, traj1)
+						.add(3, traj3)
+						.add(5, traj5);
 				when(state.getTopography().getElements(Pedestrian.class)).thenReturn(b.getList());
 
 				addToExpectedOutput(new PedestrianIdKey(1), new VPoint(1.0, 1.2));
@@ -51,9 +57,14 @@ public class PedestrianLastPositionProcessorTestEnv extends ProcessorTestEnv<Ped
 		addSimState(new SimulationStateMock(2) {
 			@Override
 			public void mockIt() {
-				b.clear().add(1, new VPoint(33.2, 3.22))
-						.add(3, new VPoint(3.2, 22.3))
-						.add(7, new VPoint(1.2, 3.3));
+
+				VTrajectory traj1 = new VTrajectory().add(new FootStep(new VPoint(33.2, 3.22), new VPoint(33.2, 3.22), 0,0));
+				VTrajectory traj3 = new VTrajectory().add(new FootStep(new VPoint(3.2, 22.3), new VPoint(3.2, 22.3), 0,0));
+				VTrajectory traj7 = new VTrajectory().add(new FootStep(new VPoint(1.2, 3.3), new VPoint(1.2, 3.3), 0,0));
+
+				b.clear().add(1, traj1)
+						.add(3, traj3)
+						.add(7, traj7);
 				when(state.getTopography().getElements(Pedestrian.class)).thenReturn(b.getList());
 
 				//overwrite Values!

VadereSimulator/tests/org/vadere/simulator/projects/dataprocessing/processor/PedestrianPositionProcessorTestEnv.java

@@ -3,6 +3,8 @@ package org.vadere.simulator.projects.dataprocessing.processor;
 import org.vadere.simulator.projects.dataprocessing.datakey.TimestepPedestrianIdKey;
 import org.vadere.simulator.utils.PedestrianListBuilder;
 import org.vadere.state.scenario.Pedestrian;