Merge remote-tracking branch 'origin/network-improvments' into networ…

…k-improvments

contribs/sumo/src/main/java/org/matsim/contrib/sumo/SumoNetworkFeatureExtractor.java

@@ -80,33 +80,59 @@ private static Set<Character> directionSet(SumoNetworkHandler.Connection c) {
 	}
 
 	/**
-	 * Calculate the curvature of an edge.
+	 * Calculate the curvature of an edge. One gon is 1/400 of a full circle.
+	 * The formula is: KU = (Sum of the curvature of the subsegments) / (Length of the edge)
 	 *
-	 * @return KU in gon/km
+	 * @return curvature in gon/km
 	 */
 	static double calcCurvature(SumoNetworkHandler.Edge edge) {
+		double totalGon = 0;
+		List<double[]> coordinates = edge.shape;
 
-		// TODO: not finished and correctly tested
+		for (int i = 2; i < coordinates.size(); i++) {
+			double[] pointA = coordinates.get(i - 2);
+			double[] pointB = coordinates.get(i - 1);
+			double[] pointC = coordinates.get(i);
 
-		double gon = 0;
-		List<double[]> coords = edge.shape;
-		for (int i = 0; i < coords.size() - 2; i++) {
+			double[] vectorAB = {pointB[0] - pointA[0], pointB[1] - pointA[1]};
+			double[] vectorBC = {pointC[0] - pointB[0], pointC[1] - pointB[1]};
 
-			double[] a = coords.get(i);
-			double[] b = coords.get(i + 1);
-			double[] c = coords.get(i + 2);
+			double dotProduct = calcDotProduct(vectorAB, vectorBC);
+			double magnitudeAB = calcMagnitude(vectorAB);
+			double magnitudeBC = calcMagnitude(vectorBC);
 
-			double ab = Math.sqrt(Math.pow(b[0] - a[0], 2) + Math.pow(b[1] - a[1], 2));
-			double bc = Math.sqrt(Math.pow(c[0] - b[0], 2) + Math.pow(c[1] - b[1], 2));
-			double ac = Math.sqrt(Math.pow(c[0] - a[0], 2) + Math.pow(c[1] - a[1], 2));
+			double cosine = dotProduct / (magnitudeAB * magnitudeBC);
+			double angleRadians = Math.acos(cosine);
+			double angleDegrees = Math.toDegrees(angleRadians);
 
-			double angle = Math.acos((ac * ac - ab * ab - bc * bc) / (-2 * ab * bc));
-			gon += Math.abs(angle) * 200 / Math.PI;
+			totalGon += Math.abs((angleDegrees / 360) * 400);
 		}
 
-		return gon / (edge.getLength() * 1000);
+		return totalGon / (edge.getLength() / 1000);
 	}
 
+	/**
+	 * Calculate the dot product of two vectors.
+	 *
+	 * @param vec1 vector 1
+	 * @param vec2 vector 2
+	 * @return dot product
+	 */
+	private static double calcDotProduct(double[] vec1, double[] vec2) {
+		return vec1[0] * vec2[0] + vec1[1] * vec2[1];
+	}
+
+	/**
+	 * Calculate the magnitude of a vector.
+	 *
+	 * @param vec vector
+	 * @return magnitude
+	 */
+	private static double calcMagnitude(double[] vec) {
+		return Math.sqrt(vec[0] * vec[0] + vec[1] * vec[1]);
+	}
+
+
 	/**
 	 * Get priority. Higher is more important.
 	 */

contribs/sumo/src/test/java/org/matsim/contrib/sumo/SumoNetworkFeatureExtractorTest.java

@@ -1,21 +1,83 @@
 package org.matsim.contrib.sumo;
 
+import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.Test;
 
 class SumoNetworkFeatureExtractorTest {
 
 	@Test
-	void curvature() throws Exception {
-
+	void twoCoordsBackAndForth() {
 		String[] coords = {"0,0", "0,100", "0,0"};
+		int lentgh = 200;
+
 		SumoNetworkHandler.Edge edge = new SumoNetworkHandler.Edge("1", "2", "3", "highway", 0, "name", coords);
-		edge.lanes.add(new SumoNetworkHandler.Lane("1", 0, 200, 50/3.6, null, null));
+		edge.lanes.add(new SumoNetworkHandler.Lane("1", 0, lentgh, 50 / 3.6, null, null));
 
 		double ku = SumoNetworkFeatureExtractor.calcCurvature(edge);
 
-		//TODO should be around 400 for 200m road
+		//TODO should be around 200 for 200m road
+
+		// Length: 0.2 km
+		// Gon: 200
+		// Curvature: 200 / 0.2 = 1000
+
+		Assertions.assertEquals(1000, ku, 0.000001);
 
 		System.out.println(ku);
+	}
+
+
+	@Test
+	void nintyDegreeCorner() {
+
+		String[] coords = {"0,0", "0,100", "100,100"};
+		int lentgh = 200;
+
+		SumoNetworkHandler.Edge edge = new SumoNetworkHandler.Edge("1", "2", "3", "highway", 0, "name", coords);
+		edge.lanes.add(new SumoNetworkHandler.Lane("1", 0, lentgh, 50 / 3.6, null, null));
+
+		double ku = SumoNetworkFeatureExtractor.calcCurvature(edge);
+
+		// Length: 0.2 km
+		// Gon: 100
+		// Curvature: 100 / 0.2 = 500
+
+		Assertions.assertEquals(500, ku, 0.000001);
+	}
+
+	@Test
+	void twoCorners() {
+
+		String[] coords = {"0,0", "0,100", "100,100", "100,200"};
+		int lentgh = 300;
+
+		SumoNetworkHandler.Edge edge = new SumoNetworkHandler.Edge("1", "2", "3", "highway", 0, "name", coords);
+		edge.lanes.add(new SumoNetworkHandler.Lane("1", 0, lentgh, 50 / 3.6, null, null));
+
+		double ku = SumoNetworkFeatureExtractor.calcCurvature(edge);
+
+		// Length: 0.3 km
+		// Gon: 100 + 100 = 200
+		// Curvature: 200 / 0.3 = 666.6666666666666
+
+		Assertions.assertEquals((200 / 0.3), ku, 0.000001);
+	}
+
+	@Test
+	void rectangle() {
+
+		String[] coords = {"0,0", "0,100", "100,100", "100,0", "0,0"};
+		int lentgh = 400;
+
+		SumoNetworkHandler.Edge edge = new SumoNetworkHandler.Edge("1", "2", "3", "highway", 0, "name", coords);
+		edge.lanes.add(new SumoNetworkHandler.Lane("1", 0, lentgh, 50 / 3.6, null, null));
+
+		double ku = SumoNetworkFeatureExtractor.calcCurvature(edge);
+
+		// Length: 0.4 km
+		// Gon: 100 + 100 + 100 = 300
+		// Curvature: 300 / 0.4 = 666.6666666666666
 
+		Assertions.assertEquals((300 / 0.4), ku, 0.000001);
 	}
 }