Added WorldMouse, Raycasting, fixed easing bug

src/main/java/dev/latvian/mods/kmath/util/Easing.java

@@ -17,7 +17,7 @@ public final class Easing {
 	public static final Codec<Easing> CODEC = Codecs.idChecked(Easing::toString, FUNCTIONS::get);
 
 	public static Easing add(String id, Double2DoubleFunction function) {
-		var easing = add(id, function);
+		var easing = new Easing(id, function);
 		FUNCTIONS.put(id, easing);
 		return easing;
 	}

src/main/java/dev/latvian/mods/kmath/util/Raycasting.java

@@ -0,0 +1,129 @@
+package dev.latvian.mods.kmath.util;
+
+import net.minecraft.entity.Entity;
+import net.minecraft.util.hit.HitResult;
+import net.minecraft.util.math.Box;
+import net.minecraft.util.math.Vec3d;
+import net.minecraft.world.RaycastContext;
+import net.minecraft.world.World;
+import org.jetbrains.annotations.Nullable;
+
+import java.util.ArrayList;
+import java.util.HashSet;
+import java.util.List;
+import java.util.Set;
+import java.util.function.Predicate;
+import java.util.stream.Collectors;
+
+public class Raycasting {
+
+	/**
+	 * Due to the way entities are stored (vertical entity "chunks"), collision detection fails with very tall entities.
+	 * To prevent this, we offer a separate raycasting method which searches a larger vertical box, but checks the distance
+	 * against any entity found to ensure it is still within our raycast.
+	 * <p>
+	 * Note that this method is much more performance-intensive than the alternative, so it should only be used when needed.
+	 */
+	public static Set<Entity> raycastVertical(World world, Vec3d from, Vec3d direction, double distance, double verticalRadius, double radius, Predicate<Entity> entityPredicate, boolean limitOne) {
+		direction = direction.normalize();
+
+		Set<Entity> found = new HashSet<>();
+		for (double i = 0; i < distance; i++) {
+			Box trueCollision = new Box(
+				from.getX() - radius,
+				from.getY() - radius,
+				from.getZ() - radius,
+				from.getX() + radius,
+				from.getY() + radius,
+				from.getZ() + radius);
+
+			Box fakeCollision = new Box(
+				from.getX() - radius,
+				from.getY() - verticalRadius,
+				from.getZ() - radius,
+				from.getX() + radius,
+				from.getY() + verticalRadius,
+				from.getZ() + radius);
+
+			List<Entity> boxed = new ArrayList<>(world.getEntitiesByClass(Entity.class, fakeCollision, entityPredicate));
+
+			// filter by entities that collide with the actual hitbox
+			boxed = boxed.stream().filter(
+				entity -> entity.getBoundingBox().intersects(trueCollision)).collect(Collectors.toList());
+
+			if (!boxed.isEmpty() && limitOne) {
+				Set<Entity> set = new HashSet<>();
+				set.add(boxed.get(0));
+				return set;
+			} else {
+				found.addAll(boxed);
+			}
+
+			from = from.add(direction);
+		}
+
+		return found;
+	}
+
+	@Nullable
+	public static Entity raycastOne(World world, Vec3d from, Vec3d direction, double distance, double radius, Predicate<Entity> entityPredicate) {
+		direction = direction.normalize();
+
+		Set<Entity> found = new HashSet<>();
+		for (double i = 0; i < distance; i++) {
+			ArrayList<Entity> boxed = new ArrayList<>(world.getEntitiesByClass(Entity.class, new Box(
+				from.getX() - radius,
+				from.getY() - radius,
+				from.getZ() - radius,
+				from.getX() + radius,
+				from.getY() + radius,
+				from.getZ() + radius), entityPredicate));
+
+			if (!boxed.isEmpty()) {
+				return boxed.get(0);
+			}
+
+			from = from.add(direction);
+		}
+
+		return null;
+	}
+
+	public static Set<Entity> raycast(World world, Vec3d from, Vec3d direction, double distance, double radius, Predicate<Entity> entityPredicate, boolean limitOne) {
+		direction = direction.normalize();
+
+		Set<Entity> found = new HashSet<>();
+		for (double i = 0; i < distance; i++) {
+			ArrayList<Entity> boxed = new ArrayList<>(world.getEntitiesByClass(Entity.class, new Box(
+				from.getX() - radius,
+				from.getY() - radius,
+				from.getZ() - radius,
+				from.getX() + radius,
+				from.getY() + radius,
+				from.getZ() + radius), entityPredicate));
+
+			if (!boxed.isEmpty() && limitOne) {
+				Set<Entity> set = new HashSet<>();
+				set.add(boxed.get(0));
+				return set;
+			} else {
+				found.addAll(boxed);
+			}
+
+			from = from.add(direction);
+		}
+
+		return found;
+	}
+
+	public static Vec3d distanceFromGround(Entity entity) {
+		return entity.getPos().subtract(raycastDown(entity, 128, 0, false).getPos());
+	}
+
+	public static HitResult raycastDown(Entity entity, double maxDistance, float tickDelta, boolean includeFluids) {
+		Vec3d cameraPosition = entity.getCameraPosVec(tickDelta);
+		Vec3d rotation = new Vec3d(0, -1, 0);
+		Vec3d vec3d3 = cameraPosition.add(rotation.x * maxDistance, rotation.y * maxDistance, rotation.z * maxDistance);
+		return entity.getWorld().raycast(new RaycastContext(cameraPosition, vec3d3, RaycastContext.ShapeType.OUTLINE, includeFluids ? RaycastContext.FluidHandling.ANY : RaycastContext.FluidHandling.NONE, entity));
+	}
+}

src/main/java/dev/latvian/mods/kmath/util/WorldMouse.java

@@ -0,0 +1,209 @@
+package dev.latvian.mods.kmath.util;
+
+import net.fabricmc.fabric.api.client.rendering.v1.WorldRenderContext;
+import net.fabricmc.fabric.api.client.rendering.v1.WorldRenderEvents;
+import net.minecraft.client.MinecraftClient;
+import net.minecraft.client.gui.screen.Screen;
+import net.minecraft.util.hit.BlockHitResult;
+import net.minecraft.util.hit.HitResult;
+import net.minecraft.util.math.BlockPos;
+import net.minecraft.util.math.Position;
+import net.minecraft.util.math.Vec3d;
+import net.minecraft.world.RaycastContext;
+import org.jetbrains.annotations.Nullable;
+import org.joml.Matrix4f;
+import org.joml.Vector2f;
+import org.joml.Vector4f;
+
+/**
+ * @author Lat
+ */
+public class WorldMouse implements WorldRenderEvents.AfterSetup {
+	private static WorldMouse instance;
+
+	public static WorldMouse get() {
+		if (instance == null) {
+			instance = new WorldMouse();
+			WorldRenderEvents.AFTER_SETUP.register(instance);
+		}
+
+		return instance;
+	}
+
+	public WorldRenderContext context;
+	public Matrix4f worldMatrix = new Matrix4f();
+	public Matrix4f invertedWorldMatrix = new Matrix4f();
+	public float scaledWidth = 1F;
+	public float scaledHeight = 1F;
+
+	/**
+	 * The current coordinate of the mouse in screen coordinates
+	 */
+	public final Vector2f screen = new Vector2f(0.5F, 0.5F);
+
+	/**
+	 * The current coordinate of the mouse in world coordinates
+	 */
+	public Vec3d world = Vec3d.ZERO;
+
+	/**
+	 * Raycast block hit result
+	 */
+	private BlockHitResult hit = null;
+
+	/**
+	 * Block position of the hit
+	 */
+	private BlockPos pos = null;
+
+	/**
+	 * Block position of the hit, offset to hit side if Alt key is held down
+	 */
+	private BlockPos altPos = null;
+
+	/**
+	 * Each frame, {@link WorldMouse} is marked as dirty. If a request for data is sent during a dirty frame, {@link WorldMouse} will
+	 * re-poll raycasting data. This is done because raycasting each frame is expensive (>=50% of tick in some scenarios).
+	 */
+	private boolean updateHit = true;
+
+	// Each frame: updated stored context.
+	@Override
+	public void afterSetup(WorldRenderContext ctx) {
+		context = ctx;
+
+		var mc = MinecraftClient.getInstance();
+		worldMatrix.set(context.projectionMatrix());
+		worldMatrix.mul(context.matrixStack().peek().getPositionMatrix());
+		invertedWorldMatrix.set(worldMatrix);
+		invertedWorldMatrix.invert();
+		scaledWidth = mc.getWindow().getScaledWidth();
+		scaledHeight = mc.getWindow().getScaledHeight();
+
+		if (mc.currentScreen != null) {
+			screen.set(mc.mouse.getX() * scaledWidth / (double) mc.getWindow().getWidth(), mc.mouse.getY() * scaledHeight / (double) mc.getWindow().getHeight());
+		} else {
+			screen.set(0.5D * scaledWidth, 0.5D * scaledHeight);
+		}
+
+		world = world(screen.x, screen.y);
+		updateHit = true;
+	}
+
+	private WorldMouse updateHit() {
+		if (updateHit) {
+			updateHit = false;
+			hit = null;
+			pos = null;
+			altPos = null;
+
+			if (context != null) {
+				var mc = MinecraftClient.getInstance();
+
+				var cameraPos = context.camera().getPos();
+				var wpos = world.add(cameraPos);
+				var dist = cameraPos.distanceTo(wpos);
+				var lerp = Math.min(1D, 1000D / dist);
+
+				hit = mc.world.raycast(new RaycastContext(
+					cameraPos,
+					cameraPos.lerp(wpos, lerp),
+					RaycastContext.ShapeType.OUTLINE,
+					RaycastContext.FluidHandling.SOURCE_ONLY,
+					mc.player
+				));
+
+				if (hit != null && hit.getType() == HitResult.Type.MISS) {
+					hit = null;
+				}
+
+				pos = hit == null ? null : hit.getBlockPos();
+				altPos = pos == null ? null : Screen.hasAltDown() ? pos.offset(hit.getSide()) : pos;
+			}
+		}
+
+		return this;
+	}
+
+	/**
+	 * Convert world position to screen coordinates
+	 *
+	 * @param worldPosX    X position
+	 * @param worldPosY    Y position
+	 * @param worldPosZ    Z position
+	 * @param allowOutside Allow outside the screen
+	 * @return Screen coordinates, or null if outside the screen
+	 */
+	@Nullable
+	public Vector2f screen(double worldPosX, double worldPosY, double worldPosZ, boolean allowOutside) {
+		var c = context.camera().getPos();
+		var v = new Vector4f((float) (worldPosX - c.x), (float) (worldPosY - c.y), (float) (worldPosZ - c.z), 1F);
+		v.mul(worldMatrix);
+		v.div(v.w);
+
+		if (allowOutside || v.z > 0F && v.z < 1F) {
+			return new Vector2f(
+				(0.5F + v.x * 0.5F) * scaledWidth,
+				(0.5F - v.y * 0.5F) * scaledHeight
+			);
+		}
+
+		return null;
+	}
+
+	/**
+	 * @param worldPosX X position
+	 * @param worldPosY Y position
+	 * @param worldPosZ Z position
+	 * @see WorldMouse#screen(double, double, double, boolean)
+	 */
+	@Nullable
+	public Vector2f screen(double worldPosX, double worldPosY, double worldPosZ) {
+		return screen(worldPosX, worldPosY, worldPosZ, false);
+	}
+
+	/**
+	 * @param worldPos     XYZ position
+	 * @param allowOutside Allow outside the screen
+	 * @see WorldMouse#screen(double, double, double, boolean)
+	 */
+	@Nullable
+	public Vector2f screen(Position worldPos, boolean allowOutside) {
+		return screen(worldPos.getX(), worldPos.getY(), worldPos.getZ(), allowOutside);
+	}
+
+	/**
+	 * @param worldPos XYZ position
+	 * @see WorldMouse#screen(double, double, double, boolean)
+	 */
+	@Nullable
+	public Vector2f screen(Position worldPos) {
+		return screen(worldPos.getX(), worldPos.getY(), worldPos.getZ(), false);
+	}
+
+	/**
+	 * Convert screen coordinates to world position. Use {@link WorldMouse#world} if you only care about current mouse position
+	 *
+	 * @param x screen coordinate x-position
+	 * @param y screen coordinate y-position
+	 * @return a {@link Vec3d} containing the screen coordinates in world position
+	 */
+	public Vec3d world(double x, double y) {
+		var v = new Vector4f((float) (x * 2D / scaledWidth - 1D), (float) (-(y * 2D / scaledHeight - 1D)), 1F, 1F);
+		v.mul(invertedWorldMatrix);
+		v.div(v.w);
+		return new Vec3d(v.x, v.y, v.z);
+	}
+
+	public BlockHitResult hit() {
+		return updateHit().hit;
+	}
+
+	public BlockPos pos() {
+		return updateHit().pos;
+	}
+
+	public BlockPos altPos() {
+		return updateHit().altPos;
+	}
+}