Feature/nav 32 range search in kd tree

src/main/java/ch/naviqore/gtfs/schedule/model/GtfsSchedule.java

@@ -1,6 +1,7 @@
 package ch.naviqore.gtfs.schedule.model;
 
-import ch.naviqore.gtfs.schedule.spatial.Coordinate;
+import ch.naviqore.utils.spatial.index.KDTree;
+import ch.naviqore.utils.spatial.index.KDTreeBuilder;
 import lombok.Getter;
 
 import java.time.LocalDate;
@@ -25,6 +26,7 @@ public class GtfsSchedule {
     private final Map<String, Stop> stops;
     private final Map<String, Route> routes;
     private final Map<String, Trip> trips;
+    private final KDTree<Stop> spatialIndex;
 
     /**
      * Constructs an immutable GTFS schedule.
@@ -39,6 +41,7 @@ public class GtfsSchedule {
         this.stops = Map.copyOf(stops);
         this.routes = Map.copyOf(routes);
         this.trips = Map.copyOf(trips);
+        this.spatialIndex = new KDTreeBuilder<Stop>().addLocations(stops.values()).build();
     }
 
     /**
@@ -59,12 +62,18 @@ public static GtfsScheduleBuilder builder() {
      * @return A list of stops within the specified distance.
      */
     public List<Stop> getNearestStops(double latitude, double longitude, int maxDistance) {
-        // TODO: Use a spatial index for efficient nearest neighbor search, e.g. KD-tree or R-tree
-        Coordinate origin = new Coordinate(latitude, longitude);
-        return stops.values()
-                .stream()
-                .filter(stop -> stop.getCoordinate().distanceTo(origin) <= maxDistance)
-                .collect(Collectors.toList());
+        return spatialIndex.rangeSearch(latitude, longitude, maxDistance);
+    }
+
+    /**
+     * Retrieves the nearest stop to a given location.
+     *
+     * @param latitude  the latitude of the location.
+     * @param longitude the longitude of the location.
+     * @return The nearest stop to the specified location.
+     */
+    public Stop getNearestStop(double latitude, double longitude) {
+        return spatialIndex.nearestNeighbour(latitude, longitude);
     }
 
     /**

src/main/java/ch/naviqore/gtfs/schedule/model/GtfsScheduleBuilder.java

@@ -1,10 +1,10 @@
 package ch.naviqore.gtfs.schedule.model;
 
-import ch.naviqore.gtfs.schedule.spatial.Coordinate;
 import ch.naviqore.gtfs.schedule.type.ExceptionType;
 import ch.naviqore.gtfs.schedule.type.RouteType;
 import ch.naviqore.gtfs.schedule.type.ServiceDayTime;
 import ch.naviqore.gtfs.schedule.type.TransferType;
+import ch.naviqore.utils.spatial.GeoCoordinate;
 import lombok.AccessLevel;
 import lombok.NoArgsConstructor;
 import lombok.extern.log4j.Log4j2;
@@ -55,7 +55,7 @@ public GtfsScheduleBuilder addStop(String id, String name, double lat, double lo
             throw new IllegalArgumentException("Agency " + id + " already exists");
         }
         log.debug("Adding stop {}", id);
-        stops.put(id, new Stop(id, name, new Coordinate(lat, lon)));
+        stops.put(id, new Stop(id, name, new GeoCoordinate(lat, lon)));
         return this;
     }
 
@@ -171,7 +171,6 @@ public GtfsSchedule build() {
         stops.values().parallelStream().forEach(Initializable::initialize);
         routes.values().parallelStream().forEach(Initializable::initialize);
         calendars.values().parallelStream().forEach(Initializable::initialize);
-        // TODO: Build k-d tree for spatial indexing
         GtfsSchedule schedule = new GtfsSchedule(agencies, calendars, stops, routes, trips);
         clear();
         built = true;

src/main/java/ch/naviqore/gtfs/schedule/model/Stop.java

@@ -1,6 +1,7 @@
 package ch.naviqore.gtfs.schedule.model;
 
-import ch.naviqore.gtfs.schedule.spatial.Coordinate;
+import ch.naviqore.utils.spatial.GeoCoordinate;
+import ch.naviqore.utils.spatial.Location;
 import lombok.AccessLevel;
 import lombok.Getter;
 import lombok.RequiredArgsConstructor;
@@ -12,11 +13,11 @@
 
 @RequiredArgsConstructor(access = AccessLevel.PACKAGE)
 @Getter
-public final class Stop implements Initializable {
+public final class Stop implements Initializable, Location<GeoCoordinate> {
 
     private final String id;
     private final String name;
-    private final Coordinate coordinate;
+    private final GeoCoordinate coordinate;
     private List<StopTime> stopTimes = new ArrayList<>();
     private List<Transfer> transfers = new ArrayList<>();
 

src/main/java/ch/naviqore/utils/spatial/CartesianCoordinate.java

@@ -0,0 +1,67 @@
+package ch.naviqore.utils.spatial;
+
+public class CartesianCoordinate implements Coordinate {
+    private final double x;
+    private final double y;
+
+    public CartesianCoordinate(double x, double y) {
+        validateCoordinate(x, y);
+        this.x = x;
+        this.y = y;
+    }
+
+    private static void validateCoordinate(double x, double y) {
+        if (Double.isNaN(x) || Double.isNaN(y)) {
+            throw new IllegalArgumentException("Coordinates cannot be NaN");
+        }
+    }
+
+    private void isOfSameType(Coordinate other) {
+        if (other == null) {
+            throw new IllegalArgumentException("Other coordinate must not be null");
+        }
+        if (other.getClass() != this.getClass()) {
+            throw new IllegalArgumentException("Other coordinate must be of type " + this.getClass().getSimpleName());
+        }
+    }
+
+    @Override
+    public double getFirstComponent() {
+        return x;
+    }
+
+    @Override
+    public double getSecondComponent() {
+        return y;
+    }
+
+    @Override
+    public double distanceTo(Coordinate other) {
+        isOfSameType(other);
+        return distanceTo(other.getFirstComponent(), other.getSecondComponent());
+    }
+
+    @Override
+    public double distanceTo(double x, double y) {
+        validateCoordinate(x, y);
+        return Math.sqrt(Math.pow(this.x - x, 2) + Math.pow(this.y - y, 2));
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (obj == this) {
+            return true;
+        }
+        if (obj == null || obj.getClass() != this.getClass()) {
+            return false;
+        }
+        var that = (CartesianCoordinate) obj;
+        return this.x == that.x && this.y == that.y;
+    }
+
+    @Override
+    public String toString() {
+        return "[" + this.getClass().getSimpleName() + ": " + x + ", " + y + "]";
+    }
+
+}

src/main/java/ch/naviqore/utils/spatial/Coordinate.java

@@ -0,0 +1,50 @@
+package ch.naviqore.utils.spatial;
+
+/**
+ * This interface represents a generic 2D coordinate. Implementations of this interface should provide methods to get
+ * the two components of the coordinate and calculate distances.
+ */
+public interface Coordinate {
+
+    /**
+     * Examples for the first component are latitude or X coordinate.
+     *
+     * @return the first component of the 2D-coordinate
+     */
+    double getFirstComponent();
+
+    /**
+     * Examples for the first component are longitude or Y coordinate.
+     *
+     * @return the second component of the 2D-coordinate
+     */
+    double getSecondComponent();
+
+    /**
+     * Calculates the distance to another {@code Coordinate} object.
+     * <p><i>Note: Implementations may raise an {@code IllegalArgumentException} if the other {@code Coordinate} object
+     * is not of the same type.</i></p>
+     */
+    double distanceTo(Coordinate other);
+    // TODO: Should we somehow enforce that distances between geo and cartesian coordinates cannot be calculated?
+
+    /**
+     * Calculates the distance to another point specified by its components.
+     */
+    double distanceTo(double firstComponent, double secondComponent);
+
+    /**
+     * Gets the coordinate component based on the specified {@code Axis}.
+     */
+    default double getComponent(Axis axis) {
+        return axis == Axis.FIRST ? getFirstComponent() : getSecondComponent();
+    }
+
+    /**
+     * The axes of a 2D coordinate system.
+     */
+    enum Axis {
+        FIRST,
+        SECOND
+    }
+}

src/main/java/ch/naviqore/utils/spatial/GeoCoordinate.java

@@ -0,0 +1,92 @@
+package ch.naviqore.utils.spatial;
+
+public record GeoCoordinate(double latitude, double longitude) implements Coordinate, Comparable<GeoCoordinate> {
+
+    private static final int EARTH_RADIUS = 6371000;
+
+    public GeoCoordinate {
+        validateCoordinate(latitude, longitude);
+    }
+
+    private static void validateCoordinate(double latitude, double longitude) {
+        if (latitude < -90 || latitude > 90) {
+            throw new IllegalArgumentException("Latitude must be between -90 and 90 degrees");
+        }
+        if (longitude < -180 || longitude > 180) {
+            throw new IllegalArgumentException("Longitude must be between -180 and 180 degrees");
+        }
+        if (Double.isNaN(latitude) || Double.isNaN(longitude)) {
+            throw new IllegalArgumentException("Coordinates cannot be NaN");
+        }
+    }
+
+    private void isOfSameType(Coordinate other) {
+        if (other == null) {
+            throw new IllegalArgumentException("Other coordinate must not be null");
+        }
+        if (other.getClass() != this.getClass()) {
+            throw new IllegalArgumentException("Other coordinate must be of type " + this.getClass().getSimpleName());
+        }
+    }
+
+    @Override
+    public double getFirstComponent() {
+        return latitude;
+    }
+
+    @Override
+    public double getSecondComponent() {
+        return longitude;
+    }
+
+    /**
+     * Calculates the distance to another Coordinates object using the Haversine formula.
+     *
+     * @param other The other Coordinates object to calculate the distance to.
+     * @return The distance in meters.
+     */
+    @Override
+    public double distanceTo(Coordinate other) {
+        isOfSameType(other);
+        return distanceTo(other.getFirstComponent(), other.getSecondComponent());
+    }
+
+    @Override
+    public double distanceTo(double firstComponent, double secondComponent) {
+        validateCoordinate(firstComponent, secondComponent);
+
+        double lat1 = Math.toRadians(this.latitude);
+        double lat2 = Math.toRadians(firstComponent);
+        double lon1 = Math.toRadians(this.longitude);
+        double lon2 = Math.toRadians(secondComponent);
+
+        double dLat = lat2 - lat1;
+        double dLon = lon2 - lon1;
+
+        double a = Math.pow(Math.sin(dLat / 2), 2) + Math.pow(Math.sin(dLon / 2), 2) * Math.cos(lat1) * Math.cos(lat2);
+        double c = 2 * Math.asin(Math.sqrt(a));
+        return EARTH_RADIUS * c;
+    }
+
+    @Override
+    public int compareTo(GeoCoordinate other) {
+        double epsilon = 1e-5;
+
+        double diffLatitude = this.latitude - other.latitude();
+        if (Math.abs(diffLatitude) > epsilon) {
+            return diffLatitude > 0 ? 1 : -1;
+        }
+
+        double diffLongitude = this.longitude - other.longitude();
+        if (Math.abs(diffLongitude) > epsilon) {
+            return diffLongitude > 0 ? 1 : -1;
+        }
+
+        return 0;
+    }
+
+    @Override
+    public String toString() {
+        return "[" + this.getClass().getSimpleName() + ": " + latitude + "°, " + longitude + "°]";
+    }
+}

src/main/java/ch/naviqore/utils/spatial/Location.java

@@ -0,0 +1,15 @@
+package ch.naviqore.utils.spatial;
+
+/**
+ * A location in a generic spatial coordinate system.
+ *
+ * @param <T> The type of the coordinates that defines this location, must extend {@code Coordinate}.
+ */
+public interface Location<T extends Coordinate> {
+
+    /**
+     * Gets the coordinate that defines this location.
+     */
+    T getCoordinate();
+
+}