From 7fda51881e3f9110f73b3976a0016e5c4dec2818 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Mon, 2 Sep 2024 10:52:49 +0200 Subject: [PATCH 1/5] fix: improved EdgeFinder in cases where adjacent connections share the same FROM and TO points MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../mosaic/lib/database/spatial/Edge.java | 7 ++ .../lib/database/spatial/EdgeFinder.java | 50 +++++++++-- .../lib/database/spatial/EdgeFinderTest.java | 30 ++++++- .../src/test/resources/edgeFinderTest.db | Bin 0 -> 73728 bytes .../lib/spatial/SpatialTreeTraverser.java | 60 +++++++++++++ .../lib/routing/database/DatabaseRouting.java | 13 +-- .../routing/database/DatabaseRoutingTest.java | 82 +++++++++--------- 7 files changed, 187 insertions(+), 55 deletions(-) create mode 100644 lib/mosaic-database/src/test/resources/edgeFinderTest.db diff --git a/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/Edge.java b/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/Edge.java index 350e95274..3b5385260 100644 --- a/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/Edge.java +++ b/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/Edge.java @@ -83,4 +83,11 @@ protected double getCenterDistanceSqr(E item, SpatialTree tree) { } } + static class KNearest> extends SpatialTreeTraverser.KNearest { + @Override + protected double getCenterDistanceSqr(E item, SpatialTree tree) { + return item.getNearestPointOnEdge(center).distanceSqrTo(center); + } + } + } diff --git a/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java b/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java index 887d656ab..16d8d42d3 100644 --- a/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java +++ b/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java @@ -20,22 +20,31 @@ import org.eclipse.mosaic.lib.database.road.Node; import org.eclipse.mosaic.lib.geo.GeoPoint; import org.eclipse.mosaic.lib.math.Vector3d; +import org.eclipse.mosaic.lib.math.VectorUtils; import org.eclipse.mosaic.lib.spatial.KdTree; import org.eclipse.mosaic.lib.spatial.SpatialItemAdapter; import org.eclipse.mosaic.lib.spatial.SpatialTreeTraverser; +import com.google.common.collect.Lists; import edu.umd.cs.findbugs.annotations.SuppressWarnings; import java.util.ArrayList; import java.util.List; +import java.util.stream.Collectors; /** * Edge Finder searches for the closest edge to a specified geo location. */ public class EdgeFinder { + /** + * In order to handle cases were adjacent edges use the same FROM- and TO-Nodes, + * we return the two nearest edges, which allows more concrete handling in later applications. + */ + private static final int K_EDGES = 2; + private final KdTree edgeIndex; - private final SpatialTreeTraverser.Nearest edgeSearch; + private final SpatialTreeTraverser.KNearest edgeSearch; /** * Constructs a new edgeFinder object with the specified database. @@ -43,7 +52,7 @@ public class EdgeFinder { * @param database Database which contains all connections. */ public EdgeFinder(Database database) { - List items = new ArrayList<>(); + List items = new ArrayList<>(); for (Connection con : database.getConnections()) { for (int i = 0; i < con.getNodes().size() - 1; i++) { @@ -53,7 +62,7 @@ public EdgeFinder(Database database) { } } edgeIndex = new KdTree<>(new SpatialItemAdapter.EdgeAdapter<>(), items); - edgeSearch = new org.eclipse.mosaic.lib.database.spatial.Edge.Nearest<>(); + edgeSearch = new org.eclipse.mosaic.lib.database.spatial.Edge.KNearest<>(); } /** @@ -61,16 +70,41 @@ public EdgeFinder(Database database) { * * @return Closest edge to the given location. */ - public Edge findClosestEdge(GeoPoint location) { + public List findClosestEdge(GeoPoint location) { synchronized (edgeSearch) { - edgeSearch.setup(location.toVector3d()); + Vector3d locationVector = location.toVector3d(); + edgeSearch.setup(locationVector, K_EDGES); edgeSearch.traverse(edgeIndex); - EdgeWrapper result = edgeSearch.getNearest(); - if (result == null) { + List result = edgeSearch.getKNearest(); + if (result == null || result.isEmpty()) { return null; } - return result.edge; + + EdgeWrapper edgeWrapper0 = result.get(0); + EdgeWrapper edgeWrapper1 = result.get(1); + Connection connection0 = edgeWrapper0.edge.getConnection(); + Connection connection1 = edgeWrapper1.edge.getConnection(); + // check if roads are adjacent + if (connection0.getFrom() == connection1.getTo() && connection0.getTo() == connection1.getFrom() + && connection0.getNodes().size() == connection1.getNodes().size()) { + Vector3d origin0 = connection0.getFrom().getPosition().toVector3d(); + Vector3d direction0 = connection0.getTo().getPosition().toVector3d().subtract(origin0, new Vector3d()); + Vector3d origin1 = connection1.getFrom().getPosition().toVector3d(); + Vector3d direction1 = connection1.getTo().getPosition().toVector3d().subtract(origin1, new Vector3d()); + if (!VectorUtils.isLeftOfLine(locationVector, origin0, direction0)) { + // if location is right of first connection, return first one + return Lists.newArrayList(edgeWrapper0.edge); + } else if (!VectorUtils.isLeftOfLine(locationVector, origin1, direction1)) { + // if location is right of second connection, return second one + return Lists.newArrayList(edgeWrapper1.edge); + } else { + // TODO: this should probably be the first if-clause and the dot product should be checked with a fuzzy-equals to zero + return result.stream().map(wrapper -> wrapper.edge).collect(Collectors.toList()); + } + } else { + return Lists.newArrayList(edgeWrapper0.edge); + } } } diff --git a/lib/mosaic-database/src/test/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinderTest.java b/lib/mosaic-database/src/test/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinderTest.java index 05e6fde92..aded96c09 100644 --- a/lib/mosaic-database/src/test/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinderTest.java +++ b/lib/mosaic-database/src/test/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinderTest.java @@ -23,15 +23,21 @@ import org.eclipse.mosaic.lib.junit.GeoProjectionRule; import org.eclipse.mosaic.lib.util.junit.TestFileRule; +import com.google.common.collect.Iterables; import org.junit.Rule; import org.junit.Test; +import java.util.List; + public class EdgeFinderTest { @Rule public TestFileRule rule = new TestFileRule() .with("tiergarten.db", "/tiergarten.db"); + @Rule + public TestFileRule rule2 = new TestFileRule().with("edgeFinderTest.db", "/edgeFinderTest.db"); + @Rule public GeoProjectionRule projectionRule = new GeoProjectionRule(GeoPoint.latLon(52, 13)); @@ -43,11 +49,31 @@ public void findClosestEdge() { final EdgeFinder edgeFinder = new EdgeFinder(db); - // RUN + ASSERT - Edge edge = edgeFinder.findClosestEdge(GeoPoint.latLon(52.51303, 13.32743)); + // RUN + Edge edge = Iterables.getOnlyElement(edgeFinder.findClosestEdge(GeoPoint.latLon(52.51303, 13.32743))); + + // ASSERT assertEquals("36337926_408194196_408194192", edge.getConnection().getId()); assertEquals("410846037", edge.getPreviousNode().getId()); } + @Test + public void findClosestEdge2() { + // SETUP + Database db = Database.loadFromFile(rule2.get("edgeFinderTest.db")); + assertFalse(db.getConnections().isEmpty()); + + final EdgeFinder edgeFinder = new EdgeFinder(db); + + // RUN + List edgesWest = edgeFinder.findClosestEdge(GeoPoint.latLon(0.0, 10.510506)); + List edgesEast = edgeFinder.findClosestEdge(GeoPoint.latLon(0.0, 10.511805)); + Edge edgeWest = Iterables.getOnlyElement(edgesWest); + Edge edgeEast = Iterables.getOnlyElement(edgesEast); + // ASSERT + assertEquals("E0", edgeEast.getConnection().getId()); + assertEquals("-E0", edgeWest.getConnection().getId()); + } + } \ No newline at end of file diff --git a/lib/mosaic-database/src/test/resources/edgeFinderTest.db b/lib/mosaic-database/src/test/resources/edgeFinderTest.db new file mode 100644 index 0000000000000000000000000000000000000000..420fd69270e07d99afcf022896edcf5b5b1959a7 GIT binary patch literal 73728 zcmeI*&u`Oa7{GBm{o$m)+z^-r2+b8jG@?K=K$<2^v_J+FlmaaRX`3?BxWyx}i|v%L z9az|b9VRZ*r2PxKOxj^*4jeY7X+oNW5T{Mzw2dRuE}Qn+aT2>tr)?M1?rY)l`nBWd zeLjBkHm_sB-1)PXZ|aMVTQhw9kTfdEvNWaZk|f;`>lU%LmXAfDx3wYumSg4hig%;~ z?>-z>f0we#SCU#&?~nXAa$@-V;quUTL$43MI`C-V)6AclFETHsKTD5`Ls$?%009IL zK%kpIJ*#N@kI0Q}mR&L5bDg?fF|IguzZ`B~Xm1~PY|rw1Z+5Zw_=V#1d{IALI#Il= zuYQH@*m}2DjmHnHd-Gb*ka)q1m>Z;Xy8T0x|HCTzjl=B}+7O|)U{em$OH6tOBka*1nmc0#WPS0OBU z=JmR1FPLS!Ub|wt;;PLTPZlra=Z5>WUAyED5)EpFQM(3@b?5ZYp!VIqyzMXIW zodHGDG`aC|>m0r*UhbPit6wQT`gS_UIt_~VnpW%cw^JGMhRDkw9SuXwyT;<8wJ>Kb z*+$h%ZVe`aCilgnyXIm!3AB9G@Jz4OQLUPI!$lj5u2U=DFmAS*Goi0o#^josLbAqp zlG|0oHobCiu6BP6DuPq3t|%lbd?(z@HwMy*wtKhSc(dtjdcJFg@6-#O;y@z(*p684 zo>O;4mtURHDi7aqmrOr-RQvra`mz%5%big`l%kQaTU{p6ylzM6;|uLkqXVA2RO@t| z{Iz~1=*x|<5GZl>XiQ756F;VtU{5Btj;3>EnrD|18PIP_Lrpa&qHVE3i0c|LP#Igkqyn`}$4j-m+;{TH|5RHQ#XD z8-`nnB^%TpGZwBTs~y`Et=5mt&Ymq!m+}p*N71%zlW%9k=9I*s6RdrS1_d>-7Ihnm z&8iu-#`4O3^9^3EhJ$AmYC;!#EZa9-ulWnA)rN4#l4+Z+;ftJFEt{9ak*yxK>zwN> zo33w}UZ<2ww6Rm)ZmE8;bo%_oqIkb|epww#^=l``WGVd3>|L*l`Fz=^`%buB?wq7t z=#+cJM~igu@nTqgCZ*JSl6p^l-0kBAbqFAU00IagfB*srAbZHv}cbTzZH-g=-HWxZUY4KfAP?N zumsQgXT=(91!Xnb4(bM@bttM2_6Nrei*-b-!Se(x2q1s}0tg_000IagfB*srAn*bO z{%-ysOyE=MLrMKj{9r)<0R#|0009ILKmY**5I_Kd4I!{2mChT6>1?{WZ!)Z#nA+N$|33(x|9`L{5~Ps`Abl^*7tutwL?zln!EO|*%!?Jd(}sh`ds}*{Yey9 z5I_I{1Q0*~0R#|0009ILKwyIiqn&W-^oN&1Ey<4uB}iqL>bf zh35DFk0te2{{8 { @@ -166,6 +168,64 @@ protected void traverseLeaf(SpatialTree.Node node, SpatialTree tree) { } } + public static class KNearest extends CenterDistanceBased { + private int k; + private PriorityQueue> maxHeap; + + public KNearest setup(Vector3d center, int k) { + setCenter(center); + this.k = k; + this.maxHeap = new PriorityQueue<>(Comparator.comparingDouble(neighbor -> -neighbor.distance)); + return this; + } + + public List getKNearest() { + List result = new ArrayList<>(); + while (!maxHeap.isEmpty()) { + result.add(maxHeap.poll().item); + } + return result; + } + + @Override + protected void traverseChildren(SpatialTree.Node node, SpatialTree tree) { + List.Node> children = node.getChildren(); + for (int i = 0; i < children.size(); i++) { + SpatialTree.Node child = children.get(i); + traverseNode(child, tree); + + if (maxHeap.size() < k || child.getBounds().distanceSqrToPoint(center) < maxHeap.peek().distance) { + traverseNode(child, tree); + } + } + } + + @Override + protected void traverseLeaf(SpatialTree.Node node, SpatialTree tree) { + List items = node.getItems(); + for (int i = 0; i < items.size(); i++) { + T item = items.get(i); + double distance = getCenterDistanceSqr(item, tree); + if (maxHeap.size() < k) { + maxHeap.add(new Neighbor<>(item, distance)); + } else if (distance < maxHeap.peek().distance) { + maxHeap.poll(); + maxHeap.add(new Neighbor<>(item, distance)); + } + } + } + + private static class Neighbor { + T item; + double distance; + + Neighbor(T item, double distance) { + this.item = item; + this.distance = distance; + } + } + } + public abstract static class InPolygon

, T extends Polygon

> extends Nearest { private P search; diff --git a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java index 4b9e8409f..36e7442d9 100644 --- a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java +++ b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java @@ -210,7 +210,7 @@ public IRoadPosition refineRoadPosition(IRoadPosition roadPosition) { public CandidateRoute approximateCostsForCandidateRoute(CandidateRoute route, String lastNodeId) { double length = 0; double time = 0; - for (String connectionId: route.getConnectionIds()) { + for (String connectionId : route.getConnectionIds()) { Connection con = getScenarioDatabase().getConnection(connectionId); length += con.getLength(); time += con.getLength() / con.getMaxSpeedInMs(); @@ -218,7 +218,7 @@ public CandidateRoute approximateCostsForCandidateRoute(CandidateRoute route, St return new CandidateRoute(route.getConnectionIds(), length, time); } - private Edge findClosestEdge(GeoPoint location) { + private List findClosestEdge(GeoPoint location) { if (edgeFinder == null) { edgeFinder = new EdgeFinder(scenarioDatabase); } @@ -232,11 +232,14 @@ private Edge findClosestEdge(GeoPoint location) { */ @Override public IRoadPosition findClosestRoadPosition(GeoPoint location) { - Edge closestEdge = findClosestEdge(location); - if (closestEdge == null) { + List closestEdges = findClosestEdge(location); + if (closestEdges == null || closestEdges.isEmpty()) { return null; } - + if (closestEdges.size() > 1) { + log.info("findClosestRoadPosition returned more than one edge, returning first result."); + } + Edge closestEdge = closestEdges.get(0); LazyLoadingConnection connection = new LazyLoadingConnection(closestEdge.getConnection()); LazyLoadingNode previousNode = new LazyLoadingNode(closestEdge.getPreviousNode()); LazyLoadingNode upcomingNode = new LazyLoadingNode(closestEdge.getNextNode()); diff --git a/lib/mosaic-routing/src/test/java/org/eclipse/mosaic/lib/routing/database/DatabaseRoutingTest.java b/lib/mosaic-routing/src/test/java/org/eclipse/mosaic/lib/routing/database/DatabaseRoutingTest.java index 3bf4bd85b..66b283cc9 100644 --- a/lib/mosaic-routing/src/test/java/org/eclipse/mosaic/lib/routing/database/DatabaseRoutingTest.java +++ b/lib/mosaic-routing/src/test/java/org/eclipse/mosaic/lib/routing/database/DatabaseRoutingTest.java @@ -65,7 +65,7 @@ public class DatabaseRoutingTest { private final static String dbFile = "/tiergarten.db"; - private DatabaseRouting routingAPIScenarioDatabase; + private DatabaseRouting databaseRouting; private CRouting configuration; private File cfgDir; @@ -81,7 +81,7 @@ public void setup() throws IOException { FileUtils.copyInputStreamToFile(getClass().getResourceAsStream(dbFile), dbFileCopy); configuration = new CRouting(); - routingAPIScenarioDatabase = new DatabaseRouting(); + databaseRouting = new DatabaseRouting(); } @@ -91,10 +91,10 @@ public void initialize_seekDatabase() throws InternalFederateException { configuration.source = null; //RUN - routingAPIScenarioDatabase.initialize(configuration, cfgDir); + databaseRouting.initialize(configuration, cfgDir); //ASSERT - assertEquals(1, routingAPIScenarioDatabase.getRoutesFromDatabaseForMessage().size()); + assertEquals(1, databaseRouting.getRoutesFromDatabaseForMessage().size()); } @Test @@ -103,10 +103,10 @@ public void initialize_locateDatabase() throws InternalFederateException { configuration.source = "tiergarten.db"; //RUN - routingAPIScenarioDatabase.initialize(configuration, cfgDir); + databaseRouting.initialize(configuration, cfgDir); //ASSERT - assertEquals(1, routingAPIScenarioDatabase.getRoutesFromDatabaseForMessage().size()); + assertEquals(1, databaseRouting.getRoutesFromDatabaseForMessage().size()); } @Test(expected = InternalFederateException.class) @@ -115,29 +115,30 @@ public void initialize_locateDatabase_error() throws InternalFederateException { configuration.source = "tiergarten_not_found.db"; //RUN -> Throw error - routingAPIScenarioDatabase.initialize(configuration, cfgDir); + databaseRouting.initialize(configuration, cfgDir); } @Test public void getRoutesFromDatabaseForMessage_routesCorrectlyLoaded() throws InternalFederateException { //PREPARE configuration.source = "tiergarten.db"; - routingAPIScenarioDatabase.initialize(configuration, cfgDir); + databaseRouting.initialize(configuration, cfgDir); final String routeID = "0"; //RUN - Map routes = routingAPIScenarioDatabase.getRoutesFromDatabaseForMessage(); + Map routes = databaseRouting.getRoutesFromDatabaseForMessage(); //ASSERT assertEquals(1, routes.size()); assertNotNull(routes.get(routeID)); - assertEquals(Arrays.asList("26704482", "26938219", "26938220", "26785753", "26785752", "21487147", "26704584", "26938208", "26938209", - "21487146", "281787666", "281787664", "21487168", "26938204", "251150126", "21487167", "272365223", "428788319", "272256206", - "408194194", "26738489", "313006383", "423839224", "26704448", "27537750", "27537749", "252864801", "265786533", "252864802", - "341364279", "248919692", "82654385", "27011308", "341364277", "341364270", "27011305", "540312558", "281787656", "281787657", - "21487170", "27011237", "27423744", "27011241", "299080425", "26703663", "299080426", "21487176", "21487175", "197687090", - "342813080", "27011231", "21487174", "27011842", "27011256", "26873451", "406585016", "414959615", "82654384", "564738832", - "249734328", "26873453", "152533555", "417709064", "391498256", "26873454"), + assertEquals(Arrays.asList("26704482", "26938219", "26938220", "26785753", "26785752", "21487147", "26704584", "26938208", + "26938209", "21487146", "281787666", "281787664", "21487168", "26938204", "251150126", "21487167", "272365223", + "428788319", "272256206", "408194194", "26738489", "313006383", "423839224", "26704448", "27537750", "27537749", + "252864801", "265786533", "252864802", "341364279", "248919692", "82654385", "27011308", "341364277", "341364270", + "27011305", "540312558", "281787656", "281787657", "21487170", "27011237", "27423744", "27011241", "299080425", + "26703663", "299080426", "21487176", "21487175", "197687090", "342813080", "27011231", "21487174", "27011842", + "27011256", "26873451", "406585016", "414959615", "82654384", "564738832", "249734328", "26873453", "152533555", + "417709064", "391498256", "26873454"), routes.get(routeID).getNodeIds()); } @@ -145,10 +146,10 @@ public void getRoutesFromDatabaseForMessage_routesCorrectlyLoaded() throws Inter public void getMaxSpeedOfConnection() throws InternalFederateException { //PREPARE configuration.source = "tiergarten.db"; - routingAPIScenarioDatabase.initialize(configuration, cfgDir); + databaseRouting.initialize(configuration, cfgDir); //RUN - double speed = routingAPIScenarioDatabase.getMaxSpeedOfConnection("32909782_26704482_26785753"); + double speed = databaseRouting.getMaxSpeedOfConnection("32909782_26704482_26785753"); //ASSERT assertEquals(22.222, speed, 0.001d); @@ -158,10 +159,10 @@ public void getMaxSpeedOfConnection() throws InternalFederateException { public void getMaxSpeedOfConnection_noSuchConnection_noException() throws InternalFederateException { //PREPARE configuration.source = "tiergarten.db"; - routingAPIScenarioDatabase.initialize(configuration, cfgDir); + databaseRouting.initialize(configuration, cfgDir); //RUN - double speed = routingAPIScenarioDatabase.getMaxSpeedOfConnection("32909782_26704482_0"); + double speed = databaseRouting.getMaxSpeedOfConnection("32909782_26704482_0"); //ASSERT assertEquals(0, speed, 0.001d); @@ -171,10 +172,10 @@ public void getMaxSpeedOfConnection_noSuchConnection_noException() throws Intern public void getPositionOfNode() throws InternalFederateException { //PREPARE configuration.source = "tiergarten.db"; - routingAPIScenarioDatabase.initialize(configuration, cfgDir); + databaseRouting.initialize(configuration, cfgDir); //RUN - GeoPoint gp = routingAPIScenarioDatabase.getNode("21487171").getPosition(); + GeoPoint gp = databaseRouting.getNode("21487171").getPosition(); //ASSERT assertEquals(52.515, gp.getLatitude(), 0.001d); @@ -182,21 +183,22 @@ public void getPositionOfNode() throws InternalFederateException { } @Test - public void createRouteForRTI() throws InternalFederateException, IllegalRouteException { + public void createRouteForRti() throws InternalFederateException, IllegalRouteException { //PREPARE configuration.source = "tiergarten.db"; - routingAPIScenarioDatabase.initialize(configuration, cfgDir); + databaseRouting.initialize(configuration, cfgDir); final CandidateRoute candidateRoute = new CandidateRoute( - Arrays.asList("4068038_423839224_26704448", "36337928_26704448_27537750", "4609244_27537750_27537749", "4609243_27537749_252864801", "4609243_252864801_252864802"), 0, 0); + Arrays.asList("4068038_423839224_26704448", "36337928_26704448_27537750", "4609244_27537750_27537749", + "4609243_27537749_252864801", "4609243_252864801_252864802"), 0, 0); //RUN - final VehicleRoute route = routingAPIScenarioDatabase.createRouteForRTI(candidateRoute); + final VehicleRoute route = databaseRouting.createRouteForRTI(candidateRoute); //ASSERT assertEquals(376.4d, route.getLength(), 0.1d); assertEquals(candidateRoute.getConnectionIds(), route.getConnectionIds()); assertEquals(Arrays.asList("4068038_423839224_26704448", "36337928_26704448_27537750", - "4609244_27537750_27537749", "4609243_27537749_252864801", "4609243_252864801_252864802"), + "4609244_27537750_27537749", "4609243_27537749_252864801", "4609243_252864801_252864802"), route.getConnectionIds()); } @@ -204,27 +206,29 @@ public void createRouteForRTI() throws InternalFederateException, IllegalRouteEx public void createRouteForRTI_falseCandidateRoute() throws InternalFederateException, IllegalRouteException { //PREPARE configuration.source = "tiergarten.db"; - routingAPIScenarioDatabase.initialize(configuration, cfgDir); + databaseRouting.initialize(configuration, cfgDir); final CandidateRoute candidateRoute = new CandidateRoute( Arrays.asList("4068038_423839224_26704448", "36337928_26704448_27537750", "4609244_27537750_27537749", /* "4609243_27537749_252864801", */ "4609243_252864801_252864802"), 0, 0); //RUN (throw error) - routingAPIScenarioDatabase.createRouteForRTI(candidateRoute); + databaseRouting.createRouteForRTI(candidateRoute); } @Test public void findRoutes() throws InternalFederateException { //PREPARE configuration.source = "tiergarten.db"; - routingAPIScenarioDatabase.initialize(configuration, cfgDir); + databaseRouting.initialize(configuration, cfgDir); - final RoutingParameters routingParameters = new RoutingParameters().alternativeRoutes(0).costFunction(RoutingCostFunction.Shortest); - final GeoPoint start = routingAPIScenarioDatabase.getNode("26704482").getPosition(); - final GeoPoint target = routingAPIScenarioDatabase.getNode("26704584").getPosition(); + final RoutingParameters routingParameters = new RoutingParameters() + .alternativeRoutes(0) + .costFunction(RoutingCostFunction.Shortest); + final GeoPoint start = databaseRouting.getNode("26704482").getPosition(); + final GeoPoint target = databaseRouting.getNode("26704584").getPosition(); final RoutingRequest request = new RoutingRequest(new RoutingPosition(start), new RoutingPosition(target), routingParameters); //RUN - final RoutingResponse response = routingAPIScenarioDatabase.findRoutes(request); + final RoutingResponse response = databaseRouting.findRoutes(request); //ASSERT assertEquals(0, response.getAlternativeRoutes().size()); @@ -239,15 +243,14 @@ public void findRoutes() throws InternalFederateException { public void getPositionOfNode_noSuchNode() throws InternalFederateException { //PREPARE configuration.source = "tiergarten.db"; - routingAPIScenarioDatabase.initialize(configuration, cfgDir); + databaseRouting.initialize(configuration, cfgDir); //RUN -> throw error - routingAPIScenarioDatabase.getNode("1234").getPosition(); + databaseRouting.getNode("1234").getPosition(); } @Test - public void testApproximateCostsForCandidateRoute() throws Exception { - + public void testApproximateCostsForCandidateRoute() { Node node0 = new Node("0", GeoPoint.lonLat(0.0, 0.0)); //node on route but vehicle already passed it Node node1 = new Node("1", GeoPoint.lonLat(1.0, 1.0)); Node node2 = new Node("2", GeoPoint.lonLat(2.0, 2.0)); //node on Route but no intersection @@ -268,7 +271,6 @@ public void testApproximateCostsForCandidateRoute() throws Exception { node1ToNode3.addNode(node1); node1ToNode3.addNode(node3); - Connection node3ToNode4 = new Connection("3-4", someWay); node3ToNode4.setLength(8.0); node3ToNode4.addNode(node3); @@ -279,7 +281,7 @@ public void testApproximateCostsForCandidateRoute() throws Exception { node3.addConnection(node3ToNode4); node4.addConnection(node3ToNode4); - DatabaseRouting spyRSDB = Mockito.spy(routingAPIScenarioDatabase); + DatabaseRouting spyRSDB = Mockito.spy(databaseRouting); Mockito.doReturn(dbMock).when(spyRSDB).getScenarioDatabase(); Mockito.when(dbMock.getConnection("0-1")).thenReturn(node0ToNode1); From e81767ab879c4662c7332a5759bfa30d08c7d289 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Mon, 2 Sep 2024 12:13:23 +0200 Subject: [PATCH 2/5] fix: adjusted logic a little bit MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../mosaic/lib/database/spatial/EdgeFinder.java | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java b/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java index 16d8d42d3..8566809cc 100644 --- a/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java +++ b/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java @@ -30,7 +30,6 @@ import java.util.ArrayList; import java.util.List; -import java.util.stream.Collectors; /** * Edge Finder searches for the closest edge to a specified geo location. @@ -80,7 +79,6 @@ public List findClosestEdge(GeoPoint location) { if (result == null || result.isEmpty()) { return null; } - EdgeWrapper edgeWrapper0 = result.get(0); EdgeWrapper edgeWrapper1 = result.get(1); Connection connection0 = edgeWrapper0.edge.getConnection(); @@ -92,16 +90,16 @@ public List findClosestEdge(GeoPoint location) { Vector3d direction0 = connection0.getTo().getPosition().toVector3d().subtract(origin0, new Vector3d()); Vector3d origin1 = connection1.getFrom().getPosition().toVector3d(); Vector3d direction1 = connection1.getTo().getPosition().toVector3d().subtract(origin1, new Vector3d()); + List resultEdges = new ArrayList<>(); if (!VectorUtils.isLeftOfLine(locationVector, origin0, direction0)) { // if location is right of first connection, return first one - return Lists.newArrayList(edgeWrapper0.edge); - } else if (!VectorUtils.isLeftOfLine(locationVector, origin1, direction1)) { + resultEdges.add(edgeWrapper0.edge); + } + if (!VectorUtils.isLeftOfLine(locationVector, origin1, direction1)) { // if location is right of second connection, return second one - return Lists.newArrayList(edgeWrapper1.edge); - } else { - // TODO: this should probably be the first if-clause and the dot product should be checked with a fuzzy-equals to zero - return result.stream().map(wrapper -> wrapper.edge).collect(Collectors.toList()); + resultEdges.add(edgeWrapper1.edge); } + return resultEdges; } else { return Lists.newArrayList(edgeWrapper0.edge); } From 4f52adf49bcdcd102b45b39e7a75452084e5dd71 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Tue, 3 Sep 2024 16:04:57 +0200 Subject: [PATCH 3/5] feat: re-added findClosestEdge for EdgeFinder but it additionally takes a heading now MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../lib/database/spatial/EdgeFinder.java | 104 +++++++++++++----- .../lib/database/spatial/EdgeFinderTest.java | 26 ++++- .../lib/routing/database/DatabaseRouting.java | 8 +- 3 files changed, 103 insertions(+), 35 deletions(-) diff --git a/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java b/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java index 8566809cc..8788530cd 100644 --- a/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java +++ b/lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java @@ -19,6 +19,7 @@ import org.eclipse.mosaic.lib.database.road.Connection; import org.eclipse.mosaic.lib.database.road.Node; import org.eclipse.mosaic.lib.geo.GeoPoint; +import org.eclipse.mosaic.lib.math.MathUtils; import org.eclipse.mosaic.lib.math.Vector3d; import org.eclipse.mosaic.lib.math.VectorUtils; import org.eclipse.mosaic.lib.spatial.KdTree; @@ -65,11 +66,82 @@ public EdgeFinder(Database database) { } /** - * Searches for the closest edge to the geo location. + * Searches for the closest edge given a location and a heading. + * If two adjacent edges overlap, the heading will be used as a similarity measure. * - * @return Closest edge to the given location. + * @param location the location to find the closest edge to + * @param heading used as a measure of similarity + * @return the closest edge to the given location considering the heading */ - public List findClosestEdge(GeoPoint location) { + public Edge findClosestEdge(GeoPoint location, double heading) { + List result = findKNearestEdgeWrappers(location, K_EDGES); + if (result == null || result.isEmpty()) { + return null; + } + if (result.size() == 1) { + return result.get(0).edge; + } + Edge bestMatch = null; + for (EdgeWrapper contestant : result) { + if (bestMatch == null + || MathUtils.angleDif(getHeadingOfEdge(bestMatch), heading) + > MathUtils.angleDif(getHeadingOfEdge(contestant.edge), heading)) { + bestMatch = contestant.edge; + } else { + getHeadingOfEdge(bestMatch); + } + } + return bestMatch; + } + + private double getHeadingOfEdge(Edge bestMatch) { + return VectorUtils.getHeadingFromDirection(bestMatch.getNextNode().getPosition().toVector3d() + .subtract(bestMatch.getPreviousNode().getPosition().toVector3d(), new Vector3d()) + ); + } + + /** + * Searches for the two closest edges to the geo location. + * The number of searched edges can be configured using {@link #K_EDGES}. + * + * @param location the location to find the closest edge to + * @return The two closest edges to the given location. + */ + public List findClosestEdges(GeoPoint location) { + List result = findKNearestEdgeWrappers(location, K_EDGES); + if (result == null || result.isEmpty()) { + return null; + } + if (result.size() == 1) { + return Lists.newArrayList(result.get(0).edge); + } + Edge edge0 = result.get(0).edge; + Edge edge1 = result.get(1).edge; + // check if roads are adjacent + if (edge0.getPreviousNode() == edge1.getNextNode() && edge0.getNextNode() == edge1.getPreviousNode() + && edge0.getConnection().getNodes().size() == edge1.getConnection().getNodes().size()) { + final Vector3d locationVector = location.toVector3d(); + final Vector3d origin0 = edge0.getPreviousNode().getPosition().toVector3d(); + final Vector3d direction0 = edge0.getNextNode().getPosition().toVector3d().subtract(origin0, new Vector3d()); + final Vector3d origin1 = edge1.getPreviousNode().getPosition().toVector3d(); + final Vector3d direction1 = edge1.getNextNode().getPosition().toVector3d().subtract(origin1, new Vector3d()); + List resultEdges = new ArrayList<>(); + if (!VectorUtils.isLeftOfLine(locationVector, origin0, direction0)) { + // if location is right of first connection, return first one + resultEdges.add(edge0); + } + if (!VectorUtils.isLeftOfLine(locationVector, origin1, direction1)) { + // if location is right of second connection, return second one + resultEdges.add(edge1); + } + return resultEdges; + } else { + return Lists.newArrayList(edge0); + } + + } + + private List findKNearestEdgeWrappers(GeoPoint location, int k) { synchronized (edgeSearch) { Vector3d locationVector = location.toVector3d(); edgeSearch.setup(locationVector, K_EDGES); @@ -79,30 +151,10 @@ public List findClosestEdge(GeoPoint location) { if (result == null || result.isEmpty()) { return null; } - EdgeWrapper edgeWrapper0 = result.get(0); - EdgeWrapper edgeWrapper1 = result.get(1); - Connection connection0 = edgeWrapper0.edge.getConnection(); - Connection connection1 = edgeWrapper1.edge.getConnection(); - // check if roads are adjacent - if (connection0.getFrom() == connection1.getTo() && connection0.getTo() == connection1.getFrom() - && connection0.getNodes().size() == connection1.getNodes().size()) { - Vector3d origin0 = connection0.getFrom().getPosition().toVector3d(); - Vector3d direction0 = connection0.getTo().getPosition().toVector3d().subtract(origin0, new Vector3d()); - Vector3d origin1 = connection1.getFrom().getPosition().toVector3d(); - Vector3d direction1 = connection1.getTo().getPosition().toVector3d().subtract(origin1, new Vector3d()); - List resultEdges = new ArrayList<>(); - if (!VectorUtils.isLeftOfLine(locationVector, origin0, direction0)) { - // if location is right of first connection, return first one - resultEdges.add(edgeWrapper0.edge); - } - if (!VectorUtils.isLeftOfLine(locationVector, origin1, direction1)) { - // if location is right of second connection, return second one - resultEdges.add(edgeWrapper1.edge); - } - return resultEdges; - } else { - return Lists.newArrayList(edgeWrapper0.edge); + if (result.size() == 1) { + return Lists.newArrayList(result.get(0)); } + return result; } } diff --git a/lib/mosaic-database/src/test/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinderTest.java b/lib/mosaic-database/src/test/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinderTest.java index aded96c09..b96cd9efa 100644 --- a/lib/mosaic-database/src/test/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinderTest.java +++ b/lib/mosaic-database/src/test/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinderTest.java @@ -42,7 +42,7 @@ public class EdgeFinderTest { public GeoProjectionRule projectionRule = new GeoProjectionRule(GeoPoint.latLon(52, 13)); @Test - public void findClosestEdge() { + public void findClosestEdges() { // SETUP Database db = Database.loadFromFile(rule.get("tiergarten.db")); assertFalse(db.getConnections().isEmpty()); @@ -50,7 +50,7 @@ public void findClosestEdge() { final EdgeFinder edgeFinder = new EdgeFinder(db); // RUN - Edge edge = Iterables.getOnlyElement(edgeFinder.findClosestEdge(GeoPoint.latLon(52.51303, 13.32743))); + Edge edge = Iterables.getOnlyElement(edgeFinder.findClosestEdges(GeoPoint.latLon(52.51303, 13.32743))); // ASSERT assertEquals("36337926_408194196_408194192", edge.getConnection().getId()); @@ -59,7 +59,7 @@ public void findClosestEdge() { } @Test - public void findClosestEdge2() { + public void findClosestEdges2() { // SETUP Database db = Database.loadFromFile(rule2.get("edgeFinderTest.db")); assertFalse(db.getConnections().isEmpty()); @@ -67,8 +67,8 @@ public void findClosestEdge2() { final EdgeFinder edgeFinder = new EdgeFinder(db); // RUN - List edgesWest = edgeFinder.findClosestEdge(GeoPoint.latLon(0.0, 10.510506)); - List edgesEast = edgeFinder.findClosestEdge(GeoPoint.latLon(0.0, 10.511805)); + List edgesWest = edgeFinder.findClosestEdges(GeoPoint.latLon(0.0, 10.510506)); + List edgesEast = edgeFinder.findClosestEdges(GeoPoint.latLon(0.0, 10.511805)); Edge edgeWest = Iterables.getOnlyElement(edgesWest); Edge edgeEast = Iterables.getOnlyElement(edgesEast); // ASSERT @@ -76,4 +76,20 @@ public void findClosestEdge2() { assertEquals("-E0", edgeWest.getConnection().getId()); } + @Test + public void findClosestEdge() { + // SETUP + Database db = Database.loadFromFile(rule2.get("edgeFinderTest.db")); + assertFalse(db.getConnections().isEmpty()); + + final EdgeFinder edgeFinder = new EdgeFinder(db); + + // RUN + Edge edgeUpwards = edgeFinder.findClosestEdge(GeoPoint.latLon(0.0, 10.510506), 0d); + Edge edgeDownwards = edgeFinder.findClosestEdge(GeoPoint.latLon(0.0, 10.511805), 180d); + // ASSERT + assertEquals("E0", edgeUpwards.getConnection().getId()); + assertEquals("-E0", edgeDownwards.getConnection().getId()); + } + } \ No newline at end of file diff --git a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java index 36e7442d9..9cc6ad6c5 100644 --- a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java +++ b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java @@ -218,21 +218,21 @@ public CandidateRoute approximateCostsForCandidateRoute(CandidateRoute route, St return new CandidateRoute(route.getConnectionIds(), length, time); } - private List findClosestEdge(GeoPoint location) { + private List findClosestEdges(GeoPoint location) { if (edgeFinder == null) { edgeFinder = new EdgeFinder(scenarioDatabase); } - return edgeFinder.findClosestEdge(location); + return edgeFinder.findClosestEdges(location); } /** - * Searches for the closest edge to the geo location. + * Searches for the closest {@link IRoadPosition} to a given geo location. * * @return Closest edge to the given location. */ @Override public IRoadPosition findClosestRoadPosition(GeoPoint location) { - List closestEdges = findClosestEdge(location); + List closestEdges = findClosestEdges(location); if (closestEdges == null || closestEdges.isEmpty()) { return null; } From a2fb73b7bb7d351dd624e44f6419b3d98c2587ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Tue, 3 Sep 2024 16:38:16 +0200 Subject: [PATCH 4/5] feat: findClosestRoadPosition signature has been added to the Routing and IRoutingModule interfaces MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../ambassador/navigation/IRoutingModule.java | 10 ++++++++ .../navigation/NavigationModule.java | 5 ++++ .../eclipse/mosaic/lib/routing/Routing.java | 12 ++++++++- .../lib/routing/database/DatabaseRouting.java | 25 +++++++++++++++++-- .../lib/routing/norouting/NoRouting.java | 5 ++++ 5 files changed, 54 insertions(+), 3 deletions(-) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/IRoutingModule.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/IRoutingModule.java index 92eceb808..1d6b689c9 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/IRoutingModule.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/IRoutingModule.java @@ -69,4 +69,14 @@ public interface IRoutingModule { * @return The road position, which is closest to the given location. */ IRoadPosition getClosestRoadPosition(GeoPoint geoPoint); + + /** + * Returns the road position, which is closest to the given {@link GeoPoint}. + * If two adjacent edges overlap, the heading will be used as a similarity measure. + * + * @param geoPoint The geographical location to search a road position for. + * @param heading used as a measure of similarity if multiple edges match + * @return The road position, which is closest to the given location. + */ + IRoadPosition getClosestRoadPosition(GeoPoint geoPoint, double heading); } diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModule.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModule.java index c66fc0509..293551a74 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModule.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/NavigationModule.java @@ -286,4 +286,9 @@ public INode getClosestNode(GeoPoint geoPoint) { public IRoadPosition getClosestRoadPosition(GeoPoint geoPoint) { return SimulationKernel.SimulationKernel.getCentralNavigationComponent().getRouting().findClosestRoadPosition(geoPoint); } + + @Override + public IRoadPosition getClosestRoadPosition(GeoPoint geoPoint, double heading) { + return SimulationKernel.SimulationKernel.getCentralNavigationComponent().getRouting().findClosestRoadPosition(geoPoint, heading); + } } diff --git a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/Routing.java b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/Routing.java index 002971c5a..d3be8cef5 100644 --- a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/Routing.java +++ b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/Routing.java @@ -90,13 +90,23 @@ public interface Routing { CandidateRoute approximateCostsForCandidateRoute(CandidateRoute route, String lastNodeId); /** - * Searches for the closest road position to {@param point}. + * Searches for the closest road position to a given {@link GeoPoint}. * * @param point The closest road position to the given location. * @return The closest road position as {@link IRoadPosition}. */ IRoadPosition findClosestRoadPosition(GeoPoint point); + /** + * Searches for the closest road position to a given {@link GeoPoint}, + * If two adjacent edges overlap, the heading will be used as a similarity measure. + * + * @param point The closest road position to the given location. + * @param heading used as a measure of similarity if multiple edges match + * @return The closest road position as {@link IRoadPosition}. + */ + IRoadPosition findClosestRoadPosition(GeoPoint point, double heading); + /** * Searches for the closest node to {@param point}. * diff --git a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java index 9cc6ad6c5..1186b05b2 100644 --- a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java +++ b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java @@ -225,6 +225,13 @@ private List findClosestEdges(GeoPoint location) { return edgeFinder.findClosestEdges(location); } + private Edge findClosestEdge(GeoPoint location, double heading) { + if (edgeFinder == null) { + edgeFinder = new EdgeFinder(scenarioDatabase); + } + return edgeFinder.findClosestEdge(location, heading); + } + /** * Searches for the closest {@link IRoadPosition} to a given geo location. * @@ -240,7 +247,19 @@ public IRoadPosition findClosestRoadPosition(GeoPoint location) { log.info("findClosestRoadPosition returned more than one edge, returning first result."); } Edge closestEdge = closestEdges.get(0); - LazyLoadingConnection connection = new LazyLoadingConnection(closestEdge.getConnection()); + return redefineRoadPosition(location, closestEdge); + } + + @Override + public IRoadPosition findClosestRoadPosition(GeoPoint point, double heading) { + Edge closestEdge = findClosestEdge(point, heading); + if (closestEdge == null) { + return null; + } + return redefineRoadPosition(point, closestEdge); + } + + private static LazyLoadingRoadPosition redefineRoadPosition(GeoPoint location, Edge closestEdge) { LazyLoadingNode previousNode = new LazyLoadingNode(closestEdge.getPreviousNode()); LazyLoadingNode upcomingNode = new LazyLoadingNode(closestEdge.getNextNode()); @@ -250,7 +269,9 @@ public IRoadPosition findClosestRoadPosition(GeoPoint location) { double distanceFromStart = closestEdge.getPreviousNode().getPosition().distanceTo(closestPointOnEdge); distanceFromStart = min(distanceFromStart, previousNode.getPosition().distanceTo(upcomingNode.getPosition())); - return new LazyLoadingRoadPosition(connection, previousNode, upcomingNode, distanceFromStart); + return new LazyLoadingRoadPosition( + new LazyLoadingConnection(closestEdge.getConnection()), previousNode, upcomingNode, distanceFromStart + ); } @Override diff --git a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/norouting/NoRouting.java b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/norouting/NoRouting.java index a7f337582..6922e1ef5 100644 --- a/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/norouting/NoRouting.java +++ b/lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/norouting/NoRouting.java @@ -83,6 +83,11 @@ public IRoadPosition findClosestRoadPosition(GeoPoint point) { return null; } + @Override + public IRoadPosition findClosestRoadPosition(GeoPoint point, double heading) { + return null; + } + @Override public INode findClosestNode(GeoPoint point) { return null; From c4d9af0c43000f2d01ad96d5d38fbe828ef62aaf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Moritz=20Schweppenh=C3=A4user?= Date: Wed, 4 Sep 2024 10:48:08 +0200 Subject: [PATCH 5/5] fix: getClosestRoadPosition signature has now also been added to the INavigationModule interface MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Moritz Schweppenhäuser --- .../ambassador/navigation/INavigationModule.java | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/INavigationModule.java b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/INavigationModule.java index 17b4c8449..9677b694d 100644 --- a/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/INavigationModule.java +++ b/fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/navigation/INavigationModule.java @@ -164,4 +164,14 @@ public interface INavigationModule { * @return the closest {@link IRoadPosition} */ IRoadPosition getClosestRoadPosition(GeoPoint geoPoint); + + /** + * Returns the road position, which is closest to the given {@link GeoPoint}. + * If two adjacent edges overlap, the heading will be used as a similarity measure. + * + * @param geoPoint The geographical location to search a road position for. + * @param heading used as a measure of similarity if multiple edges match + * @return The road position, which is closest to the given location. + */ + IRoadPosition getClosestRoadPosition(GeoPoint geoPoint, double heading); }