diff --git a/src/main/java/net/tfminecraft/vehicleframework/tracks/TrackRegistry.java b/src/main/java/net/tfminecraft/vehicleframework/tracks/TrackRegistry.java index 50d895b..fc1878f 100644 --- a/src/main/java/net/tfminecraft/vehicleframework/tracks/TrackRegistry.java +++ b/src/main/java/net/tfminecraft/vehicleframework/tracks/TrackRegistry.java @@ -1087,6 +1087,31 @@ public OptionalDouble shortestRouteLength( world, ax, az, radiusA, bx, bz, radiusB); } + /** + * The shortest route along track from any point within {@code radiusA} + * of {@code (ax, az)} to any point within {@code radiusB} of {@code (bx, bz)} + * in {@code world}, with points ordered from A to B and sampled by arc length. + * Empty when no such route exists. A broken segment blocks the route. + * Invalid spacing defaults to 4 blocks, with a minimum of 0.5 blocks. + * Each spline piece's exit is included; overlapping areas give one point + * and length 0. Height is ignored when finding the areas on track. + * Other plugins reach this through {@link VehicleFramework#getTrackRegistry()}. + *

+ * Call on the server thread. Splines and junctions live in separate maps + * and are not one snapshot off that thread, so a route read elsewhere can + * mix a new junction with an old spline. This does not load chunks, touch + * entities, or modify the registry. + */ + public Optional shortestRoute( + String world, + double ax, double az, double radiusA, + double bx, double bz, double radiusB, + double spacing) { + return TrackRouteQuery.shortestRoute( + splines.values(), junctions.values(), + world, ax, az, radiusA, bx, bz, radiusB, spacing); + } + public void dumpToLog() { TrackLog.append("DUMP splines=" + splines.size() + " junctions=" + junctions.size()); for (TrackSpline spline : splines.values()) { diff --git a/src/main/java/net/tfminecraft/vehicleframework/tracks/TrackRoute.java b/src/main/java/net/tfminecraft/vehicleframework/tracks/TrackRoute.java new file mode 100644 index 0000000..421a561 --- /dev/null +++ b/src/main/java/net/tfminecraft/vehicleframework/tracks/TrackRoute.java @@ -0,0 +1,10 @@ +package net.tfminecraft.vehicleframework.tracks; + +import java.util.List; + +/** A shortest along-track route with immutable points ordered from A to B. */ +public record TrackRoute(double length, List points) { + public TrackRoute { + points = List.copyOf(points); + } +} diff --git a/src/main/java/net/tfminecraft/vehicleframework/tracks/TrackRouteQuery.java b/src/main/java/net/tfminecraft/vehicleframework/tracks/TrackRouteQuery.java index a53de27..b4fcc46 100644 --- a/src/main/java/net/tfminecraft/vehicleframework/tracks/TrackRouteQuery.java +++ b/src/main/java/net/tfminecraft/vehicleframework/tracks/TrackRouteQuery.java @@ -3,10 +3,12 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.Collection; +import java.util.Collections; import java.util.Comparator; import java.util.HashMap; import java.util.List; import java.util.Map; +import java.util.Optional; import java.util.OptionalDouble; import java.util.PriorityQueue; import java.util.UUID; @@ -41,11 +43,71 @@ public static OptionalDouble shortestRouteLength( String world, double ax, double az, double radiusA, double bx, double bz, double radiusB) { + Optional path = findRoute(splines, junctions, world, ax, az, radiusA, bx, bz, radiusB); + return path.isEmpty() ? OptionalDouble.empty() : OptionalDouble.of(path.get().length); + } + + /** + * The shortest route measured by {@link #shortestRouteLength}, with points + * ordered from A to B and sampled by arc length. Each spline piece's exit + * is included. Overlapping areas give one point and length 0. Invalid + * spacing defaults to 4 blocks, with a minimum of 0.5 blocks. + * The splines and junctions are read only; no chunks or entities are touched. + */ + public static Optional shortestRoute( + Collection splines, + Collection junctions, + String world, + double ax, double az, double radiusA, + double bx, double bz, double radiusB, + double spacing) { + if (!Double.isFinite(spacing) || spacing <= 0) { + spacing = 4; + } else { + spacing = Math.max(spacing, 0.5); + } + Optional found = findRoute(splines, junctions, world, ax, az, radiusA, bx, bz, radiusB); + if (found.isEmpty()) { + return Optional.empty(); + } + Path path = found.get(); + List points = new ArrayList<>(); + for (Leg leg : path.legs) { + TrackSamplePoint start = samplePoint(leg.spline, leg.from); + if (points.isEmpty() || !points.getLast().equals(start)) { + points.add(start); + } + if (path.length == 0) { + break; + } + double span = Math.abs(leg.to - leg.from); + double sign = Math.signum(leg.to - leg.from); + for (int i = 1; i * spacing < span - EPS; i++) { + points.add(samplePoint(leg.spline, leg.from + sign * i * spacing)); + } + if (span > 0) { + points.add(samplePoint(leg.spline, leg.to)); + } + } + return Optional.of(new TrackRoute(path.length, points)); + } + + private static TrackSamplePoint samplePoint(TrackSpline spline, double s) { + TrackPose pose = spline.sampleAt(s); + return new TrackSamplePoint(spline.getId(), pose.x, pose.y, pose.z); + } + + private static Optional findRoute( + Collection splines, + Collection junctions, + String world, + double ax, double az, double radiusA, + double bx, double bz, double radiusB) { if (world == null || splines == null || junctions == null) { - return OptionalDouble.empty(); + return Optional.empty(); } if (!(radiusA >= 0) || !(radiusB >= 0)) { - return OptionalDouble.empty(); + return Optional.empty(); } Map byId = new HashMap<>(); for (TrackSpline spline : splines) { @@ -54,7 +116,7 @@ public static OptionalDouble shortestRouteLength( } } if (byId.isEmpty()) { - return OptionalDouble.empty(); + return Optional.empty(); } List touches = new ArrayList<>(); @@ -72,7 +134,7 @@ public static OptionalDouble shortestRouteLength( touches.add(new Touch(spline, nearA, nearB)); } if (!sawA || !sawB) { - return OptionalDouble.empty(); + return Optional.empty(); } List linked = new ArrayList<>(); @@ -108,44 +170,42 @@ public static OptionalDouble shortestRouteLength( double stemS = TrackJunction.wrapS(junction.s, stem.length(), stem.isLoop()); anchor(anchors, stem.getId(), stemS, stemNode); anchor(anchors, branch.getId(), 0, branchNode); - link(adj, stemNode, branchNode, 0); + link(adj, stemNode, branchNode, 0, null); } for (Touch touch : touches) { linkSpline(adj, touch.spline, touch.nearA, touch.nearB, anchors.get(touch.spline.getId())); } - double best = dijkstra(adj, SRC, DST); - if (!Double.isFinite(best)) { - return OptionalDouble.empty(); - } - return OptionalDouble.of(best); + return dijkstra(adj, SRC, DST); } private static void anchor(Map> anchors, UUID splineId, double s, int node) { anchors.computeIfAbsent(splineId, id -> new ArrayList<>()).add(new Anchor(s, node)); } - private static void link(List> adj, int a, int b, double weight) { - oneWay(adj, a, b, weight); - oneWay(adj, b, a, weight); + private static void link(List> adj, int a, int b, double weight, Leg leg) { + oneWay(adj, a, b, weight, leg); + oneWay(adj, b, a, weight, leg == null ? null : leg.reverse()); } - private static void oneWay(List> adj, int from, int to, double weight) { + private static void oneWay(List> adj, int from, int to, double weight, Leg leg) { if (from == to || !Double.isFinite(weight)) { return; } if (weight < 0) { weight = 0; } - adj.get(from).add(new Edge(to, weight)); + adj.get(from).add(new Edge(to, weight, leg)); } - private static double dijkstra(List> adj, int src, int dst) { + private static Optional dijkstra(List> adj, int src, int dst) { double[] dist = new double[adj.size()]; Arrays.fill(dist, Double.POSITIVE_INFINITY); dist[src] = 0; boolean[] done = new boolean[adj.size()]; + int[] previous = new int[adj.size()]; + Edge[] incoming = new Edge[adj.size()]; PriorityQueue queue = new PriorityQueue<>(Comparator.comparingDouble(step -> step.dist)); queue.add(new Walk(src, 0)); while (!queue.isEmpty()) { @@ -155,17 +215,26 @@ private static double dijkstra(List> adj, int src, int dst) { } done[step.node] = true; if (step.node == dst) { - return step.dist; + List legs = new ArrayList<>(); + for (int at = dst; at != src; at = previous[at]) { + if (incoming[at].leg != null) { + legs.add(incoming[at].leg); + } + } + Collections.reverse(legs); + return Optional.of(new Path(step.dist, legs)); } for (Edge edge : adj.get(step.node)) { double next = step.dist + edge.weight; if (next < dist[edge.to]) { dist[edge.to] = next; + previous[edge.to] = step.node; + incoming[edge.to] = edge; queue.add(new Walk(edge.to, next)); } } } - return dist[dst]; + return Optional.empty(); } private static List cover(TrackSpline spline, double x, double z, double radius) { @@ -254,33 +323,51 @@ private static List merge(List raw) { return out; } - private static double separation(List a, List b, double length, boolean loop) { - double best = Double.POSITIVE_INFINITY; + private static Span separation(List a, List b, double length, boolean loop) { + Span best = null; for (Interval left : a) { for (Interval right : b) { - best = Math.min(best, separation(left, right, length, loop)); + Span next = separation(left, right, length, loop); + if (best == null || next.length < best.length) { + best = next; + } } } return best; } - private static double separation(Interval a, Interval b, double length, boolean loop) { + private static Span separation(Interval a, Interval b, double length, boolean loop) { if (overlaps(a, b, length, loop)) { - return 0; + if (a.hi + EPS >= b.lo && b.hi + EPS >= a.lo) { + double s = Math.max(a.lo, b.lo); + return new Span(s, s, 0); + } + return new Span(0, 0, 0); } if (!loop) { if (a.hi < b.lo) { - return b.lo - a.hi; + return new Span(a.hi, b.lo, b.lo - a.hi); + } + return new Span(a.lo, b.hi, a.lo - b.hi); + } + Span best = arc(a.lo, b.lo, length); + for (Span next : List.of(arc(a.lo, b.hi, length), arc(a.hi, b.lo, length), arc(a.hi, b.hi, length))) { + if (next.length < best.length) { + best = next; } - return a.lo - b.hi; } - double best = TrackJunction.arcDistance(a.lo, b.lo, length, true); - best = Math.min(best, TrackJunction.arcDistance(a.lo, b.hi, length, true)); - best = Math.min(best, TrackJunction.arcDistance(a.hi, b.lo, length, true)); - best = Math.min(best, TrackJunction.arcDistance(a.hi, b.hi, length, true)); return best; } + private static Span arc(double from, double to, double length) { + double delta = to - from; + double distance = TrackJunction.arcDistance(from, to, length, true); + if (length > 0 && Math.abs(delta) > length / 2) { + delta += delta > 0 ? -length : length; + } + return new Span(from, from + delta, distance); + } + private static boolean overlaps(Interval a, Interval b, double length, boolean loop) { if (a.hi + EPS >= b.lo && b.hi + EPS >= a.lo) { return true; @@ -291,27 +378,34 @@ private static boolean overlaps(Interval a, Interval b, double length, boolean l return (a.hi >= length - EPS && b.lo <= EPS) || (b.hi >= length - EPS && a.lo <= EPS); } - private static double distanceTo(double s, List covered, double length, boolean loop) { - double best = Double.POSITIVE_INFINITY; + private static Span distanceTo(double s, List covered, double length, boolean loop) { + Span best = null; for (Interval interval : covered) { - best = Math.min(best, distanceTo(s, interval, length, loop)); + Span next; + if (s >= interval.lo - EPS && s <= interval.hi + EPS) { + next = new Span(s, s, 0); + } else if (!loop) { + double to = s < interval.lo ? interval.lo : interval.hi; + next = new Span(s, to, Math.abs(to - s)); + } else { + Span lo = arc(s, interval.lo, length); + Span hi = arc(s, interval.hi, length); + next = lo.length <= hi.length ? lo : hi; + } + if (best == null || next.length < best.length) { + best = next; + } } return best; } - private static double distanceTo(double s, Interval interval, double length, boolean loop) { - if (s >= interval.lo - EPS && s <= interval.hi + EPS) { - return 0; - } - if (!loop) { - if (s < interval.lo) { - return interval.lo - s; - } - return s - interval.hi; + private static void oneWay( + List> adj, int from, int to, + TrackSpline spline, Piece piece, Span span) { + if (span != null) { + double origin = piece.circular ? 0 : piece.start; + oneWay(adj, from, to, span.length, new Leg(spline, origin + span.from, origin + span.to)); } - return Math.min( - TrackJunction.arcDistance(s, interval.lo, length, true), - TrackJunction.arcDistance(s, interval.hi, length, true)); } private static void linkSpline( @@ -323,7 +417,7 @@ private static void linkSpline( double length = spline.length(); for (Piece piece : pieces(spline)) { if (!nearA.isEmpty() && !nearB.isEmpty()) { - oneWay(adj, SRC, DST, pieceSeparation(piece, nearA, nearB, length)); + oneWay(adj, SRC, DST, spline, piece, pieceSeparation(piece, nearA, nearB, length)); } if (onSpline == null || onSpline.isEmpty()) { continue; @@ -339,28 +433,30 @@ private static void linkSpline( Anchor prev = onPiece.get(i - 1); Anchor next = onPiece.get(i); link(adj, prev.node, next.node, - along(piece, next.s, length) - along(piece, prev.s, length)); + along(piece, next.s, length) - along(piece, prev.s, length), + new Leg(spline, piece.start + along(piece, prev.s, length), piece.start + along(piece, next.s, length))); } if (piece.circular && onPiece.size() >= 2) { Anchor first = onPiece.get(0); Anchor last = onPiece.get(onPiece.size() - 1); - link(adj, last.node, first.node, length - last.s + first.s); + link(adj, last.node, first.node, length - last.s + first.s, + new Leg(spline, last.s, length + first.s)); } List localA = piece.circular ? nearA : clip(nearA, piece, length); List localB = piece.circular ? nearB : clip(nearB, piece, length); for (Anchor at : onPiece) { double atS = along(piece, at.s, length); if (!nearA.isEmpty()) { - double d = piece.circular + Span span = piece.circular ? distanceTo(at.s, nearA, length, true) : distanceTo(atS, localA, piece.span, false); - oneWay(adj, SRC, at.node, d); + oneWay(adj, SRC, at.node, spline, piece, span == null ? null : span.reverse()); } if (!nearB.isEmpty()) { - double d = piece.circular + Span span = piece.circular ? distanceTo(at.s, nearB, length, true) : distanceTo(atS, localB, piece.span, false); - oneWay(adj, at.node, DST, d); + oneWay(adj, at.node, DST, spline, piece, span); } } } @@ -463,7 +559,7 @@ private static double seam(double s, double length) { return s; } - private static double pieceSeparation(Piece piece, List a, List b, double length) { + private static Span pieceSeparation(Piece piece, List a, List b, double length) { if (piece.circular) { return separation(a, b, length, true); } @@ -524,7 +620,23 @@ private record Touch(TrackSpline spline, List nearA, List ne private record Anchor(double s, int node) { } - private record Edge(int to, double weight) { + private record Edge(int to, double weight, Leg leg) { + } + + private record Span(double from, double to, double length) { + Span reverse() { + return new Span(to, from, length); + } + } + + // Unwrapped s preserves travel direction through the seam and broken loops. + private record Leg(TrackSpline spline, double from, double to) { + Leg reverse() { + return new Leg(spline, to, from); + } + } + + private record Path(double length, List legs) { } private record Walk(int node, double dist) { diff --git a/src/test/java/net/tfminecraft/vehicleframework/tracks/TrackRouteQueryPointsTest.java b/src/test/java/net/tfminecraft/vehicleframework/tracks/TrackRouteQueryPointsTest.java new file mode 100644 index 0000000..2afb0bc --- /dev/null +++ b/src/test/java/net/tfminecraft/vehicleframework/tracks/TrackRouteQueryPointsTest.java @@ -0,0 +1,292 @@ +package net.tfminecraft.vehicleframework.tracks; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import java.util.OptionalDouble; +import java.util.UUID; + +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.io.TempDir; + +class TrackRouteQueryPointsTest { + + @Test + void lengthMatchesEveryExistingFixture() { + TrackSpline slope = TrackSpline.fromPoints(UUID.randomUUID(), "world", false, + List.of(new double[] {0, 0, 0}, new double[] {0, 3, 4})); + route(List.of(slope), List.of(), "world", 0, 2, 0, 0, 4, 0, 4); + + TrackSpline stem = line("world", 0, 0, 0, 80); + TrackSpline branch = line("world", 0, 40, 50, 40); + TrackJunction junction = join(stem, 40, branch); + route(List.of(stem, branch), List.of(junction), "world", 0, 10, 0, 20, 40, 0, 4); + route(List.of(stem, branch), List.of(junction.withThrown(true).withFacing(1)), + "world", 0, 10, 0, 20, 40, 0, 4); + + TrackSpline first = line("world", 0, 0, 100, 0); + TrackSpline second = line("world", 100, 0, 100, 60); + TrackSpline third = line("world", 100, 60, 140, 60); + route(List.of(first, second, third), List.of(join(first, 100, second), join(second, 60, third)), + "world", 0, 0, 0, 140, 60, 0, 4); + + route(List.of(line("world", 0, 0, 0, 20), line("world", 100, 0, 120, 0)), + List.of(), "world", 0, 10, 0, 110, 0, 0, 4); + TrackSpline loop = rectangle(); + route(List.of(loop), List.of(), "world", 0, 0, 0, 0, 10, 0, 4); + route(List.of(loop), List.of(), "world", 0, 0, 0, 30, 0, 0, 4); + route(List.of(samples(0, 50)), List.of(), "world", 0, 10, 2, 10, 10, 2, 4); + route(List.of(samples(0, 10, 30)), List.of(), "world", 0, 10, 5, 0, 25, 2, 4); + route(List.of(samples(0, 10, 30)), List.of(), "world", 0, 10, 5, 0, 14, 2, 4); + + stem = line("world", 0, 0, 80, 0); + branch = line("world", 40, 0, 40, 50); + route(List.of(stem, branch), List.of(join(stem, 40, null)), "world", 0, 0, 0, 40, 50, 0, 4); + route(List.of(breakAt(samples(0, 40, 80, 120), 1)), List.of(), "world", 0, 0, 0, 0, 120, 0, 4); + route(List.of(breakAt(samples(0, 40, 80, 120), 2)), List.of(), "world", 0, 0, 0, 0, 40, 0, 4); + route(List.of(breakAt(rectangle(), 3)), List.of(), "world", 0, 0, 0, 0, 10, 0, 4); + route(List.of(breakAt(rectangle(), 3, 1)), List.of(), "world", 0, 0, 0, 0, 10, 0, 4); + stem = breakAt(samples(0, 30, 60, 90), 1); + branch = line("world", 0, 90, 40, 90); + route(List.of(stem, branch), List.of(join(stem, 90, branch)), "world", 0, 0, 0, 40, 90, 0, 4); + route(List.of(breakAt(samples(0, 10, 40, 100), 0)), List.of(), "world", 0, 0, 25, 0, 100, 0, 4); + + TrackSpline alpha = line("alpha", 0, 0, 10, 0); + route(List.of(alpha), List.of(), "beta", 0, 0, 0, 10, 0, 0, 4); + route(List.of(alpha), List.of(), "missing", 0, 0, 0, 10, 0, 0, 4); + stem = line("overworld", 0, 0, 30, 0); + branch = line("nether", 15, 0, 15, 40); + route(List.of(stem, branch), List.of(join(stem, 15, branch)), "overworld", 0, 0, 0, 15, 40, 1, 4); + } + + @Test + void straightSpline_pointsRunFromAToBInsideRadii() { + TrackSpline spline = samples(0, 10, 30); + TrackRoute route = route(List.of(spline), List.of(), "world", 0, 10, 5, 0, 25, 2, 3).orElseThrow(); + assertEquals(List.of(15.0, 18.0, 21.0, 23.0), route.points().stream().map(TrackSamplePoint::z).toList()); + assertSteps(route, 3); + } + + @Test + void againstSplineDirection_pointsRunFromAToB() { + TrackSpline spline = samples(0, 10, 30); + TrackRoute route = route(List.of(spline), List.of(), "world", 0, 25, 2, 0, 10, 5, 3).orElseThrow(); + assertEquals(List.of(23.0, 20.0, 17.0, 15.0), route.points().stream().map(TrackSamplePoint::z).toList()); + assertSteps(route, 3); + } + + @Test + void junction_pointsStayContinuousAndIncludeSplineExit() { + TrackSpline stem = samples(0, 80); + TrackSpline branch = line("world", 0, 40, 50, 40); + TrackJunction junction = join(stem, 40, branch); + TrackRoute forward = route(List.of(stem, branch), List.of(junction), + "world", 0, 11, 0, 20, 40, 0, 4).orElseThrow(); + assertEquals(49, forward.length(), 1e-6); + assertSteps(forward, 4); + assertTrue(forward.points().contains(new TrackSamplePoint(stem.getId(), 0, 0, 40))); + assertTrue(forward.points().contains(new TrackSamplePoint(branch.getId(), 0, 0, 40))); + boolean onBranch = false; + for (TrackSamplePoint point : forward.points()) { + if (point.splineId().equals(branch.getId())) { + onBranch = true; + assertEquals(40, point.z(), 1e-6); + } else { + assertTrue(!onBranch); + assertEquals(0, point.x(), 1e-6); + } + } + TrackRoute reverse = route(List.of(stem, branch), List.of(junction), + "world", 20, 40, 0, 0, 11, 0, 4).orElseThrow(); + assertSteps(reverse, 4); + assertEquals(branch.getId(), reverse.points().getFirst().splineId()); + assertEquals(stem.getId(), reverse.points().getLast().splineId()); + assertTrue(reverse.points().contains(new TrackSamplePoint(branch.getId(), 0, 0, 40))); + } + + @Test + void loop_shortWayCrossesSeamInBothDirections() { + TrackSpline loop = rectangle(); + TrackRoute reverse = route(List.of(loop), List.of(), "world", 2, 0, 0, 0, 8, 0, 2).orElseThrow(); + assertEquals(10, reverse.length(), 1e-6); + assertEquals(List.of( + point(loop, 2, 0), point(loop, 0, 0), point(loop, 0, 2), + point(loop, 0, 4), point(loop, 0, 6), point(loop, 0, 8)), reverse.points()); + assertSteps(reverse, 2); + TrackRoute forward = route(List.of(loop), List.of(), "world", 0, 8, 0, 2, 0, 0, 2).orElseThrow(); + assertEquals(reverse.points().reversed(), forward.points()); + } + + @Test + void junctionsOnLoop_crossSeamBetweenAnchors() { + for (TrackSpline loop : List.of(rectangle(), breakAt(rectangle(), 1))) { + TrackSpline first = line("world", 2, 0, 2, -10); + TrackSpline second = line("world", 0, 8, -10, 8); + List splines = List.of(loop, first, second); + List junctions = List.of(join(loop, 2, first), join(loop, 72, second)); + TrackRoute route = route(splines, junctions, "world", 2, -10, 0, -10, 8, 0, 2).orElseThrow(); + assertEquals(30, route.length(), 1e-6); + assertEquals(List.of( + point(loop, 2, 0), point(loop, 0, 0), point(loop, 0, 2), + point(loop, 0, 4), point(loop, 0, 6), point(loop, 0, 8)), + route.points().stream().filter(point -> point.splineId().equals(loop.getId())).toList()); + TrackRoute reverse = route(splines, junctions, "world", -10, 8, 0, 2, -10, 0, 2).orElseThrow(); + assertEquals(route.points().reversed(), reverse.points()); + } + } + + @Test + void repeatedPositionsKeepDistinctArcLengthSamples() { + TrackSpline spline = samples(0, 4, 6, 4, 8); + TrackRoute route = route(List.of(spline), List.of(), "world", 0, 0, 0, 0, 8, 0, 4).orElseThrow(); + assertEquals(12, route.length(), 1e-6); + assertEquals(List.of(0.0, 4.0, 4.0, 8.0), route.points().stream().map(TrackSamplePoint::z).toList()); + } + + @Test + void loop_brokenShortSideForcesLongWay() { + TrackSpline loop = breakAt(rectangle(), 3); + TrackRoute route = route(List.of(loop), List.of(), "world", 0, 0, 0, 0, 10, 0, 5).orElseThrow(); + assertEquals(70, route.length(), 1e-6); + assertSteps(route, 5); + assertTrue(route.points().contains(point(loop, 30, 0))); + assertTrue(route.points().contains(point(loop, 30, 10))); + assertTrue(route(List.of(breakAt(loop, 1)), List.of(), "world", 0, 0, 0, 0, 10, 0, 5).isEmpty()); + } + + @Test + void brokenLoopPieceStillCrossesSeam() { + TrackSpline loop = breakAt(rectangle(), 1); + TrackRoute route = route(List.of(loop), List.of(), "world", 2, 0, 0, 0, 8, 0, 2).orElseThrow(); + assertEquals(10, route.length(), 1e-6); + assertEquals(List.of( + point(loop, 2, 0), point(loop, 0, 0), point(loop, 0, 2), + point(loop, 0, 4), point(loop, 0, 6), point(loop, 0, 8)), route.points()); + } + + @Test + void overlappingAreas_haveOnePointAndZeroLength() { + TrackSpline spline = samples(0, 10, 30); + TrackRoute route = route(List.of(spline), List.of(), "world", 0, 10, 5, 0, 14, 2, 4).orElseThrow(); + assertEquals(0, route.length()); + assertEquals(1, route.points().size()); + TrackRoute seam = route(List.of(rectangle()), List.of(), "world", 0, 0, 0, 0, 0, 0, 4).orElseThrow(); + assertEquals(0, seam.length()); + assertEquals(1, seam.points().size()); + } + + @Test + void spacingDefaultsAndMinimumMatchSampleTrack() { + TrackSpline spline = samples(0, 10); + for (double spacing : new double[] {0, -1, Double.NaN, Double.POSITIVE_INFINITY, Double.NEGATIVE_INFINITY}) { + TrackRoute route = route(List.of(spline), List.of(), "world", 0, 0, 0, 0, 10, 0, spacing).orElseThrow(); + assertEquals(List.of(0.0, 4.0, 8.0, 10.0), route.points().stream().map(TrackSamplePoint::z).toList()); + } + TrackRoute small = route(List.of(spline), List.of(), "world", 0, 0, 0, 0, 10, 0, 0.1).orElseThrow(); + assertEquals(21, small.points().size()); + assertSteps(small, 0.5); + assertEquals(2, route(List.of(spline), List.of(), "world", 0, 0, 0, 0, 10, 0, 20).orElseThrow().points().size()); + } + + @Test + void pointsUseArcLengthIncludingHeight() { + TrackSpline spline = TrackSpline.fromPoints(UUID.randomUUID(), "world", false, + List.of(new double[] {0, 0, 0}, new double[] {0, 6, 8}, new double[] {10, 6, 8})); + TrackRoute route = route(List.of(spline), List.of(), "world", 0, 0, 0, 10, 8, 0, 5).orElseThrow(); + assertEquals(List.of( + new TrackSamplePoint(spline.getId(), 0, 0, 0), + new TrackSamplePoint(spline.getId(), 0, 3, 4), + new TrackSamplePoint(spline.getId(), 0, 6, 8), + new TrackSamplePoint(spline.getId(), 5, 6, 8), + new TrackSamplePoint(spline.getId(), 10, 6, 8)), route.points()); + } + + @Test + void recordCopiesPointsAndMakesThemUnmodifiable() { + List points = new ArrayList<>(List.of(new TrackSamplePoint(UUID.randomUUID(), 1, 2, 3))); + TrackRoute route = new TrackRoute(0, points); + points.clear(); + assertEquals(1, route.points().size()); + assertThrows(UnsupportedOperationException.class, () -> route.points().clear()); + } + + @Test + void registryDelegatesWithoutChangingTrack(@TempDir java.nio.file.Path dir) { + TrackRegistry registry = new TrackRegistry(dir.toFile()); + TrackSpline spline = registry.replace(samples(0, 10)); + TrackRoute expected = route(List.of(spline), List.of(), "world", 0, 0, 0, 0, 10, 0, 3).orElseThrow(); + assertEquals(expected, registry.shortestRoute("world", 0, 0, 0, 0, 10, 0, 3).orElseThrow()); + assertEquals(List.of(spline), List.copyOf(registry.all())); + } + + private static Optional route( + List splines, + List junctions, + String world, + double ax, double az, double radiusA, + double bx, double bz, double radiusB, + double spacing) { + OptionalDouble length = TrackRouteQuery.shortestRouteLength(splines, junctions, world, ax, az, radiusA, bx, bz, radiusB); + Optional route = TrackRouteQuery.shortestRoute(splines, junctions, world, ax, az, radiusA, bx, bz, radiusB, spacing); + assertEquals(length.isPresent(), route.isPresent()); + if (route.isPresent()) { + assertEquals(length.orElseThrow(), route.get().length(), 1e-6); + assertTrue(!route.get().points().isEmpty()); + TrackSamplePoint start = route.get().points().getFirst(); + TrackSamplePoint end = route.get().points().getLast(); + assertTrue(Math.hypot(start.x() - ax, start.z() - az) <= radiusA + 1e-6); + assertTrue(Math.hypot(end.x() - bx, end.z() - bz) <= radiusB + 1e-6); + assertSteps(route.get(), !Double.isFinite(spacing) || spacing <= 0 ? 4 : Math.max(spacing, 0.5)); + } + return route; + } + + private static void assertSteps(TrackRoute route, double spacing) { + for (int i = 1; i < route.points().size(); i++) { + TrackSamplePoint a = route.points().get(i - 1); + TrackSamplePoint b = route.points().get(i); + double dx = b.x() - a.x(); + double dy = b.y() - a.y(); + double dz = b.z() - a.z(); + assertTrue(Math.sqrt(dx * dx + dy * dy + dz * dz) <= spacing + 1e-6); + } + } + + private static TrackSamplePoint point(TrackSpline spline, double x, double z) { + return new TrackSamplePoint(spline.getId(), x, 0, z); + } + + private static TrackSpline samples(double... z) { + List points = new ArrayList<>(); + for (double at : z) { + points.add(new double[] {0, 0, at}); + } + return TrackSpline.fromPoints(UUID.randomUUID(), "world", false, points); + } + + private static TrackSpline rectangle() { + return TrackSpline.fromPoints(UUID.randomUUID(), "world", true, + List.of(new double[] {0, 0, 0}, new double[] {30, 0, 0}, new double[] {30, 0, 10}, new double[] {0, 0, 10})); + } + + private static TrackSpline breakAt(TrackSpline spline, int... edges) { + for (int edge : edges) { + spline = spline.withSegment(edge, spline.segment(edge).withBroken(true)); + } + return spline; + } + + private static TrackSpline line(String world, double x0, double z0, double x1, double z1) { + return TrackSpline.fromPoints(UUID.randomUUID(), world, false, + List.of(new double[] {x0, 0, z0}, new double[] {x1, 0, z1})); + } + + private static TrackJunction join(TrackSpline stem, double s, TrackSpline branch) { + return new TrackJunction(UUID.randomUUID(), stem.getId(), s, -1, TrackJunction.Side.RIGHT, + branch == null ? null : branch.getId(), false, branch == null ? 0 : branch.length()); + } +}