Added BresenhamLineTracer, Rotations.getVector, KMath.curve

latvian-dev · Apr 20, 2024 · 7ad3753 · 7ad3753
1 parent 012cd2c
commit 7ad3753
Show file tree

Hide file tree

Showing 3 changed files with 180 additions and 7 deletions.
diff --git a/src/main/java/dev/latvian/mods/kmath/KMath.java b/src/main/java/dev/latvian/mods/kmath/KMath.java
@@ -132,4 +132,10 @@ static double smootherstep(double t) {
 	static float smootherstep(float t) {
 		return t * t * t * (t * (t * 6F - 15F) + 10F);
 	}
+
+	static double curve(double t, double p1x, double p2x, double p3x) {
+		double t1 = 1D - t;
+		double v = -(p1x - 8D * p2x + p3x) / 6D;
+		return t1 * t1 * t1 * p1x + 3D * t * t1 * t1 * v + 3D * t * t * t1 * v + t * t * t * p3x;
+	}
 }
diff --git a/src/main/java/dev/latvian/mods/kmath/util/BresenhamLineTracer.java b/src/main/java/dev/latvian/mods/kmath/util/BresenhamLineTracer.java
@@ -0,0 +1,155 @@
+package dev.latvian.mods.kmath.util;
+
+import net.minecraft.util.math.BlockPos;
+import net.minecraft.util.math.Direction;
+import org.jetbrains.annotations.NotNull;
+
+import java.util.Iterator;
+
+// https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
+// https://www.youtube.com/watch?v=RGB-wlatStc
+public class BresenhamLineTracer implements Iterable<BlockPos> {
+	public static class BLTIterator implements Iterator<BlockPos> {
+		private final BresenhamLineTracer tracer;
+		private final BlockPos.Mutable voxel;
+		private int iteration, error1, error2;
+
+		public BLTIterator(BresenhamLineTracer tracer) {
+			this.tracer = tracer;
+			this.voxel = tracer.start.mutableCopy();
+			this.iteration = 0;
+			this.error1 = tracer.startError1;
+			this.error2 = tracer.startError2;
+		}
+
+		@Override
+		public boolean hasNext() {
+			return this.iteration < tracer.length;
+		}
+
+		@Override
+		public BlockPos next() {
+			final BlockPos ret = this.voxel.toImmutable();
+
+			if (this.hasNext()) {
+				this.traverse();
+				this.iteration++;
+			}
+
+			return ret;
+		}
+
+		private void traverse() {
+			switch (tracer.axis) {
+				case X -> {
+					if (this.error1 > 0) {
+						this.bumpY();
+						this.error1 -= tracer.doubleAbsDx;
+					}
+					if (this.error2 > 0) {
+						this.bumpZ();
+						this.error2 -= tracer.doubleAbsDx;
+					}
+
+					this.error1 += tracer.doubleAbsDy;
+					this.error2 += tracer.doubleAbsDz;
+
+					this.bumpX();
+				}
+				case Y -> {
+					if (this.error1 > 0) {
+						this.bumpX();
+						this.error1 -= tracer.doubleAbsDy;
+					}
+					if (this.error2 > 0) {
+						this.bumpZ();
+						this.error2 -= tracer.doubleAbsDy;
+					}
+
+					this.error1 += tracer.doubleAbsDx;
+					this.error2 += tracer.doubleAbsDz;
+
+					this.bumpY();
+				}
+				case Z -> {
+					if (this.error1 > 0) {
+						this.bumpY();
+						this.error1 -= tracer.doubleAbsDz;
+					}
+					if (this.error2 > 0) {
+						this.bumpX();
+						this.error2 -= tracer.doubleAbsDz;
+					}
+
+					this.error1 += tracer.doubleAbsDy;
+					this.error2 += tracer.doubleAbsDx;
+
+					this.bumpZ();
+				}
+			}
+		}
+
+		private void bumpX() {
+			this.voxel.move(tracer.xD, 0, 0);
+		}
+
+		private void bumpY() {
+			this.voxel.move(0, tracer.yD, 0);
+		}
+
+		private void bumpZ() {
+			this.voxel.move(0, 0, tracer.zD);
+		}
+	}
+
+	public final BlockPos start;
+	public final int xD, yD, zD, doubleAbsDx, doubleAbsDy, doubleAbsDz, length;
+	public final Direction.Axis axis;
+	public final int startError1, startError2;
+
+	public BresenhamLineTracer(BlockPos start, BlockPos dest) {
+		this(start, dest.getX() - start.getX(), dest.getY() - start.getY(), dest.getZ() - start.getZ());
+	}
+
+	public BresenhamLineTracer(BlockPos start, int xDiff, int yDiff, int zDiff) {
+		this.start = start;
+
+		this.xD = (xDiff < 0) ? -1 : 1;
+		this.yD = (yDiff < 0) ? -1 : 1;
+		this.zD = (zDiff < 0) ? -1 : 1;
+
+		int absDx = Math.abs(xDiff);
+		int absDy = Math.abs(yDiff);
+		int absDz = Math.abs(zDiff);
+
+		this.doubleAbsDx = absDx << 1;
+		this.doubleAbsDy = absDy << 1;
+		this.doubleAbsDz = absDz << 1;
+
+		if (absDx >= absDy && absDx >= absDz) {
+			this.startError1 = this.doubleAbsDy - absDx;
+			this.startError2 = this.doubleAbsDz - absDx;
+
+			this.axis = Direction.Axis.X;
+			this.length = absDx + 1;
+		} else if (absDy >= absDx && absDy >= absDz) {
+			this.startError1 = this.doubleAbsDy - absDy;
+			this.startError2 = this.doubleAbsDz - absDy;
+
+			this.axis = Direction.Axis.Y;
+			this.length = absDy + 1;
+		} else {
+			this.startError1 = this.doubleAbsDy - absDz;
+			this.startError2 = this.doubleAbsDz - absDz;
+
+			this.axis = Direction.Axis.Z;
+			this.length = absDz + 1;
+		}
+	}
+
+	@NotNull
+	@Override
+	public Iterator<BlockPos> iterator() {
+		return new BLTIterator(this);
+	}
+}
diff --git a/src/main/java/dev/latvian/mods/kmath/util/Rotations.java b/src/main/java/dev/latvian/mods/kmath/util/Rotations.java
@@ -1,10 +1,11 @@
 package dev.latvian.mods.kmath.util;
 
