diff --git a/src/main/java/dev/latvian/mods/kmath/KMath.java b/src/main/java/dev/latvian/mods/kmath/KMath.java index 97566d9..ca7fc76 100644 --- 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 new file mode 100644 index 0000000..fc85ee7 --- /dev/null +++ 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 { + public static class BLTIterator implements Iterator { + 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 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 index ef29a4a..c0f246b 100644 --- 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); + } }