| 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 | Trail trail = trails.get(trainId); |
| 69 | if (trail != null && !reference.getUniqueId().equals(trail.ownerId)) |
| 70 | { |
| 71 | trails.remove(trainId); |
| 72 | } |
| 73 | recordTrail(trainId, reference, heading); |
| 74 | |
| 75 | double spacing = aimedDistance(reference); |
| 76 | for (int i = referenceIndex + 1; i < train.size(); i++) |
| 77 | { |
| 78 | place(train.get(i), reference, heading, (i - referenceIndex) * spacing, !lowerIsAhead, trainId); |
| 79 | } |
| 80 | for (int i = referenceIndex - 1; i >= 0; i--) |
| 81 | { |
| 82 | place(train.get(i), reference, heading, (referenceIndex - i) * spacing, lowerIsAhead, trainId); |
| 83 | } |
| 84 | |
| 85 | pruneTrails(); |
| 86 | } |
| 87 | |
| 88 | private void place(Minecart follower, Minecart reference, Vector heading, double distance, boolean ahead, UUID trainId) |
| 89 | { |
| 90 | Rails.Pose pose = ahead |
| 91 | ? Rails.walk(reference.getLocation(), heading, distance) |
| 92 | : poseBehind(trainId, reference, heading, distance); |
| 93 | if (pose == null) |
| 94 | { |
| 95 | return; |
| 96 | } |
| 97 | Rails.Pose onRail = Rails.snap(pose.location(), pose.tangent()); |
| 98 | if (onRail != null) |
| 99 | { |
| 100 | pose = onRail; |
| 101 | } |
| 102 | |
| 103 | Vector tangent = pose.tangent().clone(); |
| 104 | if (tangent.lengthSquared() < STILL) |
| 105 | { |
| 106 | tangent = heading.clone(); |
| 107 | } |
| 108 | else |
| 109 | { |
| 110 | tangent.normalize(); |
| 111 | } |
| 112 | if (tangent.dot(heading) < 0) |
| 113 | { |
| 114 | tangent.multiply(-1); |
| 115 | } |
| 116 | |
| 117 | Location target = pose.location(); |
| 118 | target.setYaw(Rails.yaw(tangent)); |
| 119 | target.setPitch(0f); |
| 120 | |
| 121 | if (follower.getLocation().distanceSquared(target) > snapEpsilonSquared) |
| 122 | { |
| 123 | follower.teleport(target); |
| 124 | follower.setRotation(target.getYaw(), 0f); |
| 125 | } |
| 126 | |
| 127 | follower.setVelocity(tangent.multiply(reference.getVelocity().length())); |
| 128 | follower.setMaxSpeed(reference.getMaxSpeed()); |
| 129 | } |
| 130 | |
| 131 | private Rails.Pose poseBehind(UUID trainId, Minecart reference, Vector heading, double distance) |
| 132 | { |
| 133 | Trail trail = trails.get(trainId); |
| 134 | Rails.Pose fromTrail = trail == null ? null : trail.behind(reference.getWorld(), distance); |
| 135 | if (fromTrail != null) |
| 136 | { |
| 137 | return fromTrail; |
| 138 | } |
| 139 | return Rails.walk(reference.getLocation(), heading.clone().multiply(-1), distance); |
| 140 | } |
| 141 | |
| 142 | private void recordTrail(UUID trainId, Minecart reference, Vector heading) |
| 143 | { |
| 144 | Rails.Pose snapped = Rails.snap(reference.getLocation(), heading); |
| 145 | Location sample = snapped == null ? reference.getLocation() : snapped.location(); |
| 146 | Trail trail = trails.computeIfAbsent(trainId, id -> new Trail()); |
| 147 | trail.ownerId = reference.getUniqueId(); |
| 148 | trail.record(sample, heading, sampleDistance, maxPathLength); |
| 149 | } |
| 150 | |
| 151 | private void pruneTrails() |
| 152 | { |
| 153 | if (trails.size() < 16) |
| 154 | { |
| 155 | return; |
| 156 | } |
| 157 | trails.keySet().removeIf(id -> Bukkit.getEntity(id) == null); |
| 158 | } |
| 159 | |
| 160 | private Vector headingOf(Minecart cart) |
| 161 | { |
| 162 | Vector velocity = cart.getVelocity(); |
| 163 | if (velocity.lengthSquared() >= STILL) |
| 164 | { |
| 165 | return velocity.clone().normalize(); |
| 166 | } |
| 167 | if (cart instanceof PoweredMinecart powered && FurnaceCarts.hasDrive(powered)) |
| 168 | { |
| 169 | return new Vector(powered.getPushX(), 0, powered.getPushZ()).normalize(); |
| 170 | } |
| 171 | Vector rail = Rails.tangent(cart.getLocation(), new Vector(0, 0, 1)); |
| 172 | if (rail.lengthSquared() >= STILL) |
| 173 | { |
| 174 | return rail; |
| 175 | } |
| 176 | return new Vector(0, 0, 1); |
| 177 | } |
| 178 | |
| 179 | private boolean lowerIsAhead(List<Minecart> train, int referenceIndex, Vector heading) |
| 180 | { |
| 181 | if (referenceIndex > 0) |
| 182 | { |
| 183 | Vector toLower = train.get(referenceIndex - 1).getLocation().toVector() |
| 184 | .subtract(train.get(referenceIndex).getLocation().toVector()); |
| 185 | if (toLower.lengthSquared() >= STILL) |
| 186 | { |
| 187 | return toLower.dot(heading) > 0; |
| 188 | } |
| 189 | } |
| 190 | if (referenceIndex < train.size() - 1) |
| 191 | { |
| 192 | Vector toHigher = train.get(referenceIndex + 1).getLocation().toVector() |
| 193 | .subtract(train.get(referenceIndex).getLocation().toVector()); |
| 194 | if (toHigher.lengthSquared() >= STILL) |
| 195 | { |
| 196 | return toHigher.dot(heading) < 0; |
| 197 | } |
| 198 | } |
| 199 | return true; |
| 200 | } |
| 201 | |
| 202 | private double aimedDistance(Minecart reference) |
| 203 | { |
| 204 | if (reference.getVelocity().lengthSquared() > highSpeedGateSquared) |
| 205 | { |
| 206 | return aimedDistanceHigh; |
| 207 | } |
| 208 | return aimedDistanceLow; |
| 209 | } |
| 210 | |
| 211 | private int indexOf(List<Minecart> train, Minecart cart) |
| 212 | { |
| 213 | for (int i = 0; i < train.size(); i++) |
| 214 | { |
| 215 | if (train.get(i).getUniqueId().equals(cart.getUniqueId())) |
| 216 | { |
| 217 | return i; |
| 218 | } |
| 219 | } |
| 220 | return -1; |
| 221 | } |
| 222 | |
| 223 | private static final class Trail |
| 224 | { |
| 225 | private UUID ownerId; |
| 226 | private final List<double[]> points = new ArrayList<>(); |
| 227 | private Vector lastHeading; |
| 228 | |
| 229 | private void record(Location location, Vector heading, double sampleDistance, double maxLength) |
| 230 | { |
| 231 | if (lastHeading != null && heading.lengthSquared() >= STILL && lastHeading.dot(heading) < -0.15) |
| 232 | { |
| 233 | points.clear(); |
| 234 | } |
| 235 | if (heading.lengthSquared() >= STILL) |
| 236 | { |
| 237 | lastHeading = heading.clone().normalize(); |
| 238 | } |
| 239 | |
| 240 | if (points.isEmpty()) |
| 241 | { |
| 242 | points.add(coords(location)); |
| 243 | return; |
| 244 | } |
| 245 | |
| 246 | double[] last = points.get(points.size() - 1); |
| 247 | double dx = location.getX() - last[0]; |
| 248 | double dy = location.getY() - last[1]; |
| 249 | double dz = location.getZ() - last[2]; |
| 250 | double gapSquared = dx * dx + dy * dy + dz * dz; |
| 251 | if (gapSquared < sampleDistance * sampleDistance) |
| 252 | { |
| 253 | last[0] = location.getX(); |
| 254 | last[1] = location.getY(); |
| 255 | last[2] = location.getZ(); |
| 256 | return; |
| 257 | } |
| 258 | |
| 259 | Location cursor = new Location(location.getWorld(), last[0], last[1], last[2]); |
| 260 | Vector alongRails = new Vector(dx, dy, dz); |
| 261 | double gap = Math.sqrt(gapSquared); |
| 262 | double walked = 0; |
| 263 | while (walked + sampleDistance < gap) |
| 264 | { |
| 265 | Rails.Pose step = Rails.walk(cursor, alongRails, sampleDistance); |
| 266 | if (step == null) |
| 267 | { |
| 268 | break; |
| 269 | } |
| 270 | cursor = step.location(); |
| 271 | alongRails = step.tangent(); |
| 272 | points.add(coords(cursor)); |
| 273 | walked += sampleDistance; |
| 274 | } |
| 275 | points.add(coords(location)); |
| 276 | trim(maxLength); |
| 277 | } |
| 278 | |
| 279 | private Rails.Pose behind(World world, double distance) |
| 280 | { |
| 281 | if (points.size() < 2) |
| 282 | { |
| 283 | return null; |
| 284 | } |
| 285 | |
| 286 | double remaining = distance; |
| 287 | for (int i = points.size() - 1; i > 0; i--) |
| 288 | { |
| 289 | double[] newer = points.get(i); |
| 290 | double[] older = points.get(i - 1); |
| 291 | double dx = older[0] - newer[0]; |
| 292 | double dy = older[1] - newer[1]; |
| 293 | double dz = older[2] - newer[2]; |
| 294 | double length = Math.sqrt(dx * dx + dy * dy + dz * dz); |
| 295 | if (length < 1.0E-8) |
| 296 | { |
| 297 | continue; |
| 298 | } |
| 299 | if (remaining <= length) |
| 300 | { |
| 301 | double t = remaining / length; |
| 302 | Location location = new Location(world, newer[0] + dx * t, newer[1] + dy * t, newer[2] + dz * t); |
| 303 | return new Rails.Pose(location, new Vector(-dx, -dy, -dz).normalize()); |
| 304 | } |
| 305 | remaining -= length; |
| 306 | } |
| 307 | return null; |
| 308 | } |
| 309 | |
| 310 | private static double[] coords(Location location) |
| 311 | { |
| 312 | return new double[] {location.getX(), location.getY(), location.getZ()}; |
| 313 | } |
| 314 | |
| 315 | private void trim(double maxLength) |
| 316 | { |
| 317 | double length = 0; |
| 318 | for (int i = points.size() - 1; i > 0; i--) |
| 319 | { |
| 320 | double[] newer = points.get(i); |
| 321 | double[] older = points.get(i - 1); |
| 322 | double dx = older[0] - newer[0]; |
| 323 | double dy = older[1] - newer[1]; |
| 324 | double dz = older[2] - newer[2]; |
| 325 | length += Math.sqrt(dx * dx + dy * dy + dz * dz); |
| 326 | if (length > maxLength) |
| 327 | { |
| 328 | points.subList(0, i).clear(); |
| 329 | return; |
| 330 | } |
| 331 | } |
| 332 | } |
| 333 | } |
| 334 | } |