| 1 | package me.fridtjof.minecarttrains.managers; |
| 2 | |
| 3 | import me.fridtjof.minecarttrains.utils.FurnaceCarts; |
| 4 | import me.fridtjof.minecarttrains.utils.Rails; |
| 5 | import org.bukkit.Bukkit; |
| 6 | import org.bukkit.Location; |
| 7 | import org.bukkit.World; |
| 8 | import org.bukkit.entity.Minecart; |
| 9 | import org.bukkit.entity.minecart.PoweredMinecart; |
| 10 | import org.bukkit.util.Vector; |
| 11 | |
| 12 | import java.util.ArrayList; |
| 13 | import java.util.HashMap; |
| 14 | import java.util.List; |
| 15 | import java.util.Map; |
| 16 | import java.util.UUID; |
| 17 | |
| 18 | public class LinkPhysics |
| 19 | { |
| 20 | |
| 21 | private static final double STILL = 1.0E-4; |
| 22 | |
| 23 | private final LinkageManager linkageManager; |
| 24 | private final double aimedDistanceLow; |
| 25 | private final double aimedDistanceHigh; |
| 26 | private final double highSpeedGateSquared; |
| 27 | private final double sampleDistance; |
| 28 | private final double maxPathLength; |
| 29 | private final double snapEpsilonSquared; |
| 30 | private final Map<UUID, Trail> trails = new HashMap<>(); |
| 31 | |
| 32 | public LinkPhysics(LinkageManager linkageManager, ConfigManager configManager) |
| 33 | { |
| 34 | this.linkageManager = linkageManager; |
| 35 | this.aimedDistanceLow = configManager.physicsConfig.getConfig().getDouble("link.low_speed.distance.aimed"); |
| 36 | this.aimedDistanceHigh = configManager.physicsConfig.getConfig().getDouble("link.high_speed.distance.aimed"); |
| 37 | double highSpeedGate = configManager.physicsConfig.getConfig().getDouble("link.high_speed.gate_speed"); |
| 38 | this.highSpeedGateSquared = highSpeedGate * highSpeedGate; |
| 39 | this.sampleDistance = configManager.physicsConfig.getConfig().getDouble("link.kinematic.sample_distance"); |
| 40 | this.maxPathLength = configManager.physicsConfig.getConfig().getDouble("link.kinematic.max_path_length"); |
| 41 | double snapEpsilon = configManager.physicsConfig.getConfig().getDouble("link.kinematic.snap_epsilon"); |
| 42 | this.snapEpsilonSquared = snapEpsilon * snapEpsilon; |
| 43 | } |
| 44 | |
| 45 | public void updateTrain(Minecart cart) |
| 46 | { |
| 47 | if (!linkageManager.markTrainSolved(cart)) |
| 48 | { |
| 49 | return; |
| 50 | } |
| 51 | |
| 52 | List<Minecart> train = linkageManager.getTrain(cart); |
| 53 | if (train.size() < 2) |
| 54 | { |
| 55 | return; |
| 56 | } |
| 57 | |
| 58 | Minecart reference = linkageManager.findMotionFront(cart); |
| 59 | int referenceIndex = indexOf(train, reference); |
| 60 | if (referenceIndex < 0) |
| 61 | { |
| 62 | return; |
| 63 | } |
| 64 | |
| 65 | Vector heading = headingOf(reference); |
| 66 | boolean lowerIsAhead = lowerIsAhead(train, referenceIndex, heading); |
| 67 | UUID trainId = linkageManager.findHead(cart).getUniqueId(); |
| 68 | recordTrail(trainId, reference, heading); |
| 69 | |
| 70 | double spacing = aimedDistance(reference); |
| 71 | for (int i = referenceIndex + 1; i < train.size(); i++) |
| 72 | { |
| 73 | place(train.get(i), reference, heading, (i - referenceIndex) * spacing, !lowerIsAhead, trainId); |
| 74 | } |
| 75 | for (int i = referenceIndex - 1; i >= 0; i--) |
| 76 | { |
| 77 | place(train.get(i), reference, heading, (referenceIndex - i) * spacing, lowerIsAhead, trainId); |
| 78 | } |
| 79 | |
| 80 | pruneTrails(); |
| 81 | } |
| 82 | |
| 83 | private void place(Minecart follower, Minecart reference, Vector heading, double distance, boolean ahead, UUID trainId) |
| 84 | { |
| 85 | Rails.Pose pose = ahead |
| 86 | ? Rails.walk(reference.getLocation(), heading, distance) |
| 87 | : poseBehind(trainId, reference, heading, distance); |
| 88 | if (pose == null) |
| 89 | { |
| 90 | return; |
| 91 | } |
| 92 | |
| 93 | Vector tangent = pose.tangent().clone(); |
| 94 | if (tangent.lengthSquared() < STILL) |
| 95 | { |
| 96 | tangent = heading.clone(); |
| 97 | } |
| 98 | else |
| 99 | { |
| 100 | tangent.normalize(); |
| 101 | } |
| 102 | if (tangent.dot(heading) < 0) |
| 103 | { |
| 104 | tangent.multiply(-1); |
| 105 | } |
| 106 | |
| 107 | Location target = pose.location(); |
| 108 | target.setYaw(Rails.yaw(tangent)); |
| 109 | target.setPitch(0f); |
| 110 | |
| 111 | if (follower.getLocation().distanceSquared(target) > snapEpsilonSquared) |
| 112 | { |
| 113 | follower.teleport(target); |
| 114 | follower.setRotation(target.getYaw(), 0f); |
| 115 | } |
| 116 | |
| 117 | follower.setVelocity(tangent.multiply(reference.getVelocity().length())); |
| 118 | follower.setMaxSpeed(reference.getMaxSpeed()); |
| 119 | } |
| 120 | |
| 121 | private Rails.Pose poseBehind(UUID trainId, Minecart reference, Vector heading, double distance) |
| 122 | { |
| 123 | Trail trail = trails.get(trainId); |
| 124 | Rails.Pose fromTrail = trail == null ? null : trail.behind(reference.getWorld(), distance); |
| 125 | if (fromTrail != null) |
| 126 | { |
| 127 | return fromTrail; |
| 128 | } |
| 129 | return Rails.walk(reference.getLocation(), heading.clone().multiply(-1), distance); |
| 130 | } |
| 131 | |
| 132 | private void recordTrail(UUID trainId, Minecart reference, Vector heading) |
| 133 | { |
| 134 | Rails.Pose snapped = Rails.snap(reference.getLocation(), heading); |
| 135 | Location sample = snapped == null ? reference.getLocation() : snapped.location(); |
| 136 | trails.computeIfAbsent(trainId, id -> new Trail()).record(sample, heading, sampleDistance, maxPathLength); |
| 137 | } |
| 138 | |
| 139 | private void pruneTrails() |
| 140 | { |
| 141 | if (trails.size() < 16) |
| 142 | { |
| 143 | return; |
| 144 | } |
| 145 | trails.keySet().removeIf(id -> Bukkit.getEntity(id) == null); |
| 146 | } |
| 147 | |
| 148 | private Vector headingOf(Minecart cart) |
| 149 | { |
| 150 | Vector velocity = cart.getVelocity(); |
| 151 | if (velocity.lengthSquared() >= STILL) |
| 152 | { |
| 153 | return velocity.clone().normalize(); |
| 154 | } |
| 155 | if (cart instanceof PoweredMinecart powered && FurnaceCarts.hasDrive(powered)) |
| 156 | { |
| 157 | return new Vector(powered.getPushX(), 0, powered.getPushZ()).normalize(); |
| 158 | } |
| 159 | Vector rail = Rails.tangent(cart.getLocation(), new Vector(0, 0, 1)); |
| 160 | if (rail.lengthSquared() >= STILL) |
| 161 | { |
| 162 | return rail; |
| 163 | } |
| 164 | return new Vector(0, 0, 1); |
| 165 | } |
| 166 | |
| 167 | private boolean lowerIsAhead(List<Minecart> train, int referenceIndex, Vector heading) |
| 168 | { |
| 169 | if (referenceIndex > 0) |
| 170 | { |
| 171 | Vector toLower = train.get(referenceIndex - 1).getLocation().toVector() |
| 172 | .subtract(train.get(referenceIndex).getLocation().toVector()); |
| 173 | if (toLower.lengthSquared() >= STILL) |
| 174 | { |
| 175 | return toLower.dot(heading) > 0; |
| 176 | } |
| 177 | } |
| 178 | if (referenceIndex < train.size() - 1) |
| 179 | { |
| 180 | Vector toHigher = train.get(referenceIndex + 1).getLocation().toVector() |
| 181 | .subtract(train.get(referenceIndex).getLocation().toVector()); |
| 182 | if (toHigher.lengthSquared() >= STILL) |
| 183 | { |
| 184 | return toHigher.dot(heading) < 0; |
| 185 | } |
| 186 | } |
| 187 | return true; |
| 188 | } |
| 189 | |
| 190 | private double aimedDistance(Minecart reference) |
| 191 | { |
| 192 | if (reference.getVelocity().lengthSquared() > highSpeedGateSquared) |
| 193 | { |
| 194 | return aimedDistanceHigh; |
| 195 | } |
| 196 | return aimedDistanceLow; |
| 197 | } |
| 198 | |
| 199 | private int indexOf(List<Minecart> train, Minecart cart) |
| 200 | { |
| 201 | for (int i = 0; i < train.size(); i++) |
| 202 | { |
| 203 | if (train.get(i).getUniqueId().equals(cart.getUniqueId())) |
| 204 | { |
| 205 | return i; |
| 206 | } |
| 207 | } |
| 208 | return -1; |
| 209 | } |
| 210 | |
| 211 | private static final class Trail |
| 212 | { |
| 213 | private final List<double[]> points = new ArrayList<>(); |
| 214 | private Vector lastHeading; |
| 215 | |
| 216 | private void record(Location location, Vector heading, double sampleDistance, double maxLength) |
| 217 | { |
| 218 | if (lastHeading != null && heading.lengthSquared() >= STILL && lastHeading.dot(heading) < -0.15) |
| 219 | { |
| 220 | points.clear(); |
| 221 | } |
| 222 | if (heading.lengthSquared() >= STILL) |
| 223 | { |
| 224 | lastHeading = heading.clone().normalize(); |
| 225 | } |
| 226 | |
| 227 | if (points.isEmpty()) |
| 228 | { |
| 229 | points.add(coords(location)); |
| 230 | return; |
| 231 | } |
| 232 | |
| 233 | double[] last = points.get(points.size() - 1); |
| 234 | double dx = location.getX() - last[0]; |
| 235 | double dy = location.getY() - last[1]; |
| 236 | double dz = location.getZ() - last[2]; |
| 237 | double gapSquared = dx * dx + dy * dy + dz * dz; |
| 238 | if (gapSquared < sampleDistance * sampleDistance) |
| 239 | { |
| 240 | last[0] = location.getX(); |
| 241 | last[1] = location.getY(); |
| 242 | last[2] = location.getZ(); |
| 243 | return; |
| 244 | } |
| 245 | |
| 246 | Location cursor = new Location(location.getWorld(), last[0], last[1], last[2]); |
| 247 | Vector alongRails = new Vector(dx, dy, dz); |
| 248 | double gap = Math.sqrt(gapSquared); |
| 249 | double walked = 0; |
| 250 | while (walked + sampleDistance < gap) |
| 251 | { |
| 252 | Rails.Pose step = Rails.walk(cursor, alongRails, sampleDistance); |
| 253 | if (step == null) |
| 254 | { |
| 255 | break; |
| 256 | } |
| 257 | cursor = step.location(); |
| 258 | alongRails = step.tangent(); |
| 259 | points.add(coords(cursor)); |
| 260 | walked += sampleDistance; |
| 261 | } |
| 262 | points.add(coords(location)); |
| 263 | trim(maxLength); |
| 264 | } |
| 265 | |
| 266 | private Rails.Pose behind(World world, double distance) |
| 267 | { |
| 268 | if (points.size() < 2) |
| 269 | { |
| 270 | return null; |
| 271 | } |
| 272 | |
| 273 | double remaining = distance; |
| 274 | for (int i = points.size() - 1; i > 0; i--) |
| 275 | { |
| 276 | double[] newer = points.get(i); |
| 277 | double[] older = points.get(i - 1); |
| 278 | double dx = older[0] - newer[0]; |
| 279 | double dy = older[1] - newer[1]; |
| 280 | double dz = older[2] - newer[2]; |
| 281 | double length = Math.sqrt(dx * dx + dy * dy + dz * dz); |
| 282 | if (length < 1.0E-8) |
| 283 | { |
| 284 | continue; |
| 285 | } |
| 286 | if (remaining <= length) |
| 287 | { |
| 288 | double t = remaining / length; |
| 289 | Location location = new Location(world, newer[0] + dx * t, newer[1] + dy * t, newer[2] + dz * t); |
| 290 | return new Rails.Pose(location, new Vector(-dx, -dy, -dz).normalize()); |
| 291 | } |
| 292 | remaining -= length; |
| 293 | } |
| 294 | return null; |
| 295 | } |
| 296 | |
| 297 | private static double[] coords(Location location) |
| 298 | { |
| 299 | return new double[] {location.getX(), location.getY(), location.getZ()}; |
| 300 | } |
| 301 | |
| 302 | private void trim(double maxLength) |
| 303 | { |
| 304 | double length = 0; |
| 305 | for (int i = points.size() - 1; i > 0; i--) |
| 306 | { |
| 307 | double[] newer = points.get(i); |
| 308 | double[] older = points.get(i - 1); |
| 309 | double dx = older[0] - newer[0]; |
| 310 | double dy = older[1] - newer[1]; |
| 311 | double dz = older[2] - newer[2]; |
| 312 | length += Math.sqrt(dx * dx + dy * dy + dz * dz); |
| 313 | if (length > maxLength) |
| 314 | { |
| 315 | points.subList(0, i).clear(); |
| 316 | return; |
| 317 | } |
| 318 | } |
| 319 | } |
| 320 | } |
| 321 | } |