In January 2021 we will introduce a 10 GB quota for project repositories. Higher limits for individual projects will be available on request. Please see https://doku.lrz.de/display/PUBLIC/GitLab for more information.

Commit 22941d1e authored by Stefan Schuhbaeck's avatar Stefan Schuhbaeck

add getNorthBoundHeadingAngleDeg to FootstepHistory

Angle measuring the heading of the pedestrian. North is
defined as 0 degree with cock-wise angle (i.e. East := 90)
parent 5f782e0c
package org.vadere.state.simulation;
import org.jetbrains.annotations.Nullable;
import org.vadere.util.geometry.shapes.VPoint;
import org.vadere.util.geometry.shapes.Vector2D;
import java.util.ArrayList;
import java.util.stream.Collectors;
......@@ -69,6 +71,46 @@ public class FootstepHistory {
return oldestFootStep;
}
/**
* Heading based on TraCI angle
* * - measured in degree
* * - 0 is headed north
* * - clockwise orientation (i.e. 90 heads east, 180 heads south, etc.)
* * - range from 0 to 360.
*
* @return Heading angle based on last FootStep.
*/
public double getNorthBoundHeadingAngleDeg(){
return getNorthBoundHeadingAngle(1, true);
}
public double getNorthBoundHeadingAngleRad(){
return getNorthBoundHeadingAngle(1, false);
}
public double getNorthBoundHeadingAngle(int histLength, boolean degree){
if (footSteps.size() < histLength)
return 0.0; // not enough data. Return North heading.
VPoint currentLocation = footSteps.get(footSteps.size() -1).getEnd();
VPoint pastLocation = footSteps.get(footSteps.size() - histLength).getStart();
Vector2D heading = new Vector2D(currentLocation.x - pastLocation.x, currentLocation.y - pastLocation.y);
if (Math.abs(heading.getLength() -0.0) < 0.0001){
//Footstep to small
return 0.0; // assume North heading
}
// TraCI Angle defined as Clockwise with North as 0 deg.
// clockwise: | 2PI - angle |
// 0 at North: + PI
// mod 2PI for [0, 2PI]
double angel = (Math.abs(2*Math.PI - heading.angleToZero()) + 0.5*Math.PI) % (2* Math.PI); // angle with y-axis
if (degree)
return angel*(180/Math.PI);
return angel;
}
@Nullable
public FootStep getYoungestFootStep() {
FootStep youngestFootStep = null;
......
......@@ -303,4 +303,23 @@ public class FootstepHistoryTest {
assertEquals(expectedValue, actualX, ALLOWED_DOUBLE_ERROR);
}
}
@Test
public void getHeadingAngleDegTest(){
footstepHistory.add(new FootStep(new VPoint(1,1), new VPoint(3,2), 1, 2 ));
double ret = footstepHistory.getNorthBoundHeadingAngleDeg();
assertEquals(63.434, ret, 0.01);
footstepHistory.removeLast();
footstepHistory.add(new FootStep(new VPoint(10,10), new VPoint(5,10), 1, 2 ));
ret = footstepHistory.getNorthBoundHeadingAngleDeg(); // move to west. expect 270
assertEquals(270.0, ret, 0.01);
footstepHistory.removeLast();
ret = footstepHistory.getNorthBoundHeadingAngleDeg();
assertEquals(0.0, ret, 0.01);
// assume North heading if FootSteps are the same.
footstepHistory.add(new FootStep(new VPoint(10,10), new VPoint(10,10), 1, 2 ));
ret = footstepHistory.getNorthBoundHeadingAngleDeg();
assertEquals(0.0, ret, 0.01);
}
}
\ No newline at end of file
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