fix: Improved EdgeFinder in cases where adjacent connections share th…

…e same FROM and TO points (#405)

* fix: improved EdgeFinder in cases where adjacent connections share the same FROM and TO points
* fix: adjusted logic a little bit
* feat: re-added findClosestEdge for EdgeFinder but it additionally takes a heading now
* feat: findClosestRoadPosition signature has been added to the Routing and IRoutingModule interfaces
* fix: getClosestRoadPosition signature has now also been added to the INavigationModule interface

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/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);
 }

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/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);
 }

fed/mosaic-application/src/main/java/org/eclipse/mosaic/fed/application/ambassador/simulation/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);
+    }
 }

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

@@ -19,11 +19,14 @@
 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;
 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 +37,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 +62,99 @@ 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.
+     * 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 Edge findClosestEdge(GeoPoint location) {
+    public Edge findClosestEdge(GeoPoint location, double heading) {
+        List<EdgeWrapper> 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<Edge> findClosestEdges(GeoPoint location) {
+        List<EdgeWrapper> 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<Edge> 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<EdgeWrapper> findKNearestEdgeWrappers(GeoPoint location, int k) {
         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;
+            if (result.size() == 1) {
+                return Lists.newArrayList(result.get(0));
+            }
+            return result;
         }
     }
 

lib/mosaic-database/src/test/java/org/eclipse/mosaic/lib/database/spatial/EdgeFinderTest.java

@@ -23,31 +23,73 @@
 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));
 
     @Test
-    public void findClosestEdge() {
+    public void findClosestEdges() {
         // SETUP
         Database db = Database.loadFromFile(rule.get("tiergarten.db"));
         assertFalse(db.getConnections().isEmpty());
 
         final EdgeFinder edgeFinder = new EdgeFinder(db);
 
-        // RUN + ASSERT
-        Edge edge = edgeFinder.findClosestEdge(GeoPoint.latLon(52.51303, 13.32743));
+        // RUN
+        Edge edge = Iterables.getOnlyElement(edgeFinder.findClosestEdges(GeoPoint.latLon(52.51303, 13.32743)));
+
+        // ASSERT
         assertEquals("36337926_408194196_408194192", edge.getConnection().getId());
         assertEquals("410846037", edge.getPreviousNode().getId());
 
     }
 
+    @Test
+    public void findClosestEdges2() {
+        // SETUP
+        Database db = Database.loadFromFile(rule2.get("edgeFinderTest.db"));
+        assertFalse(db.getConnections().isEmpty());
+
+        final EdgeFinder edgeFinder = new EdgeFinder(db);
+
+        // RUN
+        List<Edge> edgesWest = edgeFinder.findClosestEdges(GeoPoint.latLon(0.0, 10.510506));
+        List<Edge> edgesEast = edgeFinder.findClosestEdges(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());
+    }
+
+    @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());
+    }
+
 }

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/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}.
      *