diff --git a/src/main/java/cam72cam/immersiverailroading/track/IIterableTrack.java b/src/main/java/cam72cam/immersiverailroading/track/IIterableTrack.java index f638209b4..28cd2544d 100644 --- 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 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 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)); } }