fix: Improved EdgeFinder in cases where adjacent connections share the same FROM and TO points

lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/Edge.java

@@ -83,4 +83,11 @@ protected double getCenterDistanceSqr(E item, SpatialTree<E> tree) {
         }
     }
 
+    static class KNearest<V extends Vector3d, E extends org.eclipse.mosaic.lib.spatial.Edge<V>> extends SpatialTreeTraverser.KNearest<E> {
+        @Override
+        protected double getCenterDistanceSqr(E item, SpatialTree<E> tree) {
+            return item.getNearestPointOnEdge(center).distanceSqrTo(center);
+        }
+    }
+
 }

lib/mosaic-database/src/main/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinder.java

@@ -20,10 +20,12 @@
 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;
@@ -34,16 +36,22 @@
  */
 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<EdgeWrapper> edgeIndex;
-    private final SpatialTreeTraverser.Nearest<EdgeWrapper> edgeSearch;
+    private final SpatialTreeTraverser.KNearest<EdgeWrapper> edgeSearch;
 
     /**
      * Constructs a new edgeFinder object with the specified database.
      *
      * @param database Database which contains all connections.
      */
     public EdgeFinder(Database database) {
-        List<EdgeWrapper> items =  new ArrayList<>();
+        List<EdgeWrapper> items = new ArrayList<>();
 
         for (Connection con : database.getConnections()) {
             for (int i = 0; i < con.getNodes().size() - 1; i++) {
@@ -53,24 +61,48 @@ 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<>();
     }
 
     /**
      * Searches for the closest edge to the geo location.
      *
      * @return Closest edge to the given location.
      */
-    public Edge findClosestEdge(GeoPoint location) {
+    public List<Edge> 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<EdgeWrapper> 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());
+                List<Edge> 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);
+            }
         }
     }
 

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<Edge> edgesWest = edgeFinder.findClosestEdge(GeoPoint.latLon(0.0, 10.510506));
+        List<Edge> 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());
+    }
+
 }

lib/mosaic-geomath/src/main/java/org/eclipse/mosaic/lib/spatial/SpatialTreeTraverser.java

@@ -24,7 +24,9 @@
 import org.eclipse.mosaic.lib.math.Vector3d;
 
 import java.util.ArrayList;
+import java.util.Comparator;
 import java.util.List;
+import java.util.PriorityQueue;
 
 public abstract class SpatialTreeTraverser<T> {
 
@@ -166,6 +168,64 @@ protected void traverseLeaf(SpatialTree<T>.Node node, SpatialTree<T> tree) {
         }
     }
 
+    public static class KNearest<T> extends CenterDistanceBased<T> {
+        private int k;
+        private PriorityQueue<Neighbor<T>> maxHeap;
+
+        public KNearest<T> setup(Vector3d center, int k) {
+            setCenter(center);
+            this.k = k;
+            this.maxHeap = new PriorityQueue<>(Comparator.comparingDouble(neighbor -> -neighbor.distance));
+            return this;
+        }
+
+        public List<T> getKNearest() {
+            List<T> result = new ArrayList<>();
+            while (!maxHeap.isEmpty()) {
+                result.add(maxHeap.poll().item);
+            }
+            return result;
+        }
+
+        @Override
+        protected void traverseChildren(SpatialTree<T>.Node node, SpatialTree<T> tree) {
+            List<SpatialTree<T>.Node> children = node.getChildren();
+            for (int i = 0; i < children.size(); i++) {
+                SpatialTree<T>.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<T>.Node node, SpatialTree<T> tree) {
+            List<T> 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> {
+            T item;
+            double distance;
+
+            Neighbor(T item, double distance) {
+                this.item = item;
+                this.distance = distance;
+            }
+        }
+    }
+
     public abstract static class InPolygon<P extends Point<P>, T extends Polygon<P>> extends Nearest<T> {
 
         private P search;

lib/mosaic-routing/src/main/java/org/eclipse/mosaic/lib/routing/database/DatabaseRouting.java

@@ -210,15 +210,15 @@ 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();
         }
         return new CandidateRoute(route.getConnectionIds(), length, time);
     }
 
-    private Edge findClosestEdge(GeoPoint location) {
+    private List<Edge> 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<Edge> 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());