Merge branch 'add_artery_support' into 'master'

Add artery support

See merge request !143

VadereManager/src/org/vadere/manager/Manager.java

@@ -9,6 +9,7 @@ import net.sourceforge.argparse4j.internal.HelpScreenException;
 import org.vadere.manager.server.AbstractVadereServer;
 import org.vadere.manager.server.VadereServer;
 import org.vadere.manager.server.VadereSingleClientServer;
+import org.vadere.util.io.VadereArgumentParser;
 import org.vadere.util.logging.Logger;
 
 import java.net.InetAddress;
@@ -24,11 +25,11 @@ public class Manager {
 	public static void main(String[] args) {
 		Logger.setMainArguments(args);
 		logger = Logger.getLogger(Manager.class);
-		ArgumentParser p = createArgumentParser();
+		VadereArgumentParser p = createArgumentParser();
 		Namespace ns;
 
 		try {
-			ns = p.parseArgs(args);
+			ns = p.parseArgsAndProcessInitialOptions(args);
 
 			ServerSocket serverSocket = new ServerSocket(ns.getInt("port"), 50, InetAddress.getByName(ns.getString("bind")));
 			logger.infof("Start Server(%s) with Loglevel: %s", VadereServer.currentVersion.getVersionString(), logger.getLevel().toString());
@@ -49,32 +50,31 @@ public class Manager {
 		}
 	}
 
-	private static ArgumentParser createArgumentParser() {
-		ArgumentParser parser = ArgumentParsers.newArgumentParser("Vadere Server")
-				.defaultHelp(true)
-				.description("Runs the VADERE pedestrian simulator as a server.");
+	private static VadereArgumentParser createArgumentParser() {
+		VadereArgumentParser vadereArgumentParser = new VadereManagerArgumentParser();
+		ArgumentParser parser = vadereArgumentParser.getArgumentParser();
 
 		addOptionsToParser(parser);
 
-		return parser;
+		return vadereArgumentParser;
 	}
 
 	private static void addOptionsToParser(ArgumentParser parser) {
 		// no action required call to  Logger.setMainArguments(args) already configured Logger.
-		parser.addArgument("--loglevel")
-				.required(false)
-				.type(String.class)
-				.dest("loglevel")
-				.choices("OFF", "FATAL", "ERROR", "WARN", "INFO", "DEBUG", "TRACE", "ALL")
-				.setDefault("INFO")
-				.help("Set Log Level.");
-
-		// no action required call to  Logger.setMainArguments(args) already configured Logger.
-		parser.addArgument("--logname")
-				.required(false)
-				.type(String.class)
-				.dest("logname")
-				.help("Write log to given file.");
+//		parser.addArgument("--loglevel")
+//				.required(false)
+//				.type(String.class)
+//				.dest("loglevel")
+//				.choices("OFF", "FATAL", "ERROR", "WARN", "INFO", "DEBUG", "TRACE", "ALL")
+//				.setDefault("INFO")
+//				.help("Set Log Level.");
+//
+//		// no action required call to  Logger.setMainArguments(args) already configured Logger.
+//		parser.addArgument("--logname")
+//				.required(false)
+//				.type(String.class)
+//				.dest("logname")
+//				.help("Write log to given file.");
 
 
 		// no action required call to  Logger.setMainArguments(args) already configured Logger.

VadereManager/src/org/vadere/manager/VadereManagerArgumentParser.java

+package org.vadere.manager;
+
+import net.sourceforge.argparse4j.inf.ArgumentParserException;
+import net.sourceforge.argparse4j.inf.Namespace;
+
+import org.vadere.manager.server.VadereServer;
+import org.vadere.util.config.VadereConfig;
+import org.vadere.util.io.VadereArgumentParser;
+import org.vadere.util.version.Version;
+
+public class VadereManagerArgumentParser extends VadereArgumentParser {
+
+	@Override
+	public Namespace parseArgsAndProcessInitialOptions(String[] args) throws ArgumentParserException {
+		if (versionIsRequested(args)) {
+			System.out.println(String.format("Vadere %s (Commit Hash: %s) [TraCI: %s]",
+					Version.releaseNumber(),
+					Version.getVersionControlCommitHash(),
+					VadereServer.currentVersion.getVersionString()));
+			System.exit(0);
+		}
+
+		Namespace namespace = argumentParser.parseArgs(args);
+
+		String configFile = namespace.getString("configfile");
+		if (configFile != null) {
+			VadereConfig.setConfigPath(configFile);
+		}
+
+		return namespace;
+	}
+}

VadereManager/src/org/vadere/manager/client/TestClient.java

@@ -456,12 +456,37 @@ public class TestClient extends org.vadere.manager.client.AbstractTestClient imp
 
 	}
 
+	@Override
+	public void simulationapi_getDepartedPedestrianId(String[] args) throws IOException {
+
+	}
+
+	@Override
+	public void simulationapi_getArrivedPedestrianIds(String[] args) throws IOException {
+
+	}
+
+	@Override
+	public void simulationapi_getPositionConversion(String[] args) throws IOException {
+
+	}
+
+	@Override
+	public void simulationapi_getCoordinateReference(String[] args) throws IOException {
+
+	}
+
 	@Override
 	public void simulationapi_getTime(String[] args) throws IOException {
 		TraCIResponse res = simulationapi.getTime();
 		System.out.println(res.toString());
 	}
 
+	@Override
+	public void simulationapi_getSimSte(String[] args) throws IOException {
+
+	}
+
 	@Override
 	public void simulationapi_setSimConfig(String[] args) throws IOException {
 		System.out.println("not implemented");
@@ -599,6 +624,11 @@ public class TestClient extends org.vadere.manager.client.AbstractTestClient imp
 		System.out.println(res.toString());
 	}
 
+	@Override
+	public void simulationapi_getNetworkBound(String[] args) throws IOException {
+
+	}
+
 	// vadere api
 
 	@Override

VadereManager/src/org/vadere/manager/traci/TraCICmd.java

@@ -115,6 +115,6 @@ public enum TraCICmd {
 	}
 
 	public String logShort() {
-		return String.format("{%s:0x%02X%s}", name(), id, type);
+		return String.format("{%s: 0x%02X%s}", name(), id, type);
 	}
 }

VadereManager/src/org/vadere/manager/traci/commandHandler/CommandExecutor.java

@@ -30,9 +30,9 @@ public class CommandExecutor {
 		cmdMap.put(TraCICmd.SEND_FILE.id, ControlCommandHandler.instance::process_load_file);
 		cmdMap.put(TraCICmd.GET_PERSON_VALUE.id, PersonCommandHandler.instance::processGet);
 		cmdMap.put(TraCICmd.SET_PERSON_STATE.id, PersonCommandHandler.instance::processSet);
+		cmdMap.put(TraCICmd.SUB_PERSON_VARIABLE.id, PersonCommandHandler.instance::processValueSub);
 		cmdMap.put(TraCICmd.GET_VADERE_VALUE.id, VadereCommandHandler.instance::processGet);
 		cmdMap.put(TraCICmd.SET_VADERE_STATE.id, VadereCommandHandler.instance::processSet);
-		cmdMap.put(TraCICmd.SUB_PERSON_VARIABLE.id, PersonCommandHandler.instance::processValueSub);
 		cmdMap.put(TraCICmd.GET_SIMULATION_VALUE.id, SimulationCommandHandler.instance::processGet);
 		cmdMap.put(TraCICmd.SET_SIMULATION_STATE.id, SimulationCommandHandler.instance::processSet);
 		cmdMap.put(TraCICmd.SUB_SIMULATION_VALUE.id, SimulationCommandHandler.instance::processValueSub);
@@ -67,6 +67,7 @@ public class CommandExecutor {
 			return TraCIPacket.createErr(cmd.getTraCICmd().id, e.getMessageForClient());
 		}
 
+		logger.tracef("return response for: %s", cmd.getTraCICmd().logShort());
 		return response;
 	}
 }

VadereManager/src/org/vadere/manager/traci/commandHandler/CommandHandler.java

@@ -116,8 +116,11 @@ public abstract class CommandHandler<VAR extends Enum> {
 	}
 
 
-	public TraCICommand processValueSub(TraCICommand rawCmd, RemoteManager remoteManager,
-										TraCICmdHandler traCICmdHandler, TraCICmd getCommand, TraCICmd apiCmdResponse) {
+	public TraCICommand processValueSub(TraCICommand rawCmd,
+										RemoteManager remoteManager,
+										TraCICmdHandler traCICmdHandler,
+										TraCICmd getCommand,
+										TraCICmd apiCmdResponse) {
 		TraCIValueSubscriptionCommand cmd = (TraCIValueSubscriptionCommand) rawCmd;
 
 		List<TraCIGetCommand> getCommands = new ArrayList<>();

VadereManager/src/org/vadere/manager/traci/commandHandler/ControlCommandHandler.java

@@ -84,7 +84,9 @@ public class ControlCommandHandler extends CommandHandler<ControlVar> {
 		logger.debugf("%s: execute %d subscriptions",
 				TraCICmd.SIM_STEP.name(),
 				remoteManager.getSubscriptions().size());
-		remoteManager.getSubscriptions().forEach(sub -> sub.executeSubscription(remoteManager));
+		remoteManager.getSubscriptions().forEach(sub -> {
+			sub.executeSubscription(remoteManager);
+		});
 
 		// remove subscriptions no longer valid
 		remoteManager.getSubscriptions().removeIf(Subscription::isMarkedForRemoval);

VadereManager/src/org/vadere/manager/traci/commandHandler/PersonCommandHandler.java

@@ -210,8 +210,10 @@ public class PersonCommandHandler extends CommandHandler<PersonVar> {
 		remoteManager.accessState((manager, state) -> {
 			Pedestrian ped = state.getTopography().getPedestrianDynamicElements()
 					.getElement(Integer.parseInt(cmd.getElementIdentifier()));
-			if (checkIfPedestrianExists(ped, cmd))
-				cmd.setResponse(responseOK(PersonVar.SPEED.type, ped.getFreeFlowSpeed()));
+			if (checkIfPedestrianExists(ped, cmd)){
+				double speed = ped.getFootstepHistory().getAverageSpeedInMeterPerSecond();
+				cmd.setResponse(responseOK(PersonVar.SPEED.type, Double.isNaN(speed) ? 0.0 : speed));
+			}
 		});
 		return cmd;
 	}
@@ -366,7 +368,15 @@ public class PersonCommandHandler extends CommandHandler<PersonVar> {
 	@PersonHandler(cmd = TraCICmd.GET_PERSON_VALUE, var = PersonVar.ANGLE, name = "getAngle")
 	public TraCICommand process_getAngle(TraCIGetCommand cmd, RemoteManager remoteManager) {
 		// return dummy value
-		cmd.setResponse(responseOK(PersonVar.ANGLE.type, 0.0));
+		remoteManager.accessState((manager, state) -> {
+			Pedestrian ped = state.getTopography().getPedestrianDynamicElements()
+					.getElement(Integer.parseInt(cmd.getElementIdentifier()));
+
+			if (checkIfPedestrianExists(ped, cmd)){
+				double angle = ped.getFootstepHistory().getNorthBoundHeadingAngleDeg();
+				cmd.setResponse(responseOK(PersonVar.ANGLE.type, angle));
+			}
+		});
 		return cmd;
 	}
 
@@ -486,6 +496,10 @@ public class PersonCommandHandler extends CommandHandler<PersonVar> {
 		PersonVar var = PersonVar.fromId(getCmd.getVariableIdentifier());
 		Method m = getHandler(getCmd.getTraCICmd(), var);
 
+		logger.tracef("invokeHandler: PersonCommandHandler.%s [CMD: %s VAR: %s]",
+				m.getName(),
+				cmd.getTraCICmd().logShort(),
+				var.toString());
 		return invokeHandler(m, this, getCmd, remoteManager);
 	}
 

VadereManager/src/org/vadere/manager/traci/commandHandler/variables/SimulationVar.java

@@ -2,20 +2,26 @@ package org.vadere.manager.traci.commandHandler.variables;
 
 import org.vadere.manager.TraCIException;
 import org.vadere.manager.traci.TraCIDataType;
+import org.vadere.manager.traci.commands.TraCICommand;
 
 public enum SimulationVar {
 
 	CURR_SIM_TIME(0x66, TraCIDataType.DOUBLE),
+	VAR_DELTA_T(0x7b, TraCIDataType.DOUBLE), //step length
 	NUM_LOADED_VEHICLES(0x71, TraCIDataType.INTEGER),
 	LOADED_VEHICLES_IDS(0x72, TraCIDataType.STRING_LIST),
-	NUM_DEPARTED_VEHICLES(0x73, TraCIDataType.INTEGER),
-	DEPARTED_VEHICLES_IDS(0x74, TraCIDataType.STRING_LIST),
+	NUM_DEPARTED_PEDESTRIAN(0x73, TraCIDataType.INTEGER), // alias NUM_DEPARTED_VEHICLES
+	DEPARTED_PEDESTRIAN_IDS(0x74, TraCIDataType.STRING_LIST), // alias DEPARTED_VEHICLES_IDS
+	NUM_ARRIVED_PEDESTRIAN(0x79, TraCIDataType.INTEGER), // alias VAR_ARRIVED_VEHICLES_NUMBER
+	ARRIVED_PEDESTRIAN_PEDESTRIAN_IDS(0x7a, TraCIDataType.STRING_LIST), // alias VAR_ARRIVED_VEHICLES_IDS
+	POSITION_CONVERSION(0x82, TraCIDataType.COMPOUND_OBJECT),
 	NUM_VEHICLES_START_TELEPORT(0x75, TraCIDataType.INTEGER),
 	VEHICLES_START_TELEPORT_IDS(0x76, TraCIDataType.STRING_LIST),
 	NUM_VEHICLES_END_TELEPORT(0x77, TraCIDataType.INTEGER),
 	VEHICLES_END_TELEPORT_IDS(0x78, TraCIDataType.STRING_LIST),
 	VEHICLES_START_PARKING_IDS(0x6d, TraCIDataType.STRING_LIST),
 	VEHICLES_STOP_PARKING_IDS(0x6f, TraCIDataType.STRING_LIST),
+	COORD_REF(0x90, TraCIDataType.COMPOUND_OBJECT),
 	//
 	NETWORK_BOUNDING_BOX_2D(0x7c, TraCIDataType.POLYGON),
 	CACHE_HASH(0x7d, TraCIDataType.STRING),

VadereManager/src/org/vadere/manager/traci/commands/get/TraCIGetCompoundPayload.java

+package org.vadere.manager.traci.commands.get;
+
+import org.vadere.manager.TraCIException;
+import org.vadere.manager.traci.TraCICmd;
+import org.vadere.manager.traci.TraCIDataType;
+import org.vadere.manager.traci.commands.TraCIGetCommand;
+import org.vadere.manager.traci.compound.CompoundObject;
+
+public class TraCIGetCompoundPayload extends TraCIGetCommand {
+
+	private CompoundObject data;
+
+	public TraCIGetCompoundPayload(TraCICmd traCICmd, int variableIdentifier, String elementIdentifier) {
+		super(traCICmd, variableIdentifier, elementIdentifier);
+		data = null;
+	}
+
+	public TraCIGetCompoundPayload(TraCIGetCommand c) {
+		super(c.getTraCICmd(), c.getVariableIdentifier(), c.getElementIdentifier());
+		// expecting a CompoundObject thus check if DataType Byte is present.
+		c.getCmdBuffer().ensureBytes(1);
+		TraCIDataType dType = TraCIDataType.fromId(c.getCmdBuffer().readUnsignedByte());
+		if (!dType.equals(TraCIDataType.COMPOUND_OBJECT)){
+			throw new TraCIException("expected Compound Object in GetCommand.");
+		}
+		this.data = c.getCmdBuffer().readCompoundObject();
+	}
+
+	public CompoundObject getData() {
+		return data;
+	}
+
+	public void setData(CompoundObject data) {
+		this.data = data;
+	}
+}

VadereManager/src/org/vadere/manager/traci/compound/object/CoordRef.java

+package org.vadere.manager.traci.compound.object;
+
+import org.vadere.manager.TraCIException;
+import org.vadere.manager.traci.TraCIDataType;
+import org.vadere.manager.traci.compound.CompoundObject;
+import org.vadere.manager.traci.compound.CompoundObjectBuilder;
+import org.vadere.util.geometry.Vector3D;
+import org.vadere.util.geometry.shapes.VPoint;
+
+public class CoordRef {
+
+	private String epgsCode;
+	private VPoint offset;
+
+	public CoordRef(CompoundObject obj){
+		if (obj.size() != 3){
+			throw new TraCIException("Expected 4 elements for PointConverter");
+		}
+		epgsCode = (String) obj.getData(0, TraCIDataType.STRING);
+		VPoint p = new VPoint();
+		p.x = (Double) obj.getData(1, TraCIDataType.DOUBLE);
+		p.y = (Double) obj.getData(2, TraCIDataType.DOUBLE);
+		offset = p;
+	}
+
+	public CoordRef(String epgsCode, VPoint offset) {
+		this.epgsCode = epgsCode;
+		this.offset = offset;
+	}
+
+	public static CompoundObject asCompoundObject(String epgsCode,
+												  VPoint offset){
+		return CompoundObjectBuilder.builder()
+				.rest()
+				.add(TraCIDataType.STRING)
+				.add(TraCIDataType.DOUBLE, 2)
+				.build(epgsCode, offset.x, offset.y);
+	}
+}

VadereManager/src/org/vadere/manager/traci/compound/object/PointConverter.java

+package org.vadere.manager.traci.compound.object;
+
+import org.apache.commons.math3.util.Pair;
+import org.vadere.manager.TraCIException;
+import org.vadere.manager.traci.TraCIDataType;
+import org.vadere.manager.traci.compound.CompoundObject;
+import org.vadere.state.scenario.ReferenceCoordinateSystem;
+import org.vadere.util.geometry.shapes.VPoint;
+
+public class PointConverter {
+
+	enum PointType {
+		POSITION_LON_LAT,
+		POSITION_2D,
+	}
+
+	private VPoint p; // (lon, lat) Order
+	private PointType conversionType;
+
+
+	public PointConverter(CompoundObject obj){
+		if (obj.size() != 2)
+			throw new TraCIException("Expected 3 elements for PointConverter");
+		if (obj.hasIndex(0, TraCIDataType.POS_2D)){
+			p = (VPoint) obj.getData(0, TraCIDataType.POS_2D);
+		} else if (obj.hasIndex(0, TraCIDataType.POS_LON_LAT)){
+			p = (VPoint) obj.getData(0, TraCIDataType.POS_LON_LAT);
+		} else {
+			throw new TraCIException("Unknown PointType");
+		}
+		int type = (Integer) obj.getData(1, TraCIDataType.U_BYTE);
+		if (type == 0x00){
+			conversionType = PointType.POSITION_LON_LAT;
+		} else if (type == 0x01){
+			conversionType = PointType.POSITION_2D;
+		} else {
+			throw new TraCIException("Unknown ConversionType");
+		}
+	}
+
+	public Pair<TraCIDataType, VPoint> convert(ReferenceCoordinateSystem coord){
+		Pair<TraCIDataType, VPoint>ret;
+		switch (conversionType){
+			case POSITION_2D:
+				ret = Pair.create(TraCIDataType.POS_2D, coord.convertToCartesian(p.y, p.x)); // LAT LONG axis order!!!
+				break;
+			case POSITION_LON_LAT:
+				ret = Pair.create(TraCIDataType.POS_LON_LAT, coord.convertToGeo(new VPoint(p.x, p.y)));
+				break;
+			default:
+				throw new TraCIException("Unkown ConversionType");
+		}
+		return ret;
+	}
+
+	public VPoint getP() {
+		return p;
+	}
+
+	public void setP(VPoint p) {
+		this.p = p;
+	}
+
+	public PointType getConversionType() {
+		return conversionType;
+	}
+
+	public void setConversionType(PointType conversionType) {
+		this.conversionType = conversionType;
+	}
+}

VadereManager/src/org/vadere/manager/traci/reader/TraCIPacketBuffer.java

 package org.vadere.manager.traci.reader;
 
 import org.vadere.manager.traci.TraCICmd;
+import org.vadere.manager.traci.commandHandler.CommandExecutor;
 import org.vadere.manager.traci.commands.TraCICommand;
 import org.vadere.manager.traci.response.StatusResponse;
 import org.vadere.manager.traci.response.TraCIResponse;
+import org.vadere.util.logging.Logger;
 
 import java.nio.ByteBuffer;
 
@@ -16,6 +18,8 @@ import java.nio.ByteBuffer;
  */
 public class TraCIPacketBuffer extends TraCIByteBuffer {
 
+	private static Logger logger = Logger.getLogger(TraCIPacketBuffer.class);
+
 	protected TraCIPacketBuffer(byte[] buf) {
 		super(buf);
 	}
@@ -41,7 +45,7 @@ public class TraCIPacketBuffer extends TraCIByteBuffer {
 			return null;
 
 		int cmdLen = getCommandDataLen();
-
+		logger.tracef("read next command with length %d", cmdLen);
 		return TraCICommand.create(readByteBuffer(cmdLen));
 	}
 

VadereManager/tests/org/vadere/manager/traci/commandHandler/SimulationCommandHandlerTest.java

@@ -93,7 +93,7 @@ public class SimulationCommandHandlerTest extends CommandHandlerTest {
 				when(simState.getSimTimeInSec()).thenReturn(retVal);
 			}
 		};
-		TraCICommand ret = simCmdHandler.process_getSimTime(cmd, rm, var);
+		TraCICommand ret = simCmdHandler.process_getSimTime(cmd, rm);
 		checkGET_OK(ret);
 		testGetValue(ret, varID, varType, elementID, retVal);
 		

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

 package org.vadere.state.scenario;
 
+import org.apache.sis.geometry.DirectPosition2D;
+import org.apache.sis.referencing.CRS;
+import org.apache.sis.referencing.CommonCRS;
+import org.apache.sis.referencing.crs.DefaultProjectedCRS;
+import org.opengis.geometry.DirectPosition;
+import org.opengis.referencing.crs.CoordinateReferenceSystem;
+import org.opengis.referencing.operation.CoordinateOperation;
+import org.opengis.referencing.operation.TransformException;
+import org.opengis.util.FactoryException;
 import org.vadere.util.geometry.shapes.VPoint;
+import org.vadere.util.logging.Logger;
 
 import java.util.Objects;
 
 public class ReferenceCoordinateSystem {
 
+	private static Logger logger = Logger.getLogger(ReferenceCoordinateSystem.class);
+
 	/**
 	 * international identifier for the base coordinate reference system (CRS) which was used
 	 * to build this topography.
@@ -25,18 +37,43 @@ public class ReferenceCoordinateSystem {
 	 */
 	private String description = "";
 
+	transient private CoordinateReferenceSystem geoCRS;
+	transient private CoordinateReferenceSystem utmCRS;
+	transient private CoordinateOperation geoToUtm;
+	transient private CoordinateOperation utmToGeo;
+	transient private boolean initialized;
+
 	/**
 	 * Translation vector used to move topography to origin (0,0)
 	 */
 	private VPoint translation = new VPoint(0.0,0.0);
 
 	public ReferenceCoordinateSystem() {
+		this.initialized = false;
 	}
 
 	public ReferenceCoordinateSystem(String epsgCode, String description, VPoint translation) {
 		this.epsgCode = epsgCode;
 		this.description = description;
 		this.translation = translation;
+		this.initialized = false;
+	}
+
+	public void initialize(){
+		if (initialized)
+			return;
+
+		try {
+			utmCRS = CRS.forCode(epsgCode);
+			geoCRS = ((DefaultProjectedCRS) utmCRS).getBaseCRS();
+			geoToUtm = CRS.findOperation(geoCRS, utmCRS, null);
+			utmToGeo = CRS.findOperation(utmCRS, geoCRS, null);
+		} catch (FactoryException e) {
+			logger.errorf(e.getMessage());
+			throw new RuntimeException(e);
+		}
+
+		initialized = true;
 	}
 
 	public String getEpsgCode() {
@@ -63,6 +100,40 @@ public class ReferenceCoordinateSystem {
 		this.translation = translation;
 	}
 
+	public boolean supportsConversion(){
+		return geoToUtm != null && utmToGeo != null;
+	}
+
+	public VPoint convertToGeo(VPoint cartesian){
+		if (!initialized)
+			initialize();
+
+		VPoint translated  = cartesian.add(translation);
+		DirectPosition ptSrc = new DirectPosition2D(translated.x, translated.y);
+		try {
+			DirectPosition ptDst = utmToGeo.getMathTransform().transform(ptSrc, null);
+			return new VPoint(ptDst.getCoordinate()[0], ptDst.getCoordinate()[1]);
+		} catch (TransformException e) {
+			logger.errorf(e.getMessage());
+			throw new RuntimeException(e);
+		}
+	}
+
+	public VPoint convertToCartesian(double latitude, double longitude ){
+		if (!initialized)
+			initialize();
+		//  geographic CRS, the (latitude, longitude) axis order!
+		DirectPosition ptSrc = new DirectPosition2D(latitude, longitude);
+		try {
+			DirectPosition ptDst = geoToUtm.getMathTransform().transform(ptSrc, null);
+			VPoint ret =  new VPoint(ptDst.getCoordinate()[0], ptDst.getCoordinate()[1]);
+			return ret.subtract(this.translation);
+		} catch (TransformException e) {
+			logger.errorf(e.getMessage());
+			throw new RuntimeException(e);
+		}
+	}
+
 	@Override
 	public boolean equals(Object o) {
 		if (this == o) return true;

VadereState/src/org/vadere/state/simulation/FootstepHistory.java

 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.)