BlueGit

timfa minecart-trains

Files

minecart-trains / src / main / java / me / fridtjof / minecarttrains / utils / Rails.java

Rails.java

Rails.java 13.8 KiB Download Raw
1package me.fridtjof.minecarttrains.utils;
2
3import org.bukkit.Location;
4import org.bukkit.Tag;
5import org.bukkit.block.Block;
6import org.bukkit.block.BlockFace;
7import org.bukkit.block.data.Rail;
8import org.bukkit.util.Vector;
9
10import java.util.HashSet;
11import java.util.Set;
12
13public final class Rails
14{
15
16 public static final double HEIGHT = 0.0625D;
17 private static final int MAX_WALK_BLOCKS = 96;
18
19 private Rails()
20 {
21 }
22
23 public static boolean isRail(Block block)
24 {
25 return Tag.RAILS.isTagged(block.getType());
26 }
27
28 public static Block railBlock(Location location)
29 {
30 Block block = location.getBlock();
31 if (isRail(block))
32 {
33 return block;
34 }
35 Block below = block.getRelative(BlockFace.DOWN);
36 return isRail(below) ? below : block;
37 }
38
39 public static Vector tangent(Location location, Vector hint)
40 {
41 Pose snapped = snap(location, hint);
42 if (snapped != null)
43 {
44 return snapped.tangent();
45 }
46 return flattened(hint);
47 }
48
49 public static Vector tangent(Rail.Shape shape, Vector hint)
50 {
51 Vector[] options = axes(shape);
52 Vector best = options[0];
53 double bestDot = -2;
54 Vector hintCopy = hint.clone();
55 if (hintCopy.lengthSquared() < 1.0E-8)
56 {
57 return best.clone();
58 }
59 hintCopy.normalize();
60 for (Vector option : options)
61 {
62 double dot = option.dot(hintCopy);
63 if (dot > bestDot)
64 {
65 bestDot = dot;
66 best = option;
67 }
68 }
69 return best.clone();
70 }
71
72 public static Pose snap(Location location, Vector heading)
73 {
74 Block block = railBlock(location);
75 if (!(block.getBlockData() instanceof Rail rail))
76 {
77 return null;
78 }
79
80 Vector dir = unitOrZero(heading);
81 double[] local = localPoint(block, location);
82 double[][] points = facing(pathPoints(rail.getShape()), dir, local);
83 PathHit hit = project(points, local);
84 if (hit == null)
85 {
86 return null;
87 }
88 return new Pose(worldPoint(block, hit.x, hit.y, hit.z), hit.tangent.clone());
89 }
90
91 public static Pose walk(Location start, Vector heading, double distance)
92 {
93 if (distance <= 1.0E-8)
94 {
95 return snap(start, heading);
96 }
97
98 Pose snapped = snap(start, heading);
99 if (snapped == null)
100 {
101 return null;
102 }
103
104 Vector dir = unitOrZero(heading);
105 if (dir.lengthSquared() < 1.0E-8)
106 {
107 return snapped;
108 }
109
110 Location current = snapped.location();
111 Block block = railBlock(current);
112 double remaining = distance;
113 Set<Long> visited = new HashSet<>();
114
115 for (int guard = 0; guard < MAX_WALK_BLOCKS && remaining > 1.0E-8; guard++)
116 {
117 if (!(block.getBlockData() instanceof Rail rail))
118 {
119 return new Pose(current, dir);
120 }
121
122 Step step = stepInBlock(block, rail.getShape(), current, dir, remaining);
123 current = step.location;
124 dir = step.tangent;
125 remaining -= step.consumed;
126
127 if (remaining <= 1.0E-8)
128 {
129 return new Pose(current, dir);
130 }
131 if (step.exit == null || step.exit == BlockFace.SELF)
132 {
133 return new Pose(current, dir);
134 }
135
136 Block next = nextRail(block, step.exit);
137 if (next == null)
138 {
139 return new Pose(current, dir);
140 }
141 if (step.consumed < 1.0E-7 && !visited.add(packBlock(block)))
142 {
143 return new Pose(current, dir);
144 }
145
146 block = next;
147 Pose entered = snap(current, dir);
148 if (entered != null)
149 {
150 current = entered.location();
151 dir = entered.tangent();
152 }
153 }
154 return new Pose(current, dir);
155 }
156
157 public static float yaw(Vector tangent)
158 {
159 return (float) Math.toDegrees(Math.atan2(-tangent.getX(), tangent.getZ()));
160 }
161
162 private static Step stepInBlock(Block block, Rail.Shape shape, Location world, Vector dir, double remaining)
163 {
164 double[] local = localPoint(block, world);
165 double[][] points = facing(pathPoints(shape), dir, local);
166 PathHit here = project(points, local);
167 double pathLen = pathLength(points);
168 if (here == null)
169 {
170 return new Step(world.clone(), dir.clone(), remaining, null);
171 }
172
173 double target = here.along + remaining;
174 if (target <= pathLen - 1.0E-7)
175 {
176 PathHit dest = atDistance(points, target);
177 return new Step(worldPoint(block, dest.x, dest.y, dest.z), dest.tangent.clone(), remaining, null);
178 }
179
180 PathHit end = atDistance(points, pathLen);
181 double consumed = Math.max(0, pathLen - here.along);
182 return new Step(worldPoint(block, end.x, end.y, end.z), end.tangent.clone(), consumed, exitFace(end.x, end.z));
183 }
184
185 private static Block nextRail(Block current, BlockFace exit)
186 {
187 Block adjacent = current.getRelative(exit);
188 if (isRail(adjacent))
189 {
190 return adjacent;
191 }
192 Block up = adjacent.getRelative(BlockFace.UP);
193 if (isRail(up))
194 {
195 return up;
196 }
197 Block down = adjacent.getRelative(BlockFace.DOWN);
198 return isRail(down) ? down : null;
199 }
200
201 private static double[][] pathPoints(Rail.Shape shape)
202 {
203 return switch (shape)
204 {
205 case NORTH_SOUTH -> new double[][] {{0.5, 0, 0}, {0.5, 0, 1}};
206 case EAST_WEST -> new double[][] {{0, 0, 0.5}, {1, 0, 0.5}};
207 case ASCENDING_NORTH -> new double[][] {{0.5, 0, 1}, {0.5, 1, 0}};
208 case ASCENDING_SOUTH -> new double[][] {{0.5, 0, 0}, {0.5, 1, 1}};
209 case ASCENDING_EAST -> new double[][] {{0, 0, 0.5}, {1, 1, 0.5}};
210 case ASCENDING_WEST -> new double[][] {{1, 0, 0.5}, {0, 1, 0.5}};
211 case SOUTH_EAST -> new double[][] {{0.5, 0, 1}, {0.5, 0, 0.5}, {1, 0, 0.5}};
212 case SOUTH_WEST -> new double[][] {{0.5, 0, 1}, {0.5, 0, 0.5}, {0, 0, 0.5}};
213 case NORTH_WEST -> new double[][] {{0.5, 0, 0}, {0.5, 0, 0.5}, {0, 0, 0.5}};
214 case NORTH_EAST -> new double[][] {{0.5, 0, 0}, {0.5, 0, 0.5}, {1, 0, 0.5}};
215 };
216 }
217
218 private static double[][] facing(double[][] points, Vector dir, double[] local)
219 {
220 PathHit hit = project(points, local);
221 if (hit == null || hit.tangent.dot(dir) >= 0)
222 {
223 return points;
224 }
225 return reverse(points);
226 }
227
228 private static double[][] reverse(double[][] points)
229 {
230 double[][] reversed = new double[points.length][];
231 for (int i = 0; i < points.length; i++)
232 {
233 reversed[i] = points[points.length - 1 - i];
234 }
235 return reversed;
236 }
237
238 private static PathHit project(double[][] points, double[] local)
239 {
240 PathHit best = null;
241 double bestDist = Double.MAX_VALUE;
242 double along = 0;
243 for (int i = 0; i < points.length - 1; i++)
244 {
245 double[] a = points[i];
246 double[] b = points[i + 1];
247 double abx = b[0] - a[0];
248 double aby = b[1] - a[1];
249 double abz = b[2] - a[2];
250 double ab2 = abx * abx + aby * aby + abz * abz;
251 double t = 0;
252 if (ab2 > 1.0E-12)
253 {
254 t = ((local[0] - a[0]) * abx + (local[1] - a[1]) * aby + (local[2] - a[2]) * abz) / ab2;
255 t = Math.max(0, Math.min(1, t));
256 }
257 double px = a[0] + abx * t;
258 double py = a[1] + aby * t;
259 double pz = a[2] + abz * t;
260 double dist = (local[0] - px) * (local[0] - px) + (local[1] - py) * (local[1] - py) + (local[2] - pz) * (local[2] - pz);
261 double segLen = Math.sqrt(ab2);
262 if (dist < bestDist)
263 {
264 bestDist = dist;
265 Vector tangent = segLen > 1.0E-8
266 ? new Vector(abx / segLen, aby / segLen, abz / segLen)
267 : new Vector();
268 best = new PathHit(along + segLen * t, px, py, pz, tangent);
269 }
270 along += segLen;
271 }
272 return best;
273 }
274
275 private static PathHit atDistance(double[][] points, double distance)
276 {
277 double along = 0;
278 for (int i = 0; i < points.length - 1; i++)
279 {
280 double[] a = points[i];
281 double[] b = points[i + 1];
282 double seg = dist(a, b);
283 boolean last = i == points.length - 2;
284 if (distance <= along + seg || last)
285 {
286 double t = seg < 1.0E-8 ? 0 : (distance - along) / seg;
287 t = Math.max(0, Math.min(1, t));
288 Vector tangent = seg > 1.0E-8
289 ? new Vector((b[0] - a[0]) / seg, (b[1] - a[1]) / seg, (b[2] - a[2]) / seg)
290 : new Vector();
291 return new PathHit(along + seg * t, a[0] + (b[0] - a[0]) * t, a[1] + (b[1] - a[1]) * t, a[2] + (b[2] - a[2]) * t, tangent);
292 }
293 along += seg;
294 }
295 double[] last = points[points.length - 1];
296 return new PathHit(along, last[0], last[1], last[2], new Vector());
297 }
298
299 private static double pathLength(double[][] points)
300 {
301 double length = 0;
302 for (int i = 0; i < points.length - 1; i++)
303 {
304 length += dist(points[i], points[i + 1]);
305 }
306 return length;
307 }
308
309 private static double dist(double[] a, double[] b)
310 {
311 double dx = b[0] - a[0];
312 double dy = b[1] - a[1];
313 double dz = b[2] - a[2];
314 return Math.sqrt(dx * dx + dy * dy + dz * dz);
315 }
316
317 private static BlockFace exitFace(double x, double z)
318 {
319 if (x >= 0.999)
320 {
321 return BlockFace.EAST;
322 }
323 if (x <= 0.001)
324 {
325 return BlockFace.WEST;
326 }
327 if (z >= 0.999)
328 {
329 return BlockFace.SOUTH;
330 }
331 if (z <= 0.001)
332 {
333 return BlockFace.NORTH;
334 }
335 return BlockFace.SELF;
336 }
337
338 private static double[] localPoint(Block block, Location location)
339 {
340 return new double[] {
341 location.getX() - block.getX(),
342 location.getY() - (block.getY() + HEIGHT),
343 location.getZ() - block.getZ()
344 };
345 }
346
347 private static Location worldPoint(Block block, double x, double y, double z)
348 {
349 return new Location(block.getWorld(), block.getX() + x, block.getY() + HEIGHT + y, block.getZ() + z);
350 }
351
352 private static Vector[] axes(Rail.Shape shape)
353 {
354 return switch (shape)
355 {
356 case NORTH_SOUTH -> new Vector[] {vec(0, 0, 1), vec(0, 0, -1)};
357 case EAST_WEST -> new Vector[] {vec(1, 0, 0), vec(-1, 0, 0)};
358 case ASCENDING_NORTH -> new Vector[] {norm(0, 1, -1), norm(0, -1, 1)};
359 case ASCENDING_SOUTH -> new Vector[] {norm(0, 1, 1), norm(0, -1, -1)};
360 case ASCENDING_EAST -> new Vector[] {norm(1, 1, 0), norm(-1, -1, 0)};
361 case ASCENDING_WEST -> new Vector[] {norm(-1, 1, 0), norm(1, -1, 0)};
362 case SOUTH_EAST -> new Vector[] {vec(0, 0, 1), vec(1, 0, 0)};
363 case SOUTH_WEST -> new Vector[] {vec(0, 0, 1), vec(-1, 0, 0)};
364 case NORTH_WEST -> new Vector[] {vec(0, 0, -1), vec(-1, 0, 0)};
365 case NORTH_EAST -> new Vector[] {vec(0, 0, -1), vec(1, 0, 0)};
366 };
367 }
368
369 private static Vector flattened(Vector hint)
370 {
371 Vector flat = hint.clone().setY(0);
372 if (flat.lengthSquared() < 1.0E-8)
373 {
374 return new Vector(0, 0, 0);
375 }
376 return flat.normalize();
377 }
378
379 private static Vector unitOrZero(Vector heading)
380 {
381 if (heading == null || heading.lengthSquared() < 1.0E-8)
382 {
383 return new Vector();
384 }
385 return heading.clone().normalize();
386 }
387
388 private static long packBlock(Block block)
389 {
390 return ((long) block.getX() * 73428767L) ^ ((long) block.getZ() * 19349663L) ^ block.getY();
391 }
392
393 private static Vector vec(double x, double y, double z)
394 {
395 return new Vector(x, y, z);
396 }
397
398 private static Vector norm(double x, double y, double z)
399 {
400 return new Vector(x, y, z).normalize();
401 }
402
403 public static final class Pose
404 {
405 private final Location location;
406 private final Vector tangent;
407
408 public Pose(Location location, Vector tangent)
409 {
410 this.location = location;
411 this.tangent = tangent;
412 }
413
414 public Location location()
415 {
416 return location;
417 }
418
419 public Vector tangent()
420 {
421 return tangent;
422 }
423 }
424
425 private static final class PathHit
426 {
427 private final double along;
428 private final double x;
429 private final double y;
430 private final double z;
431 private final Vector tangent;
432
433 private PathHit(double along, double x, double y, double z, Vector tangent)
434 {
435 this.along = along;
436 this.x = x;
437 this.y = y;
438 this.z = z;
439 this.tangent = tangent;
440 }
441 }
442
443 private static final class Step
444 {
445 private final Location location;
446 private final Vector tangent;
447 private final double consumed;
448 private final BlockFace exit;
449
450 private Step(Location location, Vector tangent, double consumed, BlockFace exit)
451 {
452 this.location = location;
453 this.tangent = tangent;
454 this.consumed = consumed;
455 this.exit = exit;
456 }
457 }
458}