From 4afa3604d97ebb24688d04abc5161f546da49cba Mon Sep 17 00:00:00 2001 From: frievoe97 Date: Thu, 23 May 2024 14:18:31 +0200 Subject: [PATCH] fix calcCurvature bug --- .../sumo/SumoNetworkFeatureExtractor.java | 56 +++++++++++---- .../sumo/SumoNetworkFeatureExtractorTest.java | 70 +++++++++++++++++-- 2 files changed, 107 insertions(+), 19 deletions(-) diff --git a/contribs/sumo/src/main/java/org/matsim/contrib/sumo/SumoNetworkFeatureExtractor.java b/contribs/sumo/src/main/java/org/matsim/contrib/sumo/SumoNetworkFeatureExtractor.java index a060e98ee76..d973c2f2e90 100644 --- a/contribs/sumo/src/main/java/org/matsim/contrib/sumo/SumoNetworkFeatureExtractor.java +++ b/contribs/sumo/src/main/java/org/matsim/contrib/sumo/SumoNetworkFeatureExtractor.java @@ -78,33 +78,59 @@ private static Set 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 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 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. */ diff --git a/contribs/sumo/src/test/java/org/matsim/contrib/sumo/SumoNetworkFeatureExtractorTest.java b/contribs/sumo/src/test/java/org/matsim/contrib/sumo/SumoNetworkFeatureExtractorTest.java index c42ad51db5c..fe60d858450 100644 --- a/contribs/sumo/src/test/java/org/matsim/contrib/sumo/SumoNetworkFeatureExtractorTest.java +++ b/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); } }