From fcc0bb51cc5b92036613d13fbd3ffa86179cbc45 Mon Sep 17 00:00:00 2001
From: KP <kpcftsz@users.noreply.github.com>
Date: Wed, 24 Jul 2024 03:42:46 -0500
Subject: [PATCH] Add raycast utilities

---
 KP3Dii/KP3Dii.vcxproj         |  2 +
 KP3Dii/KP3Dii.vcxproj.filters |  2 +
 KP3Dii/src/KP3D_Raycast.cpp   | 85 +++++++++++++++++++++++++++++++++++
 KP3Dii/src/KP3D_Raycast.h     | 22 +++++++++
 4 files changed, 111 insertions(+)
 create mode 100644 KP3Dii/src/KP3D_Raycast.cpp
 create mode 100644 KP3Dii/src/KP3D_Raycast.h

diff --git a/KP3Dii/KP3Dii.vcxproj b/KP3Dii/KP3Dii.vcxproj
index 9ae87fb..e05954e 100644
--- a/KP3Dii/KP3Dii.vcxproj
+++ b/KP3Dii/KP3Dii.vcxproj
@@ -157,6 +157,7 @@
     <ClInclude Include="src\KP3D_Packet.inl.h" />
     <ClInclude Include="src\KP3D_Network.h" />
     <ClInclude Include="src\KP3D_Quaternion.h" />
+    <ClInclude Include="src\KP3D_Raycast.h" />
     <ClInclude Include="src\KP3D_Renderer2D.h" />
     <ClInclude Include="src\KP3D_Renderer3D.h" />
     <ClInclude Include="src\KP3D_Shader.h" />
@@ -207,6 +208,7 @@
     <ClCompile Include="src\KP3D_Packet.cpp" />
     <ClCompile Include="src\KP3D_Network.cpp" />
     <ClCompile Include="src\KP3D_Quaternion.cpp" />
+    <ClCompile Include="src\KP3D_Raycast.cpp" />
     <ClCompile Include="src\KP3D_Renderer2D.cpp" />
     <ClCompile Include="src\KP3D_Renderer3D.cpp" />
     <ClCompile Include="src\KP3D_Shader.cpp" />
diff --git a/KP3Dii/KP3Dii.vcxproj.filters b/KP3Dii/KP3Dii.vcxproj.filters
index 227e954..62374ac 100644
--- a/KP3Dii/KP3Dii.vcxproj.filters
+++ b/KP3Dii/KP3Dii.vcxproj.filters
@@ -40,6 +40,7 @@
     <ClInclude Include="src\KP3D_Map.h" />
     <ClInclude Include="src\KP3D_Noise.h" />
     <ClInclude Include="src\KP3D_Geometry.h" />
+    <ClInclude Include="src\KP3D_Raycast.h" />
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\KP3D_Vec2.cpp" />
@@ -93,6 +94,7 @@
     <ClCompile Include="ext\src\clipper2\clipper.offset.cpp" />
     <ClCompile Include="ext\src\clipper2\clipper.rectclip.cpp" />
     <ClCompile Include="src\KP3D_Geometry.cpp" />
+    <ClCompile Include="src\KP3D_Raycast.cpp" />
   </ItemGroup>
   <ItemGroup>
     <None Include=".clang-format" />
diff --git a/KP3Dii/src/KP3D_Raycast.cpp b/KP3Dii/src/KP3D_Raycast.cpp
new file mode 100644
index 0000000..17f06a6
--- /dev/null
+++ b/KP3Dii/src/KP3D_Raycast.cpp
@@ -0,0 +1,85 @@
+#include "KP3D_Raycast.h"
+
+namespace kp3d {
+
+std::array<Vec3, 2> GetRayFromCamera(Camera& in)
+{
+	return std::array<Vec3, 2>{in.position, in.forward};
+}
+
+bool RayIntersectsPlane(const Vec3& n, const Vec3& p0, const Vec3& l0, const Vec3& l, float& t)
+{
+	// Assuming all vectors are normalized
+	float denom = n.Dot(l);
+	if (denom > 1e-6)
+	{
+		Vec3 p0l0 = p0 - l0;
+		t = p0l0.Dot(n);
+
+		return t >= 0;
+	}
+
+	return false;
+}
+
+bool RayIntersectsQuad(const std::array<Vec3, 2>& ray, const Vec3& q1, const Vec3& q2, const Vec3& q3, const Vec3& q4, Vec3* out)
+{
+	Vec3 dq21 = q2 - q1;
+	Vec3 dq31 = q3 - q1;
+	Vec3 n = dq21.Cross(dq31);
+
+	Vec3 dr = ray[0] - ray[1];
+
+	float ndr = n.Dot(dr);
+
+	if (fabsf(ndr) < 1e-6f)
+		return false;
+
+	float t = -n.Dot(ray[0] - q1);
+	Vec3 M = ray[0] + dr * t;
+	// ^ Replace with RayIntersectsPlane
+
+	Vec3 dmq1 = M - q1;
+	float u = dmq1.Dot(dq21);
+	float v = dmq1.Dot(dq31);
+
+	if (out)
+		*out = M;
+
+	return (u >= 0.0f && u <= dq21.Dot(dq21) && v >= 0.0f && v <= dq31.Dot(dq31));
+}
+
+bool RayIntersectsTriangle(const Vec3& ray_origin, const Vec3& ray_vector, Triangle* in_triangle, Vec3& out_intersection_point)
+{
+	const float e = 0.000001f;
+	Vec3 vertex0 = in_triangle->vertices[0];
+	Vec3 vertex1 = in_triangle->vertices[1];
+	Vec3 vertex2 = in_triangle->vertices[2];
+	Vec3 edge1, edge2, h, s, q;
+	float a, f, u, v;
+	edge1 = vertex1 - vertex0;
+	edge2 = vertex2 - vertex0;
+	h = ray_vector.Cross(edge2);
+	a = edge1.Dot(h);
+	if (a > -e && a < e)
+		return false;
+	f = 1.0 / a;
+	s = ray_origin - vertex0;
+	u = f * s.Dot(h);
+	if (u < 0.0 || u > 1.0)
+		return false;
+	q = s.Cross(edge1);
+	v = f * ray_vector.Dot(q);
+	if (v < 0.0 || u + v > 1.0)
+		return false;
+	float t = f * edge2.Dot(q);
+	if (t > e)
+	{
+		out_intersection_point = ray_origin + ray_vector * t;
+		return true;
+	}
+	else
+		return false;
+}
+
+} // namespace kp3d
diff --git a/KP3Dii/src/KP3D_Raycast.h b/KP3Dii/src/KP3D_Raycast.h
new file mode 100644
index 0000000..3342e81
--- /dev/null
+++ b/KP3Dii/src/KP3D_Raycast.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include "KP3D_Camera.h"
+#include "KP3D_Common.h"
+#include "KP3D_Math.h"
+
+namespace kp3d {
+
+struct Triangle
+{
+	Vec3 vertices[3];
+	int type;
+	int id;
+};
+
+std::array<Vec3, 2> GetRayFromCamera(Camera& in);
+
+bool RayIntersectsPlane(const Vec3& n, const Vec3& p0, const Vec3& l0, const Vec3& l, float& t);
+bool RayIntersectsQuad(const std::array<Vec3, 2>& ray, const Vec3& q1, const Vec3& q2, const Vec3& q3, const Vec3& q4, Vec3* out);
+bool RayIntersectsTriangle(const Vec3& ray_origin, const Vec3& ray_vector, Triangle* in_triangle, Vec3& out_intersection_point);
+
+} // namespace kp3d