Switch to binary tree pathing in offsetFromTrack

TeamOpenIndustry · Oct 5, 2023 · 8a5ba39 · 8a5ba39
1 parent 7c344d1
commit 8a5ba39
Showing 1 changed file with 26 additions and 4 deletions.
diff --git a/src/main/java/cam72cam/immersiverailroading/track/IIterableTrack.java b/src/main/java/cam72cam/immersiverailroading/track/IIterableTrack.java
@@ -12,17 +12,39 @@ public interface IIterableTrack {
     List<BuilderBase> getSubBuilders();
 
     default double offsetFromTrack(RailInfo info, Vec3i pos, Vec3d position) {
-        double distSquared = 100 * 100;
 
         // Convert to relative
-        position = position.subtract(info.placementInfo.placementPosition).subtract(pos);
+        Vec3d relative = position.subtract(info.placementInfo.placementPosition).subtract(pos);
+        relative = relative.add(0, -(relative.y % 1), 0);
+
+        List<PosStep> positions = getPath(info.settings.gauge.scale() / 8);
 
-        for (Vec3d gagPos : getPath(info.settings.gauge.scale()/8)) {
-            double offSquared = gagPos.distanceToSquared(position.add(0, -(position.y % 1), 0));
+        /*
+        double distSquared = 100 * 100;
+        for (Vec3d gagPos : positions) {
+            double offSquared = gagPos.distanceToSquared(relative);
             if (offSquared < distSquared) {
                 distSquared = offSquared;
             }
         }
         return Math.sqrt(distSquared);
+        */
+
+        int left = 0;
+        double leftDistance = positions.get(left).distanceToSquared(relative);
+        int right = positions.size() - 1;
+        double rightDistance = positions.get(right).distanceToSquared(relative);
+        while (right - left > 1) {
+            double midIdx = left + (right - left) / 2f;
+
+            if (leftDistance > rightDistance) {
+                left = (int) Math.floor(midIdx);
+                leftDistance = positions.get(left).distanceToSquared(relative);
+            } else {
+                right = (int) Math.ceil(midIdx);
+                rightDistance = positions.get(right).distanceToSquared(relative);
+            }
+        }
+        return Math.sqrt(Math.min(rightDistance, leftDistance));
     }
 }