First bits of code involving BSP traversal with front/back selection and math: determine the leaf that the camera is currently in.

common.h

@@ -8,12 +8,13 @@
 #include <psxgte.h>
 #include <psxgpu.h>
 
+#define INLINE  __attribute__((always_inline)) inline
+
 #include "memory.h"
 #include "qmath.h"
 
-#define INLINE  __attribute__((always_inline)) inline
-
 extern VECTOR cam_pos;
 extern SVECTOR cam_rot;
+extern u_short cam_leaf;
 
 #endif  // __COMMON_H__

main.c

@@ -11,6 +11,7 @@ extern u_long bsp_test[];
 VECTOR cam_pos = { 2176, 1152, 272 };	// START
 //VECTOR cam_pos = { 1920, -1408, 352 };	// E1M1
 SVECTOR cam_rot = { 0, 0, 0 };
+u_short cam_leaf = 0;
 
 world_t world;
 
@@ -46,13 +47,13 @@ int main(int argc, const char *argv[])
 
 		display_start();	// Works
 
-		FntPrint(-1, "Time: %d, ticks = %d, delta time = %d, fps = %d\n", (time_getRealTime() * 1000) >> 12, time_getFrameNumber(), time_getDeltaTime(), time_getFrameRate() >> 8);
-		FntPrint(-1, "Camera pos = (%d, %d, %d) rot = (%d, %d, %d)\n", cam_pos.vx, cam_pos.vy, cam_pos.vz, cam_rot.vx, cam_rot.vy, cam_rot.vz);
+		FntPrint(-1, "Time: %d, ticks: %d, delta time: %d, fps: %d\n", (time_getRealTime() * 1000) >> 12, time_getFrameNumber(), time_getDeltaTime(), time_getFrameRate() >> 8);
+		FntPrint(-1, "Camera pos: (%d, %d, %d) rot: (%d, %d, %d) leaf: %d\n", cam_pos.vx, cam_pos.vy, cam_pos.vz, cam_rot.vx, cam_rot.vy, cam_rot.vz, cam_leaf);
 
 		// Draw stuff
 		world_draw(&world);	// Doesn't work >:(
 	
-		FntPrint(-1, "Polygon count: %d\n", polyCount);
+		FntPrint(-1, "Polycount: %d\n", polyCount);
 		FntFlush(-1);
 		
 		display_finish();	// Works

qmath.h

@@ -3,4 +3,17 @@
 
 MATRIX *RotMatrixQ(SVECTOR *r, MATRIX *m);
 
+INLINE short m_dot12(const SVECTOR *a, const SVECTOR *b)
+{
+    return ((a->vx * b->vx) >> 12) + ((a->vy * b->vy) >> 12) + ((a->vz * b->vz) >> 12);
+}
+
+INLINE short m_pointPlaneDist4(const VECTOR *point4, const SVECTOR *normal12, short dist4)
+{
+    int x = ((int)point4->vx * normal12->vx) >> 12;
+    int y = ((int)point4->vy * normal12->vy) >> 12;
+    int z = ((int)point4->vz * normal12->vz) >> 12;
+    return (short)(x + y + z - dist4);
+}
+
 #endif // __QMATH_H__

world.c

@@ -270,21 +270,38 @@ static void world_drawnode(const world_t *world, short nodeIdx,  char *scratchpt
     const ps1bsp_node_t *node = &world->nodes[nodeIdx];
 
     // Still not sure why we have faces attached to nodes... Try to remove this and see what happens
-    ps1bsp_face_t *face = &world->faces[node->firstFace];
-    for (u_short faceIdx = 0; faceIdx < node->numFaces; ++faceIdx, ++face)
-    {
-        // Check if we've already drawn this face on the current frame
-        if (face->drawFrame == frameNum)
-            continue;
-
-        world_drawface(world, face, scratchptr);
-        face->drawFrame = frameNum;
-    }
+    // ps1bsp_face_t *face = &world->faces[node->firstFace];
+    // for (u_short faceIdx = 0; faceIdx < node->numFaces; ++faceIdx, ++face)
+    // {
+    //     // Check if we've already drawn this face on the current frame
+    //     if (face->drawFrame == frameNum)
+    //         continue;
+
+    //     world_drawface(world, face, scratchptr);
+    //     face->drawFrame = frameNum;
+    // }
     
     world_drawnode(world, node->front, scratchptr);
     world_drawnode(world, node->back, scratchptr);
 }
 
+static u_short world_leafAtPoint(const world_t *world, const VECTOR *point)
+{
+    short nodeIdx = 0;
+    while (nodeIdx >= 0)
+    {
+        const ps1bsp_node_t *node = &world->nodes[nodeIdx];
+        const ps1bsp_plane_t *plane = &world->planes[node->planeId];
+
+        // TODO: can be optimized for axis-aligned planes, no need for a dot product there
+        short dist = m_pointPlaneDist4(point, &plane->normal, plane->dist);
+
+        nodeIdx = dist < 0 ? node->back : node->front;  // TODO: this can be done branchless with (dist < 0)^1
+    }
+
+    return ~nodeIdx;
+}
+
 void world_draw(const world_t *world)
 {
     int p;
@@ -293,5 +310,7 @@ void world_draw(const world_t *world)
     gte_SetRotMatrix(&vp_matrix);
 	gte_SetTransMatrix(&vp_matrix);
 
+    cam_leaf = world_leafAtPoint(world, &cam_pos);
+
     world_drawnode(world, 0, scratchpad);
 }