+import net.minecraft.entity.Entity;
 import net.minecraft.util.math.BlockPos;
 import net.minecraft.util.math.MathHelper;
 import net.minecraft.util.math.Vec3d;
 
-public class Rotations {
+public interface Rotations {
 	/**
 	 * Calculates the minecraft yaw angle in degrees for the given x and z delta values
 	 * Note: Default facing is south for vanilla entities(0)
@@ -13,16 +14,16 @@ public class Rotations {
 	 * @param z z delta
 	 * @return angle in degree
 	 */
-	public static float getYaw(double x, double z) {
+	static float getYaw(double x, double z) {
 		// Minecraft yaw is always offset by 90 degree
 		return MathHelper.wrapDegrees((float) (Math.toDegrees(Math.atan2(z, x)) - 90));
 	}
 
-	public static float getYaw(Vec3d direction) {
+	static float getYaw(Vec3d direction) {
 		return getYaw(direction.getX(), direction.getZ());
 	}
 
-	public static float getYaw(BlockPos direction) {
+	static float getYaw(BlockPos direction) {
 		return getYaw(direction.getX(), direction.getZ());
 	}
 
@@ -32,7 +33,7 @@ public static float getYaw(BlockPos direction) {
 	 * @param y delta 0-1
 	 * @return angle in degree
 	 */
-	public static float getPitch(double y) {
+	static float getPitch(double y) {
 		if (y > 1) {
 			return Float.NaN;
 		}
@@ -41,11 +42,22 @@ public static float getPitch(double y) {
 		return MathHelper.wrapDegrees((float) -Math.toDegrees(Math.asin(y)));
 	}
 
-	public static float getPitch(Vec3d direction) {
+	static float getPitch(Vec3d direction) {
 		return getPitch(direction.normalize().y);
 	}
 
-	public static float getPitch(BlockPos direction) {
+	static float getPitch(BlockPos direction) {
 		return getPitch(Vec3d.of(direction));
 	}
+
+	static Vec3d getVectorFromBodyYaw(Entity entity) {
+		return getVector(entity.getPitch(), entity.getBodyYaw());
+	}
+
+	static Vec3d getVector(double pitch, double yaw) {
+		double p = Math.toRadians(pitch);
+		double y = Math.toRadians(-yaw);
+		double h = Math.cos(p);
+		return new Vec3d(Math.sin(y) * h, -Math.sin(p), Math.cos(y) * h);
+	}
 }