From ec0eebce3c33a06bc3c440d33b36d494dfebf159 Mon Sep 17 00:00:00 2001
From: Nico de Poel <ndepoel@gmail.com>
Date: Sun, 29 Jan 2023 00:57:01 +0100
Subject: [PATCH] Fixed frustum calculation and implemented point and sphere
 tests. Now also tested and looking decent.

---
 display.c |  2 +-
 frustum.c | 35 ++++++++++++++++++++++++++---------
 frustum.h |  5 +++--
 3 files changed, 30 insertions(+), 12 deletions(-)

diff --git a/display.c b/display.c
index 307c4a3..09d51f4 100644
--- a/display.c
+++ b/display.c
@@ -180,7 +180,7 @@ void display_start()
 	CompMatrixLV(&proj_matrix, &view_matrix, &vp_matrix);
 
 	int screenheight = (GetVideoMode() == MODE_NTSC) ? SCREENHEIGHT_NTSC : SCREENHEIGHT_PAL;
-	frustum_update(&view_matrix, (SCREENWIDTH << 12) / aspect_scale.vx, (screenheight << 12) / aspect_scale.vy);
+	frustum_update((SCREENWIDTH << 12) / aspect_scale.vx, (screenheight << 12) / aspect_scale.vy);
 }
 
 void display_finish()
diff --git a/frustum.c b/frustum.c
index 74969d2..26b2e25 100644
--- a/frustum.c
+++ b/frustum.c
@@ -6,14 +6,14 @@
 
 static SVECTOR left, right, top, bottom;
 
-static INLINE void frustum_buildPlane(MATRIX *view_matrix, VECTOR *normal, SVECTOR *outPlane)
+static INLINE void frustum_buildPlane(MATRIX *rot_matrix, VECTOR *normal, SVECTOR *outPlane)
 {
-    ApplyMatrixLV(view_matrix, normal, normal);
+    ApplyMatrixLV(rot_matrix, normal, normal);
     VectorNormalS(normal, outPlane);
-    outPlane->pad = m_dot12(cam_pos, *outPlane);
+    outPlane->pad = -m_dot12(cam_pos, *outPlane);
 }
 
-void frustum_update(MATRIX *view_matrix, int width, int height)
+void frustum_update(int width, int height)
 {
     VECTOR l, r, t, b;
 
@@ -41,16 +41,33 @@ void frustum_update(MATRIX *view_matrix, int width, int height)
     t.vy = ONE;
     t.vz = -b.vz;
 
-    frustum_buildPlane(view_matrix, &l, &left);
-    frustum_buildPlane(view_matrix, &r, &right);
-    frustum_buildPlane(view_matrix, &t, &top);
-    frustum_buildPlane(view_matrix, &b, &bottom);
+    MATRIX rot_matrix;
+    RotMatrixQ(&cam_rot, &rot_matrix);
+
+    frustum_buildPlane(&rot_matrix, &l, &left);
+    frustum_buildPlane(&rot_matrix, &r, &right);
+    frustum_buildPlane(&rot_matrix, &t, &top);
+    frustum_buildPlane(&rot_matrix, &b, &bottom);
 
     // FntPrint(-1, "l = %d %d, r = %d %d, t = %d %d, b = %d %d\n", 
     //     left.vx, left.pad, right.vx, right.pad, top.vz, top.pad, bottom.vz, bottom.pad);
 }
 
-u_char frustum_checkSphere(SVECTOR *sphere)
+u_char frustum_pointInside(SVECTOR *point)
+{
+    if (m_dot12(left, *point) + left.pad < 0) return 0;
+    if (m_dot12(right, *point) + right.pad < 0) return 0;
+    if (m_dot12(top, *point) + top.pad < 0) return 0;
+    if (m_dot12(bottom, *point) + bottom.pad < 0) return 0;
+    return 1;
+}
+
+u_char frustum_sphereInside(SVECTOR *sphere)
 {
+    short radius = -sphere->pad;
+    if (m_dot12(left, *sphere) + left.pad < radius) return 0;
+    if (m_dot12(right, *sphere) + right.pad < radius) return 0;
+    if (m_dot12(top, *sphere) + top.pad < radius) return 0;
+    if (m_dot12(bottom, *sphere) + bottom.pad < radius) return 0;
     return 1;
 }
diff --git a/frustum.h b/frustum.h
index 99f507c..adcd50c 100644
--- a/frustum.h
+++ b/frustum.h
@@ -1,7 +1,8 @@
 #ifndef __FRUSTUM_H__
 #define __FRUSTUM_H__
 
-void frustum_update(MATRIX *view_matrix, int width, int height);
-u_char frustum_checkSphere(SVECTOR *sphere);
+void frustum_update(int width, int height);
+u_char frustum_pointInside(SVECTOR *point);
+u_char frustum_sphereInside(SVECTOR *sphere);
 
 #endif  // __FRUSTUM_H__