From 9bf04d309379d89a910aa18e215f685333a56968 Mon Sep 17 00:00:00 2001
From: yoyosource <yoyosource@nidido.de>
Date: Thu, 23 Feb 2023 11:26:04 +0100
Subject: [PATCH] Add REntity RayTrace

Signed-off-by: yoyosource <yoyosource@nidido.de>
---
 .../bausystem/utils/RayTraceUtils.java        | 52 +++++++++++++++++++
 1 file changed, 52 insertions(+)

diff --git a/BauSystem_Main/src/de/steamwar/bausystem/utils/RayTraceUtils.java b/BauSystem_Main/src/de/steamwar/bausystem/utils/RayTraceUtils.java
index 463820f7..7bec2cbb 100644
--- a/BauSystem_Main/src/de/steamwar/bausystem/utils/RayTraceUtils.java
+++ b/BauSystem_Main/src/de/steamwar/bausystem/utils/RayTraceUtils.java
@@ -19,10 +19,15 @@
 
 package de.steamwar.bausystem.utils;
 
+import de.steamwar.entity.REntity;
+import de.steamwar.entity.RFallingBlockEntity;
+import lombok.Data;
 import lombok.experimental.UtilityClass;
 import org.bukkit.FluidCollisionMode;
 import org.bukkit.GameMode;
 import org.bukkit.Location;
+import org.bukkit.block.Block;
+import org.bukkit.block.BlockFace;
 import org.bukkit.entity.Entity;
 import org.bukkit.entity.Player;
 import org.bukkit.util.BoundingBox;
@@ -72,6 +77,53 @@ public class RayTraceUtils {
         }
     }
 
+    public static RRayTraceResult traceREntity(Player player, Location to, List<RFallingBlockEntity> entityList) {
+        if (player.getGameMode() == GameMode.SPECTATOR) {
+            return null;
+        }
+
+        Location startPos = to.clone().add(0.0, player.getEyeHeight(), 0.0);
+        Vector direction = to.getDirection();
+        RayTraceResult blocks = player.getWorld().rayTraceBlocks(startPos, direction, 10.0, FluidCollisionMode.NEVER, true);
+
+        REntity nearestHitEntity = null;
+        RRayTraceResult nearestHitResult = null;
+        double nearestDistanceSq = Double.MAX_VALUE;
+        for (REntity entity: entityList) {
+            if (!isOccluded(startPos.toVector(), direction, new Vector(entity.getX(), entity.getY() + 0.5, entity.getZ()))) continue;
+            double distanceSq = new Vector(entity.getX(), entity.getY() + 0.5, entity.getZ()).distanceSquared(startPos.toVector());
+            if (distanceSq < nearestDistanceSq) {
+                nearestHitEntity = entity;
+                nearestHitResult = new RRayTraceResult(new Vector(entity.getX(), entity.getY() + 0.5, entity.getZ()), null, null, entity);
+                nearestDistanceSq = distanceSq;
+            }
+        }
+        RRayTraceResult entities = nearestHitEntity == null ? null : new RRayTraceResult(nearestHitResult.getHitPosition(), nearestHitResult.getHitBlock(), nearestHitResult.getHitBlockFace(), nearestHitEntity);
+
+        if (blocks == null) {
+            return entities;
+        } else if (entities == null) {
+            return RRayTraceResult.fromRayTraceResult(blocks);
+        } else {
+            Vector startVec = startPos.toVector();
+            double blockHitDistance = startVec.distance(blocks.getHitPosition());
+            double entityHitDistanceSquared = startVec.distanceSquared(entities.getHitPosition());
+            return entityHitDistanceSquared < blockHitDistance * blockHitDistance ? entities : RRayTraceResult.fromRayTraceResult(blocks);
+        }
+    }
+
+    @Data
+    public static class RRayTraceResult {
+        private final Vector hitPosition;
+        private final Block hitBlock;
+        private final BlockFace hitBlockFace;
+        private final REntity hitEntity;
+
+        public static RRayTraceResult fromRayTraceResult(RayTraceResult rayTraceResult) {
+            return new RRayTraceResult(rayTraceResult.getHitPosition(), rayTraceResult.getHitBlock(), rayTraceResult.getHitBlockFace(), null);
+        }
+    }
+
     public static boolean isOccluded(Vector a, Vector n, Vector b) {
         // a = Head pos, n = View direction (normalized), b = entity center
         // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Vector_formulation