Merge pull request #3 from kiwanowski/main

removed unused cmath include

.gitignore

@@ -8,7 +8,13 @@ PSX.Dev-README.md
 *.a
 
 .cache/
-.vscode/
+.vscode/*
+!.vscode/launch.json
+!.vscode/tasks.json
 .editorconfig
 .clang-format
 compile_commands.json
+LICENSEE.DAT
+*.iso
+system.cnf
+iso.xml

.vscode/launch.json

@@ -0,0 +1,29 @@
+{
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "name": "Debug",
+      "type": "gdb",
+      "request": "attach",
+      "target": "localhost:3333",
+      "remote": true,
+      "cwd": "${workspaceRoot}",
+      "valuesFormatting": "parseText",
+      "stopAtConnect": true,
+      "gdbpath": "gdb-multiarch",
+      "windows": {
+        "gdbpath": "gdb-multiarch.exe"
+      },
+      "osx": {
+        "gdbpath": "gdb"
+      },
+      "executable": "${workspaceRoot}/${workspaceRootFolderName}.elf",
+      "autorun": [
+        "monitor reset shellhalt",
+        "load ${workspaceRootFolderName}.elf",
+        "tbreak main",
+        "continue"
+      ]
+    }
+  ]
+}

.vscode/tasks.json

@@ -0,0 +1,37 @@
+{
+  "version": "2.0.0",
+  "tasks": [
+    {
+      "label": "Build Debug",
+      "type": "shell",
+      "command": "make -j12 BUILD=Debug",
+      "group": {
+        "kind": "build",
+        "isDefault": true
+      },
+      "problemMatcher": [
+        "$gcc"
+      ]
+    },
+    {
+      "label": "Build Release",
+      "type": "shell",
+      "command": "make -j12",
+      "group": {
+        "kind": "build",
+        "isDefault": true
+      },
+      "problemMatcher": [
+        "$gcc"
+      ]
+    },
+    {
+      "label": "Clean",
+      "type": "shell",
+      "command": "make clean",
+      "group": {
+        "kind": "build"
+      }
+    }
+  ]
+}

Makefile

@@ -6,6 +6,8 @@ src/main.cpp \
 src/renderer.cpp \
 src/splashpack.cpp \
 src/camera.cpp \
+src/gtemath.cpp \
+src/navmesh.cpp \
 output.o
 
 include third_party/nugget/psyqo/psyqo.mk

src/camera.cpp

@@ -6,48 +6,32 @@
 #include <psyqo/trigonometry.hh>
 
 psxsplash::Camera::Camera() {
+    // Load identity
     m_rotationMatrix = psyqo::SoftMath::generateRotationMatrix33(0, psyqo::SoftMath::Axis::X, m_trig);
 }
 
-void psxsplash::Camera::moveX(psyqo::FixedPoint<12> x) { m_rotation.x += -x; }
+void psxsplash::Camera::MoveX(psyqo::FixedPoint<12> x) { m_position.x += x; }
 
-void psxsplash::Camera::moveY(psyqo::FixedPoint<12> y) { m_rotation.y += -y; }
+void psxsplash::Camera::MoveY(psyqo::FixedPoint<12> y) { m_position.y += y; }
 
-void psxsplash::Camera::moveZ(psyqo::FixedPoint<12> z) { m_rotation.z += -z; }
+void psxsplash::Camera::MoveZ(psyqo::FixedPoint<12> z) { m_position.z += z; }
 
-void psxsplash::Camera::setPosition(psyqo::FixedPoint<12> x, psyqo::FixedPoint<12> y, psyqo::FixedPoint<12> z) {
-    m_rotation.x = -x;
-    m_rotation.y = -y;
-    m_rotation.z = -z;
+void psxsplash::Camera::SetPosition(psyqo::FixedPoint<12> x, psyqo::FixedPoint<12> y, psyqo::FixedPoint<12> z) {
+    m_position.x = x;
+    m_position.y = y;
+    m_position.z = z;
 }
 
-void psxsplash::Camera::rotateX(psyqo::Angle x) {
-    auto rot = psyqo::SoftMath::generateRotationMatrix33(-x, psyqo::SoftMath::Axis::X, m_trig);
-
-    psyqo::SoftMath::multiplyMatrix33(m_rotationMatrix, rot, &m_rotationMatrix);
-}
-
-void psxsplash::Camera::rotateY(psyqo::Angle y) {
-    auto rot = psyqo::SoftMath::generateRotationMatrix33(-y, psyqo::SoftMath::Axis::Y, m_trig);
-
-    psyqo::SoftMath::multiplyMatrix33(m_rotationMatrix, rot, &m_rotationMatrix);
-}
-
-void psxsplash::Camera::rotateZ(psyqo::Angle z) {
-    auto rot = psyqo::SoftMath::generateRotationMatrix33(-z, psyqo::SoftMath::Axis::Y, m_trig);
-
-    psyqo::SoftMath::multiplyMatrix33(m_rotationMatrix, rot, &m_rotationMatrix);
-}
-
-void psxsplash::Camera::setRotation(psyqo::Angle x, psyqo::Angle y, psyqo::Angle z) {
+void psxsplash::Camera::SetRotation(psyqo::Angle x, psyqo::Angle y, psyqo::Angle z) {
     auto rotX = psyqo::SoftMath::generateRotationMatrix33(x, psyqo::SoftMath::Axis::X, m_trig);
     auto rotY = psyqo::SoftMath::generateRotationMatrix33(y, psyqo::SoftMath::Axis::Y, m_trig);
     auto rotZ = psyqo::SoftMath::generateRotationMatrix33(z, psyqo::SoftMath::Axis::Z, m_trig);
 
+    // XYZ multiplication order (matches C#)
     psyqo::SoftMath::multiplyMatrix33(rotY, rotX, &rotY);
-
     psyqo::SoftMath::multiplyMatrix33(rotY, rotZ, &rotY);
+    
     m_rotationMatrix = rotY;
 }
 
-psyqo::Matrix33& psxsplash::Camera::getRotation() { return m_rotationMatrix; }
+psyqo::Matrix33& psxsplash::Camera::GetRotation() { return m_rotationMatrix; }

src/camera.hh

@@ -1,35 +1,29 @@
 #pragma once
 
-#include <sys/types.h>
-
 #include <psyqo/fixed-point.hh>
 #include <psyqo/matrix.hh>
 #include <psyqo/trigonometry.hh>
 
 namespace psxsplash {
 
+// Camera class for managing 3D position and rotation.
 class Camera {
   public:
     Camera();
 
-    void moveX(psyqo::FixedPoint<12> x);
-    void moveY(psyqo::FixedPoint<12> y);
-    void moveZ(psyqo::FixedPoint<12> y);
+    void MoveX(psyqo::FixedPoint<12> x);
+    void MoveY(psyqo::FixedPoint<12> y);
+    void MoveZ(psyqo::FixedPoint<12> y);
 
-    void setPosition(psyqo::FixedPoint<12> x, psyqo::FixedPoint<12> y, psyqo::FixedPoint<12> z);
+    void SetPosition(psyqo::FixedPoint<12> x, psyqo::FixedPoint<12> y, psyqo::FixedPoint<12> z);
+    psyqo::Vec3& GetPosition() { return m_position; }
 
-    psyqo::Vec3& getPosition() { return m_rotation; }
-
-    void rotateX(psyqo::Angle x);
-    void rotateY(psyqo::Angle y);
-    void rotateZ(psyqo::Angle z);
-
-    void setRotation(psyqo::Angle x, psyqo::Angle y, psyqo::Angle z);
-    psyqo::Matrix33& getRotation();
+    void SetRotation(psyqo::Angle x, psyqo::Angle y, psyqo::Angle z);
+    psyqo::Matrix33& GetRotation();
 
   private:
     psyqo::Matrix33 m_rotationMatrix;
     psyqo::Trig<> m_trig;
-    psyqo::Vec3 m_rotation;
+    psyqo::Vec3 m_position;
 };
 }  // namespace psxsplash

src/gameobject.hh

@@ -10,15 +10,14 @@ namespace psxsplash {
 
 class GameObject final {
   public:
-    psyqo::Vec3 position;                  // 12 bytes
-    psyqo::Matrix33 rotation;              // 36 bytes
-    uint16_t polyCount;                    // 2 bytes
-    psyqo::PrimPieces::TPageAttr texture;  // 2 bytes
-    uint16_t clutX, clutY;
-    uint16_t clut[256];
-    union {  // 4 bytes
+    union {
         Tri *polygons;
         uint32_t polygonsOffset;
     };
+    psyqo::Vec3 position;
+    psyqo::Matrix33 rotation;
+    uint16_t polyCount;
+    uint16_t reserved;
 };
+static_assert(sizeof(GameObject) == 56, "GameObject is not 56 bytes");
 }  // namespace psxsplash

src/gtemath.cpp

@@ -0,0 +1,37 @@
+#include "gtemath.hh"
+
+#include <psyqo/gte-kernels.hh>
+#include <psyqo/gte-registers.hh>
+
+using namespace psyqo::GTE;
+
+void psxsplash::MatrixMultiplyGTE(const psyqo::Matrix33 &matA, const psyqo::Matrix33 &matB, psyqo::Matrix33 *result) {
+    writeSafe<PseudoRegister::Rotation>(matA);
+
+    psyqo::Vec3 t;
+
+    psyqo::GTE::writeSafe<PseudoRegister::V0>(psyqo::Vec3{matB.vs[0].x, matB.vs[1].x, matB.vs[2].x});
+
+    psyqo::GTE::Kernels::mvmva<Kernels::MX::RT, Kernels::MV::V0>();
+
+    t = psyqo::GTE::readSafe<psyqo::GTE::PseudoRegister::SV>();
+    result->vs[0].x = t.x;
+    result->vs[1].x = t.y;
+    result->vs[2].x = t.z;
+
+    psyqo::GTE::writeSafe<PseudoRegister::V0>(psyqo::Vec3{matB.vs[0].y, matB.vs[1].y, matB.vs[2].y});
+
+    psyqo::GTE::Kernels::mvmva<Kernels::MX::RT, Kernels::MV::V0>();
+    t = psyqo::GTE::readSafe<psyqo::GTE::PseudoRegister::SV>();
+    result->vs[0].y = t.x;
+    result->vs[1].y = t.y;
+    result->vs[2].y = t.z;
+
+    psyqo::GTE::writeSafe<PseudoRegister::V0>(psyqo::Vec3{matB.vs[0].z, matB.vs[1].z, matB.vs[2].z});
+
+    psyqo::GTE::Kernels::mvmva<Kernels::MX::RT, Kernels::MV::V0>();
+    t = psyqo::GTE::readSafe<psyqo::GTE::PseudoRegister::SV>();
+    result->vs[0].z = t.x;
+    result->vs[1].z = t.y;
+    result->vs[2].z = t.z;
+}

src/gtemath.hh

@@ -0,0 +1,6 @@
+#pragma once
+#include <psyqo/matrix.hh>
+
+namespace psxsplash {
+void MatrixMultiplyGTE(const psyqo::Matrix33 &matA, const psyqo::Matrix33 &matB, psyqo::Matrix33 *result);
+};

src/main.cpp

@@ -1,7 +1,5 @@
 #include <stdint.h>
 
-#include <cmath>
-#include <cstdint>
 #include <psyqo/advancedpad.hh>
 #include <psyqo/application.hh>
 #include <psyqo/fixed-point.hh>
@@ -10,11 +8,14 @@
 #include <psyqo/scene.hh>
 #include <psyqo/trigonometry.hh>
 
+#include "EASTL/algorithm.h"
 #include "camera.hh"
-#include "gameobject.hh"
+#include "navmesh.hh"
+#include "psyqo/vector.hh"
 #include "renderer.hh"
 #include "splashpack.hh"
 
+// Data from the splashpack
 extern uint8_t _binary_output_bin_start[];
 
 namespace {
@@ -27,8 +28,12 @@ class PSXSplash final : public psyqo::Application {
     void createScene() override;
 
   public:
+    psxsplash::SplashPackLoader m_loader;
+
     psyqo::Font<> m_font;
+
     psyqo::AdvancedPad m_input;
+    static constexpr uint8_t m_stickDeadzone = 0x30;
 };
 
 class MainScene final : public psyqo::Scene {
@@ -38,15 +43,22 @@ class MainScene final : public psyqo::Scene {
     psxsplash::Camera m_mainCamera;
     psyqo::Angle camRotX, camRotY, camRotZ;
 
-    eastl::vector<psxsplash::GameObject*> m_objects;
     psyqo::Trig<> m_trig;
     uint32_t m_lastFrameCounter;
 
-    static constexpr psyqo::FixedPoint<12> moveSpeed = 0.01_fp;
+    static constexpr psyqo::FixedPoint<12> moveSpeed = 0.002_fp;
     static constexpr psyqo::Angle rotSpeed = 0.01_pi;
+
+    bool m_sprinting = false;
+    static constexpr psyqo::FixedPoint<12> sprintSpeed = 0.01_fp;
+
+    bool m_freecam = false;
+    psyqo::FixedPoint<12> pheight = 0.0_fp;
+
+    bool m_renderSelect = false;
 };
 
-PSXSplash psxSplash;
+PSXSplash app;
 MainScene mainScene;
 
 }  // namespace
@@ -58,7 +70,9 @@ void PSXSplash::prepare() {
         .set(psyqo::GPU::ColorMode::C15BITS)
         .set(psyqo::GPU::Interlace::PROGRESSIVE);
     gpu().initialize(config);
-    psxsplash::Renderer::init(gpu());
+
+    // Initialize the Renderer singleton
+    psxsplash::Renderer::Init(gpu());
 }
 
 void PSXSplash::createScene() {
@@ -68,78 +82,112 @@ void PSXSplash::createScene() {
 }
 
 void MainScene::start(StartReason reason) {
-    m_objects = psxsplash::LoadSplashpack(_binary_output_bin_start);
-    psxsplash::Renderer::getInstance().setCamera(m_mainCamera);
+    app.m_loader.LoadSplashpack(_binary_output_bin_start);
+    psxsplash::Renderer::GetInstance().SetCamera(m_mainCamera);
+
+    m_mainCamera.SetPosition(static_cast<psyqo::FixedPoint<12>>(app.m_loader.playerStartPos.x),
+                             static_cast<psyqo::FixedPoint<12>>(app.m_loader.playerStartPos.y),
+                             static_cast<psyqo::FixedPoint<12>>(app.m_loader.playerStartPos.z));
+
+    pheight = psyqo::FixedPoint<12>(app.m_loader.playerHeight);
+
+    app.m_input.setOnEvent(
+        eastl::function<void(psyqo::AdvancedPad::Event)>{[this](const psyqo::AdvancedPad::Event& event) {
+            if (event.pad != psyqo::AdvancedPad::Pad::Pad1a) return;
+            if (app.m_loader.navmeshes.empty()) return;
+            if (event.type == psyqo::AdvancedPad::Event::ButtonPressed) {
+                if (event.button == psyqo::AdvancedPad::Button::Triangle) {
+                    m_freecam = !m_freecam;
+                } else if (event.button == psyqo::AdvancedPad::Button::Square) {
+                    m_renderSelect = !m_renderSelect;
+                }
+            }
+        }});
+
+    if (app.m_loader.navmeshes.empty()) {
+        m_freecam = true;
+    }
 }
 
 void MainScene::frame() {
     uint32_t beginFrame = gpu().now();
     auto currentFrameCounter = gpu().getFrameCount();
-    auto frameDiff = currentFrameCounter - mainScene.m_lastFrameCounter;
-    if (frameDiff == 0) {
+    auto deltaTime = currentFrameCounter - mainScene.m_lastFrameCounter;
+
+    // Unlike the torus example, this DOES happen...
+    if (deltaTime == 0) {
         return;
     }
+
     mainScene.m_lastFrameCounter = currentFrameCounter;
 
-    auto& input = psxSplash.m_input;
+    uint8_t rightX = app.m_input.getAdc(psyqo::AdvancedPad::Pad::Pad1a, 0);
+    uint8_t rightY = app.m_input.getAdc(psyqo::AdvancedPad::Pad::Pad1a, 1);
 
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Right)) {
-        m_mainCamera.moveX((m_trig.cos(camRotY) * moveSpeed));
-        m_mainCamera.moveZ(-(m_trig.sin(camRotY) * moveSpeed));
+    uint8_t leftX = app.m_input.getAdc(psyqo::AdvancedPad::Pad::Pad1a, 2);
+    uint8_t leftY = app.m_input.getAdc(psyqo::AdvancedPad::Pad::Pad1a, 3);
+
+    int16_t rightXOffset = (int16_t)rightX - 0x80;
+    int16_t rightYOffset = (int16_t)rightY - 0x80;
+    int16_t leftXOffset = (int16_t)leftX - 0x80;
+    int16_t leftYOffset = (int16_t)leftY - 0x80;
+
+    if (__builtin_abs(leftXOffset) < app.m_stickDeadzone &&
+        __builtin_abs(leftYOffset) < app.m_stickDeadzone) {
+        m_sprinting = false;
     }
 
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Left)) {
-        m_mainCamera.moveX(-(m_trig.cos(camRotY) * moveSpeed));
-        m_mainCamera.moveZ((m_trig.sin(camRotY) * moveSpeed));
+    if (app.m_input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Button::L3)) {
+        m_sprinting = true;
     }
 
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Up)) {
-        m_mainCamera.moveX((m_trig.sin(camRotY) * m_trig.cos(camRotX)) * moveSpeed);
-        m_mainCamera.moveY(-(m_trig.sin(camRotX) * moveSpeed));
-        m_mainCamera.moveZ((m_trig.cos(camRotY) * m_trig.cos(camRotX)) * moveSpeed);
+    psyqo::FixedPoint<12> speed = m_sprinting ? sprintSpeed : moveSpeed;
+
+    if (__builtin_abs(rightXOffset) > app.m_stickDeadzone) {
+        camRotY += (rightXOffset * rotSpeed * deltaTime) >> 7;
+    }
+    if (__builtin_abs(rightYOffset) > app.m_stickDeadzone) {
+        camRotX -= (rightYOffset * rotSpeed * deltaTime) >> 7;
+        camRotX = eastl::clamp(camRotX, -0.5_pi, 0.5_pi);
+    }
+    m_mainCamera.SetRotation(camRotX, camRotY, camRotZ);
+
+    if (__builtin_abs(leftYOffset) > app.m_stickDeadzone) {
+        psyqo::FixedPoint<12> forward = -(leftYOffset * speed * deltaTime) >> 7;
+        m_mainCamera.MoveX((m_trig.sin(camRotY) * forward));
+        m_mainCamera.MoveZ((m_trig.cos(camRotY) * forward));
+    }
+    if (__builtin_abs(leftXOffset) > app.m_stickDeadzone) {
+        psyqo::FixedPoint<12> strafe = -(leftXOffset * speed * deltaTime) >> 7;
+        m_mainCamera.MoveX(-(m_trig.cos(camRotY) * strafe));
+        m_mainCamera.MoveZ((m_trig.sin(camRotY) * strafe));
     }
 
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Down)) {
-        m_mainCamera.moveX(-((m_trig.sin(camRotY) * m_trig.cos(camRotX)) * moveSpeed));
-        m_mainCamera.moveY((m_trig.sin(camRotX) * moveSpeed));
-        m_mainCamera.moveZ(-((m_trig.cos(camRotY) * m_trig.cos(camRotX)) * moveSpeed));
+    if (app.m_input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Button::L1)) {
+        m_mainCamera.MoveY(speed * deltaTime);
+    }
+    if (app.m_input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Button::R1)) {
+        m_mainCamera.MoveY(-speed * deltaTime);
     }
 
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::R1)) {
-        m_mainCamera.moveY(-moveSpeed);
+    if (!m_freecam) {
+        psyqo::Vec3 adjustedPosition =
+            psxsplash::ComputeNavmeshPosition(m_mainCamera.GetPosition(), *app.m_loader.navmeshes[0], -pheight);
+        m_mainCamera.SetPosition(adjustedPosition.x, adjustedPosition.y, adjustedPosition.z);
     }
 
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::L1)) {
-        m_mainCamera.moveY(moveSpeed);
+    if (!m_renderSelect) {
+        psxsplash::Renderer::GetInstance().Render(app.m_loader.gameObjects);
+    } else {
+        psxsplash::Renderer::GetInstance().RenderNavmeshPreview(*app.m_loader.navmeshes[0], true);
     }
     
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Cross)) {
-        camRotX -= rotSpeed;
-        m_mainCamera.setRotation(camRotX, camRotY, camRotZ);
-    }
+    app.m_font.chainprintf(gpu(), {{.x = 2, .y = 2}}, {{.r = 0xff, .g = 0xff, .b = 0xff}}, "FPS: %i",
+                                 gpu().getRefreshRate() / deltaTime);
 
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Triangle)) {
-        camRotX += rotSpeed;
-        m_mainCamera.setRotation(camRotX, camRotY, camRotZ);
-    }
-
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Circle)) {
-        camRotY += rotSpeed;
-        m_mainCamera.setRotation(camRotX, camRotY, camRotZ);
-    }
-
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Square)) {
-        camRotY -= rotSpeed;
-        m_mainCamera.setRotation(camRotX, camRotY, camRotZ);
-    }
-
-    psxsplash::Renderer::getInstance().render(m_objects);
-
-    psxSplash.m_font.chainprintf(gpu(), {{.x = 2, .y = 2}}, {{.r = 0xff, .g = 0xff, .b = 0xff}}, "FPS: %i",
-                                 gpu().getRefreshRate() / frameDiff);
     gpu().pumpCallbacks();
     uint32_t endFrame = gpu().now();
     uint32_t spent = endFrame - beginFrame;
 }
 
-int main() { return psxSplash.run(); }
+int main() { return app.run(); }

src/mesh.hh

@@ -1,17 +1,25 @@
 #pragma once
 
-#include "psyqo/gte-registers.hh"
-#include "psyqo/primitives/common.hh"
+#include <psyqo/gte-registers.hh>
+#include <psyqo/primitives/common.hh>
 
 namespace psxsplash {
 
-class Tri final {
-  public:
-    psyqo::GTE::PackedVec3 v0, v1, v2;
-    psyqo::GTE::PackedVec3 normal;
-    psyqo::PrimPieces::UVCoords uvA, uvB;
-    psyqo::PrimPieces::UVCoordsPadded uvC;
-    psyqo::Color colorA, colorB, colorC;
-};
+  class Tri final {
+    public:
+      psyqo::GTE::PackedVec3 v0, v1, v2;  
+      psyqo::GTE::PackedVec3 normal;      
+  
+      psyqo::Color colorA, colorB, colorC; 
+  
+      psyqo::PrimPieces::UVCoords uvA, uvB; 
+      psyqo::PrimPieces::UVCoordsPadded uvC; 
+  
+      psyqo::PrimPieces::TPageAttr tpage; 
+      uint16_t clutX;
+      uint16_t clutY;
+      uint16_t padding; 
+  };
+  static_assert(sizeof(Tri) == 52, "Tri is not 52 bytes");
   
 } // namespace psxsplash

src/navmesh.cpp

@@ -0,0 +1,122 @@
+#include "navmesh.hh"
+
+#include <array>
+
+#include "psyqo/fixed-point.hh"
+#include "psyqo/vector.hh"
+
+using namespace psyqo::fixed_point_literals;
+
+// FIXME: This entire file uses hard FixedPoint scaling of 100. This is not ideal.
+// It would be better to move the fixedpoint precision to 19 instead.
+
+namespace psxsplash {
+
+psyqo::FixedPoint<12> DotProduct2D(const psyqo::Vec2& a, const psyqo::Vec2& b) { return a.x * b.x + a.y * b.y; }
+
+psyqo::Vec2 ClosestPointOnSegment(const psyqo::Vec2& A, const psyqo::Vec2& B, const psyqo::Vec2& P) {
+    psyqo::Vec2 AB = {B.x - A.x, B.y - A.y};
+    psyqo::Vec2 AP = {P.x - A.x, P.y - A.y};
+    auto abDot = DotProduct2D(AB, AB);
+    if (abDot == 0) return A;
+    psyqo::FixedPoint<12> t = DotProduct2D(AP, AB) / abDot;
+    if (t < 0.0_fp) t = 0.0_fp;
+    if (t > 1.0_fp) t = 1.0_fp;
+    return {(A.x + AB.x * t), (A.y + AB.y * t)};
+}
+
+bool PointInTriangle(psyqo::Vec3& p, NavMeshTri& tri) {
+    psyqo::Vec2 A = {tri.v0.x * 100, tri.v0.z * 100};
+    psyqo::Vec2 B = {tri.v1.x * 100, tri.v1.z * 100};
+    psyqo::Vec2 C = {tri.v2.x * 100, tri.v2.z * 100};
+    psyqo::Vec2 P = {p.x * 100, p.z * 100};
+
+    psyqo::Vec2 v0 = {B.x - A.x, B.y - A.y};
+    psyqo::Vec2 v1 = {C.x - A.x, C.y - A.y};
+    psyqo::Vec2 v2 = {P.x - A.x, P.y - A.y};
+
+    auto d00 = DotProduct2D(v0, v0);
+    auto d01 = DotProduct2D(v0, v1);
+    auto d11 = DotProduct2D(v1, v1);
+    auto d20 = DotProduct2D(v2, v0);
+    auto d21 = DotProduct2D(v2, v1);
+
+    psyqo::FixedPoint<12> denom = d00 * d11 - d01 * d01;
+    if (denom == 0.0_fp) {
+        return false;
+    }
+    auto invDenom = 1.0_fp / denom;
+    auto u = (d11 * d20 - d01 * d21) * invDenom;
+    auto w = (d00 * d21 - d01 * d20) * invDenom;
+
+    return (u >= 0.0_fp) && (w >= 0.0_fp) && (u + w <= 1.0_fp);
+}
+
+psyqo::Vec3 ComputeNormal(const NavMeshTri& tri) {
+    psyqo::Vec3 v1 = {tri.v1.x * 100 - tri.v0.x  * 100, tri.v1.y  * 100 - tri.v0.y  * 100, tri.v1.z  * 100 - tri.v0.z  * 100};
+    psyqo::Vec3 v2 = {tri.v2.x  * 100 - tri.v0.x  * 100, tri.v2.y  * 100 - tri.v0.y  * 100, tri.v2.z  * 100 - tri.v0.z  * 100};
+
+    psyqo::Vec3 normal = {
+        v1.y * v2.z - v1.z * v2.y,
+        v1.z * v2.x - v1.x * v2.z,
+        v1.x * v2.y - v1.y * v2.x
+    };
+    return normal;
+}
+
+psyqo::FixedPoint<12> CalculateY(const psyqo::Vec3& p, const NavMeshTri& tri) {
+    psyqo::Vec3 normal = ComputeNormal(tri);
+
+    psyqo::FixedPoint<12> A = normal.x;
+    psyqo::FixedPoint<12> B = normal.y;
+    psyqo::FixedPoint<12> C = normal.z;
+
+    psyqo::FixedPoint<12> D = -(A * tri.v0.x + B * tri.v0.y + C * tri.v0.z);
+
+    if (B != 0.0_fp) {
+        return -(A * p.x + C * p.z + D) / B;
+    } else {
+        return p.y;
+    }
+}
+
+psyqo::Vec3 ComputeNavmeshPosition(psyqo::Vec3& position, Navmesh& navmesh, psyqo::FixedPoint<12> pheight) {
+    for (int i = 0; i < navmesh.triangleCount; i++) {
+        if (PointInTriangle(position, navmesh.polygons[i])) {
+            position.y = CalculateY(position, navmesh.polygons[i]) + pheight;
+            return position;
+        }
+    }
+
+    psyqo::Vec2 P = {position.x * 100, position.z * 100};
+
+    psyqo::Vec2 closestPoint;
+    psyqo::FixedPoint<12> minDist = 0x7ffff;
+
+    for (int i = 0; i < navmesh.triangleCount; i++) {
+        NavMeshTri& tri = navmesh.polygons[i];
+        psyqo::Vec2 A = {tri.v0.x * 100, tri.v0.z * 100};
+        psyqo::Vec2 B = {tri.v1.x * 100, tri.v1.z * 100};
+        psyqo::Vec2 C = {tri.v2.x * 100, tri.v2.z * 100};
+
+        std::array<std::pair<psyqo::Vec2, psyqo::Vec2>, 3> edges = {{{A, B}, {B, C}, {C, A}}};
+
+        for (auto& edge : edges) {
+            psyqo::Vec2 proj = ClosestPointOnSegment(edge.first, edge.second, P);
+            psyqo::Vec2 diff = {proj.x - P.x, proj.y - P.y};
+            auto distSq = DotProduct2D(diff, diff);
+            if (distSq < minDist) {
+                minDist = distSq;
+                closestPoint = proj;
+                position.y = CalculateY(position, navmesh.polygons[i]) + pheight;
+            }
+        }
+    }
+
+    position.x = closestPoint.x / 100;
+    position.z = closestPoint.y / 100;
+
+    return position;
+}
+
+}  // namespace psxsplash

src/navmesh.hh

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <psyqo/vector.hh>
+
+namespace psxsplash {
+
+class NavMeshTri final {
+  public:
+    psyqo::Vec3 v0, v1, v2;
+};
+
+class Navmesh final {
+  public:
+    union {
+        NavMeshTri* polygons;
+        uint32_t polygonsOffset;
+    };
+    uint16_t triangleCount;
+    uint16_t reserved;
+};
+
+psyqo::Vec3 ComputeNavmeshPosition(psyqo::Vec3& position, Navmesh& navmesh, psyqo::FixedPoint<12> pheight);
+
+}  // namespace psxsplash

src/renderer.cpp

@@ -1,44 +1,50 @@
 #include "renderer.hh"
 
+#include <EASTL/array.h>
 #include <EASTL/vector.h>
 
-#include <cstdint>
 #include <psyqo/fixed-point.hh>
 #include <psyqo/gte-kernels.hh>
 #include <psyqo/gte-registers.hh>
 #include <psyqo/kernel.hh>
+#include <psyqo/matrix.hh>
 #include <psyqo/primitives/common.hh>
 #include <psyqo/primitives/triangles.hh>
+#include <psyqo/soft-math.hh>
+#include <psyqo/trigonometry.hh>
 #include <psyqo/vector.hh>
 
+#include "gtemath.hh"
+
 using namespace psyqo::fixed_point_literals;
 using namespace psyqo::trig_literals;
+using namespace psyqo::GTE;
 
 psxsplash::Renderer *psxsplash::Renderer::instance = nullptr;
 
-void psxsplash::Renderer::init(psyqo::GPU &gpuInstance) {
+void psxsplash::Renderer::Init(psyqo::GPU &gpuInstance) {
     psyqo::Kernel::assert(instance == nullptr, "A second intialization of Renderer was tried");
 
-    psyqo::GTE::clear<psyqo::GTE::Register::TRX, psyqo::GTE::Unsafe>();
-    psyqo::GTE::clear<psyqo::GTE::Register::TRY, psyqo::GTE::Unsafe>();
-    psyqo::GTE::clear<psyqo::GTE::Register::TRZ, psyqo::GTE::Unsafe>();
+    clear<Register::TRX, Safe>();
+    clear<Register::TRY, Safe>();
+    clear<Register::TRZ, Safe>();
 
-    psyqo::GTE::write<psyqo::GTE::Register::OFX, psyqo::GTE::Unsafe>(psyqo::FixedPoint<16>(160.0).raw());
-    psyqo::GTE::write<psyqo::GTE::Register::OFY, psyqo::GTE::Unsafe>(psyqo::FixedPoint<16>(120.0).raw());
+    write<Register::OFX, Safe>(psyqo::FixedPoint<16>(160.0).raw());
+    write<Register::OFY, Safe>(psyqo::FixedPoint<16>(120.0).raw());
 
-    psyqo::GTE::write<psyqo::GTE::Register::H, psyqo::GTE::Unsafe>(120);
+    write<Register::H, Safe>(120);
 
-    psyqo::GTE::write<psyqo::GTE::Register::ZSF3, psyqo::GTE::Unsafe>(ORDERING_TABLE_SIZE / 3);
-    psyqo::GTE::write<psyqo::GTE::Register::ZSF4, psyqo::GTE::Unsafe>(ORDERING_TABLE_SIZE / 4);
+    write<Register::ZSF3, Safe>(ORDERING_TABLE_SIZE / 3);
+    write<Register::ZSF4, Safe>(ORDERING_TABLE_SIZE / 4);
 
     if (!instance) {
         instance = new Renderer(gpuInstance);
     }
 }
 
-void psxsplash::Renderer::setCamera(psxsplash::Camera &camera) { m_currentCamera = &camera; }
+void psxsplash::Renderer::SetCamera(psxsplash::Camera &camera) { m_currentCamera = &camera; }
 
-void psxsplash::Renderer::render(eastl::vector<GameObject *> &objects) {
+void psxsplash::Renderer::Render(eastl::vector<GameObject *> &objects) {
     psyqo::Kernel::assert(m_currentCamera != nullptr, "PSXSPLASH: Tried to render without an active camera");
 
     uint8_t parity = m_gpu.getParity();
@@ -47,100 +53,323 @@ void psxsplash::Renderer::render(eastl::vector<GameObject *> &objects) {
     auto &clear = m_clear[parity];
     auto &balloc = m_ballocs[parity];
 
-    eastl::array<psyqo::Vertex, 3> projected;
-
-    m_gpu.getNextClear(clear.primitive, m_clearcolor);
-    m_gpu.chain(clear);
-
     balloc.reset();
-
+    eastl::array<psyqo::Vertex, 3> projected;
     for (auto &obj : objects) {
+        psyqo::Vec3 cameraPosition, objectPosition;
+        psyqo::Matrix33 finalMatrix;
+
+        ::clear<Register::TRX, Safe>();
+        ::clear<Register::TRY, Safe>();
+        ::clear<Register::TRZ, Safe>();
+
+        // Rotate the camera Translation vector by the camera rotation
+        writeSafe<PseudoRegister::Rotation>(m_currentCamera->GetRotation());
+        writeSafe<PseudoRegister::V0>(-m_currentCamera->GetPosition());
+
+        Kernels::mvmva<Kernels::MX::RT, Kernels::MV::V0, Kernels::TV::TR>();
+        cameraPosition = readSafe<PseudoRegister::SV>();
+
+        // Rotate the object Translation vector by the camera rotation
+        writeSafe<PseudoRegister::V0>(obj->position);
+        Kernels::mvmva<Kernels::MX::RT, Kernels::MV::V0, Kernels::TV::TR>();
+        objectPosition = readSafe<PseudoRegister::SV>();
+
+        objectPosition.x += cameraPosition.x;
+        objectPosition.y += cameraPosition.y;
+        objectPosition.z += cameraPosition.z;
+
+        // Combine object and camera rotations
+        MatrixMultiplyGTE(m_currentCamera->GetRotation(), obj->rotation, &finalMatrix);
+
+        psyqo::GTE::writeSafe<psyqo::GTE::PseudoRegister::Translation>(objectPosition);
+        psyqo::GTE::writeSafe<psyqo::GTE::PseudoRegister::Rotation>(finalMatrix);
+
         for (int i = 0; i < obj->polyCount; i++) {
             Tri &tri = obj->polygons[i];
             psyqo::Vec3 result;
 
-            psyqo::GTE::writeUnsafe<psyqo::GTE::PseudoRegister::Rotation>(obj->rotation);
-            psyqo::GTE::write<psyqo::GTE::Register::TRX, psyqo::GTE::Unsafe>(obj->position.x.raw() +
-                                                                             m_currentCamera->getPosition().x.raw());
-            psyqo::GTE::write<psyqo::GTE::Register::TRY, psyqo::GTE::Unsafe>(obj->position.y.raw() +
-                                                                             m_currentCamera->getPosition().y.raw());
-            psyqo::GTE::write<psyqo::GTE::Register::TRZ, psyqo::GTE::Safe>(obj->position.z.raw() +
-                                                                           m_currentCamera->getPosition().z.raw());
+            writeSafe<PseudoRegister::V0>(tri.v0);
+            writeSafe<PseudoRegister::V1>(tri.v1);
+            writeSafe<PseudoRegister::V2>(tri.v2);
 
-            psyqo::GTE::writeSafe<psyqo::GTE::PseudoRegister::V0>(tri.v0);
-            psyqo::GTE::writeSafe<psyqo::GTE::PseudoRegister::V1>(tri.v1);
-            psyqo::GTE::writeSafe<psyqo::GTE::PseudoRegister::V2>(tri.v2);
-
-            psyqo::GTE::Kernels::mvmva<psyqo::GTE::Kernels::MX::RT, psyqo::GTE::Kernels::MV::V0,
-                                       psyqo::GTE::Kernels::TV::TR>();
-            result = psyqo::GTE::readSafe<psyqo::GTE::PseudoRegister::SV>();
-            psyqo::GTE::writeSafe<psyqo::GTE::PseudoRegister::V0>(result);
-
-            psyqo::GTE::Kernels::mvmva<psyqo::GTE::Kernels::MX::RT, psyqo::GTE::Kernels::MV::V1,
-                                       psyqo::GTE::Kernels::TV::TR>();
-            result = psyqo::GTE::readSafe<psyqo::GTE::PseudoRegister::SV>();
-            psyqo::GTE::writeSafe<psyqo::GTE::PseudoRegister::V1>(result);
-
-            psyqo::GTE::Kernels::mvmva<psyqo::GTE::Kernels::MX::RT, psyqo::GTE::Kernels::MV::V2,
-                                       psyqo::GTE::Kernels::TV::TR>();
-            result = psyqo::GTE::readSafe<psyqo::GTE::PseudoRegister::SV>();
-            psyqo::GTE::writeSafe<psyqo::GTE::PseudoRegister::V2>(result);
-
-            psyqo::GTE::writeUnsafe<psyqo::GTE::PseudoRegister::Rotation>(m_currentCamera->getRotation());
-            psyqo::GTE::clear<psyqo::GTE::Register::TRX, psyqo::GTE::Unsafe>();
-            psyqo::GTE::clear<psyqo::GTE::Register::TRY, psyqo::GTE::Unsafe>();
-            psyqo::GTE::clear<psyqo::GTE::Register::TRZ, psyqo::GTE::Unsafe>();
-
-            psyqo::GTE::Kernels::rtpt();
-            psyqo::GTE::Kernels::nclip();
+            Kernels::rtpt();
+            Kernels::nclip();
 
             int32_t mac0 = 0;
-            psyqo::GTE::read<psyqo::GTE::Register::MAC0>(reinterpret_cast<uint32_t *>(&mac0));
+            read<Register::MAC0>(reinterpret_cast<uint32_t *>(&mac0));
             if (mac0 <= 0) continue;
 
             int32_t zIndex = 0;
-            uint32_t sz0, sz1, sz2;
-            psyqo::GTE::read<psyqo::GTE::Register::SZ0>(&sz0);
-            psyqo::GTE::read<psyqo::GTE::Register::SZ1>(&sz1);
-            psyqo::GTE::read<psyqo::GTE::Register::SZ2>(&sz2);
+            uint32_t u0, u1, u2;
+
+            read<Register::SZ1>(&u0);
+            read<Register::SZ2>(&u1);
+            read<Register::SZ3>(&u2);
+
+            int32_t sz0 = (int32_t)u0;
+            int32_t sz1 = (int32_t)u1;
+            int32_t sz2 = (int32_t)u2;
+
+            if ((sz0 < 1 && sz1 < 1 && sz2 < 1)) {
+                continue;
+            };
 
             zIndex = eastl::max(eastl::max(sz0, sz1), sz2);
-
-            // psyqo::GTE::read<psyqo::GTE::Register::OTZ>(reinterpret_cast<uint32_t *>(&zIndex));
-
             if (zIndex < 0 || zIndex >= ORDERING_TABLE_SIZE) continue;
 
-            psyqo::GTE::read<psyqo::GTE::Register::SXY0>(&projected[0].packed);
-            psyqo::GTE::read<psyqo::GTE::Register::SXY1>(&projected[1].packed);
-            psyqo::GTE::read<psyqo::GTE::Register::SXY2>(&projected[2].packed);
+            read<Register::SXY0>(&projected[0].packed);
+            read<Register::SXY1>(&projected[1].packed);
+            read<Register::SXY2>(&projected[2].packed);
 
-            auto &prim = balloc.allocateFragment<psyqo::Prim::GouraudTexturedTriangle>();
-
-            psyqo::PrimPieces::ClutIndex clut(obj->clutX, obj->clutY);
-
-            prim.primitive.pointA = projected[0];
-            prim.primitive.pointB = projected[1];
-            prim.primitive.pointC = projected[2];
-            prim.primitive.uvA = tri.uvA;
-            prim.primitive.uvB = tri.uvB;
-            prim.primitive.uvC = tri.uvC;
-            prim.primitive.tpage = obj->texture;
-            prim.primitive.clutIndex = clut;
-            prim.primitive.setColorA(tri.colorA);
-            prim.primitive.setColorB(tri.colorB);
-            prim.primitive.setColorC(tri.colorC);
-
-            prim.primitive.setOpaque();
-
-            ot.insert(prim, zIndex);
+            recursiveSubdivideAndRender(tri, projected, zIndex, 1);
         }
     }
-
+    m_gpu.getNextClear(clear.primitive, m_clearcolor);
+    m_gpu.chain(clear);
     m_gpu.chain(ot);
 }
 
-void psxsplash::Renderer::vramUpload(const uint16_t *imageData, int16_t posX, int16_t posY, int16_t width,
+void psxsplash::Renderer::RenderNavmeshPreview(psxsplash::Navmesh navmesh, bool isOnMesh) {
+    uint8_t parity = m_gpu.getParity();
+    eastl::array<psyqo::Vertex, 3> projected;
+
+    auto &ot = m_ots[parity];
+    auto &clear = m_clear[parity];
+    auto &balloc = m_ballocs[parity];
+    balloc.reset();
+
+    psyqo::Vec3 cameraPosition;
+
+    ::clear<Register::TRX, Safe>();
+    ::clear<Register::TRY, Safe>();
+    ::clear<Register::TRZ, Safe>();
+
+    // Rotate the camera Translation vector by the camera rotation
+    writeSafe<PseudoRegister::Rotation>(m_currentCamera->GetRotation());
+    writeSafe<PseudoRegister::V0>(m_currentCamera->GetPosition());
+
+    Kernels::mvmva<Kernels::MX::RT, Kernels::MV::V0, Kernels::TV::TR>();
+    cameraPosition = readSafe<PseudoRegister::SV>();
+
+    write<Register::TRX, Safe>(-cameraPosition.x.raw());
+    write<Register::TRY, Safe>(-cameraPosition.y.raw());
+    write<Register::TRZ, Safe>(-cameraPosition.z.raw());
+
+    psyqo::GTE::writeSafe<psyqo::GTE::PseudoRegister::Rotation>(m_currentCamera->GetRotation());
+
+    for (int i = 0; i < navmesh.triangleCount; i++) {
+        NavMeshTri &tri = navmesh.polygons[i];
+        psyqo::Vec3 result;
+
+        writeSafe<PseudoRegister::V0>(tri.v0);
+        writeSafe<PseudoRegister::V1>(tri.v1);
+        writeSafe<PseudoRegister::V2>(tri.v2);
+
+        Kernels::rtpt();
+        Kernels::nclip();
+
+        int32_t mac0 = 0;
+        read<Register::MAC0>(reinterpret_cast<uint32_t *>(&mac0));
+        if (mac0 <= 0) continue;
+
+        int32_t zIndex = 0;
+        uint32_t u0, u1, u2;
+        read<Register::SZ0>(&u0);
+        read<Register::SZ1>(&u1);
+        read<Register::SZ2>(&u2);
+
+        int32_t sz0 = *reinterpret_cast<int32_t *>(&u0);
+        int32_t sz1 = *reinterpret_cast<int32_t *>(&u1);
+        int32_t sz2 = *reinterpret_cast<int32_t *>(&u2);
+
+        zIndex = eastl::max(eastl::max(sz0, sz1), sz2);
+        if (zIndex < 0 || zIndex >= ORDERING_TABLE_SIZE) continue;
+
+        read<Register::SXY0>(&projected[0].packed);
+        read<Register::SXY1>(&projected[1].packed);
+        read<Register::SXY2>(&projected[2].packed);
+
+        auto &prim = balloc.allocateFragment<psyqo::Prim::Triangle>();
+
+        prim.primitive.pointA = projected[0];
+        prim.primitive.pointB = projected[1];
+        prim.primitive.pointC = projected[2];
+
+        psyqo::Color heightColor;
+
+        if (isOnMesh) {
+            heightColor.r = 0;
+            heightColor.g = ((tri.v0.y.raw() + tri.v1.y.raw() + tri.v2.y.raw()) / 3) * 100 % 256;
+            heightColor.b = 0;
+        } else {
+            heightColor.r = ((tri.v0.y.raw() + tri.v1.y.raw() + tri.v2.y.raw()) / 3) * 100 % 256;
+            heightColor.g = 0;
+            heightColor.b = 0;
+        }
+
+        prim.primitive.setColor(heightColor);
+        prim.primitive.setOpaque();
+        ot.insert(prim, zIndex);
+    }
+    m_gpu.getNextClear(clear.primitive, m_clearcolor);
+    m_gpu.chain(clear);
+    m_gpu.chain(ot);
+}
+
+void psxsplash::Renderer::VramUpload(const uint16_t *imageData, int16_t posX, int16_t posY, int16_t width,
                                      int16_t height) {
     psyqo::Rect uploadRect{.a = {.x = posX, .y = posY}, .b = {width, height}};
     m_gpu.uploadToVRAM(imageData, uploadRect);
 }
+
+psyqo::Color averageColor(const psyqo::Color &a, const psyqo::Color &b) {
+    return psyqo::Color{static_cast<uint8_t>((a.r + b.r) >> 1), static_cast<uint8_t>((a.g + b.g) >> 1),
+                        static_cast<uint8_t>((a.b + b.b) >> 1)};
+}
+
+void psxsplash::Renderer::recursiveSubdivideAndRender(Tri &tri, eastl::array<psyqo::Vertex, 3> &projected, int zIndex,
+                                                      int maxIterations) {
+    uint16_t minX = eastl::min({projected[0].x, projected[1].x, projected[2].x});
+    uint16_t maxX = eastl::max({projected[0].x, projected[1].x, projected[2].x});
+    uint16_t minY = eastl::min({projected[0].y, projected[1].y, projected[2].y});
+    uint16_t maxY = eastl::max({projected[0].y, projected[1].y, projected[2].y});
+    uint16_t width = maxX - minX;
+    uint16_t height = maxY - minY;
+
+    bool leavingScreenSpace = false;
+    if (projected[0].x < -100 || projected[0].y < -100 || projected[1].x < -100 || projected[1].y < -100 ||
+        projected[2].x < -100 || projected[2].y < -100 || width > 420 || height > 356) {
+        leavingScreenSpace = true;
+    }
+
+    if (maxIterations == 0 || ((width < 512 && height < 256 && !leavingScreenSpace))) {
+        auto &balloc = m_ballocs[m_gpu.getParity()];
+
+        // The 20 is some headroom just in case
+        if (balloc.remaining() < sizeof(psyqo::Prim::GouraudTexturedTriangle) + 20) {
+            return;
+        }
+        auto &prim = balloc.allocateFragment<psyqo::Prim::GouraudTexturedTriangle>();
+
+        prim.primitive.pointA = projected[0];
+        prim.primitive.pointB = projected[1];
+        prim.primitive.pointC = projected[2];
+
+        prim.primitive.uvA = tri.uvA;
+        prim.primitive.uvB = tri.uvB;
+        prim.primitive.uvC = tri.uvC;
+        prim.primitive.tpage = tri.tpage;
+        psyqo::PrimPieces::ClutIndex clut(tri.clutX, tri.clutY);
+        prim.primitive.clutIndex = clut;
+        prim.primitive.setColorA(tri.colorA);
+        prim.primitive.setColorB(tri.colorB);
+        prim.primitive.setColorC(tri.colorC);
+        prim.primitive.setOpaque();
+
+        m_ots[m_gpu.getParity()].insert(prim, zIndex);
+        return;
+    }
+
+    // FIXME: This is slow. The optimal way to do this would be to export the triangles from unity such that
+    // the edge between v0 and v1 is always the longest edge. This way we can always split the triangle optimally.
+    auto distanceSq = [](const psyqo::Vertex &a, const psyqo::Vertex &b) -> uint32_t {
+        int dx = a.x - b.x;
+        int dy = a.y - b.y;
+        return dx * dx + dy * dy;
+    };
+
+    uint32_t d0 = distanceSq(projected[0], projected[1]);
+    uint32_t d1 = distanceSq(projected[1], projected[2]);
+    uint32_t d2 = distanceSq(projected[2], projected[0]);
+
+    int i, j, k;
+    if (d0 >= d1 && d0 >= d2) {
+        i = 0;
+        j = 1;
+        k = 2;
+    } else if (d1 >= d0 && d1 >= d2) {
+        i = 1;
+        j = 2;
+        k = 0;
+    } else {
+        i = 2;
+        j = 0;
+        k = 1;
+    }
+
+    auto getUVu = [&](int idx) -> uint8_t {
+        if (idx == 0) return tri.uvA.u;
+        if (idx == 1) return tri.uvB.u;
+        return tri.uvC.u;
+    };
+
+    auto getUVv = [&](int idx) -> uint8_t {
+        if (idx == 0) return tri.uvA.v;
+        if (idx == 1) return tri.uvB.v;
+        return tri.uvC.v;
+    };
+
+    auto getColor = [&](int idx) -> psyqo::Color {
+        if (idx == 0) return tri.colorA;
+        if (idx == 1) return tri.colorB;
+        return tri.colorC;
+    };
+
+    psyqo::Vertex mid;
+    mid.x = (projected[i].x + projected[j].x) >> 1;
+    mid.y = (projected[i].y + projected[j].y) >> 1;
+
+    uint8_t newU = (getUVu(i) + getUVu(j)) / 2;
+    uint8_t newV = (getUVv(i) + getUVv(j)) / 2;
+
+    psyqo::Color newColor = averageColor(getColor(i), getColor(j));
+
+    eastl::array<psyqo::Vertex, 3> projA, projB;
+    projA[0] = projected[i];
+    projA[1] = mid;
+    projA[2] = projected[k];
+
+    projB[0] = mid;
+    projB[1] = projected[j];
+    projB[2] = projected[k];
+
+    Tri triA, triB;
+
+    triA.uvA = {getUVu(i), getUVv(i)};
+    triA.uvB = {newU, newV};
+    triA.uvC = {getUVu(k), getUVv(k)};
+
+    triA.colorA = getColor(i);
+    triA.colorB = newColor;
+    triA.colorC = getColor(k);
+
+    /*triA.colorA = {.r = 255};
+    triA.colorB = {.r = 255};
+    triA.colorC = {.r = 255};*/
+
+    triA.tpage = tri.tpage;
+    triA.clutX = tri.clutX;
+    triA.clutY = tri.clutY;
+    triA.normal = tri.normal;
+
+    triB.uvA = {newU, newV};
+    triB.uvB = {getUVu(j), getUVv(j)};
+    triB.uvC = {getUVu(k), getUVv(k)};
+
+    triB.colorA = newColor;
+    triB.colorB = getColor(j);
+    triB.colorC = getColor(k);
+
+    /*triB.colorA = {.g = 255};
+    triB.colorB = {.g = 255};
+    triB.colorC = {.g = 255};*/
+
+    triB.tpage = tri.tpage;
+    triB.clutX = tri.clutX;
+    triB.clutY = tri.clutY;
+    triB.normal = tri.normal;
+
+    recursiveSubdivideAndRender(triA, projA, zIndex, maxIterations - 1);
+    recursiveSubdivideAndRender(triB, projB, zIndex, maxIterations - 1);
+}

src/renderer.hh

@@ -1,8 +1,8 @@
 #pragma once
 
+#include <EASTL/array.h>
 #include <EASTL/vector.h>
 
-#include <cstdint>
 #include <psyqo/bump-allocator.hh>
 #include <psyqo/fragments.hh>
 #include <psyqo/gpu.hh>
@@ -10,10 +10,12 @@
 #include <psyqo/ordering-table.hh>
 #include <psyqo/primitives/common.hh>
 #include <psyqo/primitives/misc.hh>
+#include <psyqo/primitives/triangles.hh>
 #include <psyqo/trigonometry.hh>
 
 #include "camera.hh"
 #include "gameobject.hh"
+#include "navmesh.hh"
 
 namespace psxsplash {
 
@@ -22,17 +24,21 @@ class Renderer final {
     Renderer(const Renderer&) = delete;
     Renderer& operator=(const Renderer&) = delete;
 
-    static constexpr size_t ORDERING_TABLE_SIZE = 4096 * 16;
-    static constexpr size_t BUMP_ALLOCATOR_SIZE = 8192;
+    // FIXME: I have no idea how to precompute the required sizes of these. It would be best to allocate them based on the scene
+    static constexpr size_t ORDERING_TABLE_SIZE = 2048 * 3;
+    static constexpr size_t BUMP_ALLOCATOR_SIZE = 8096 * 24;
 
-    static void init(psyqo::GPU& gpuInstance);
+    static void Init(psyqo::GPU& gpuInstance);
 
-    void setCamera(Camera& camera);
+    void SetCamera(Camera& camera);
 
-    void render(eastl::vector<GameObject*>& objects);
-    void vramUpload(const uint16_t* imageData, int16_t posX, int16_t posY, int16_t width, int16_t height);
     
-    static Renderer& getInstance() {
+    void Render(eastl::vector<GameObject*>& objects);
+    void RenderNavmeshPreview(psxsplash::Navmesh navmesh, bool isOnMesh);
+
+    void VramUpload(const uint16_t* imageData, int16_t posX, int16_t posY, int16_t width, int16_t height);
+
+    static Renderer& GetInstance() {
         psyqo::Kernel::assert(instance != nullptr, "Access to renderer was tried without prior initialization");
         return *instance;
     }
@@ -50,8 +56,12 @@ class Renderer final {
 
     psyqo::OrderingTable<ORDERING_TABLE_SIZE> m_ots[2];
     psyqo::Fragments::SimpleFragment<psyqo::Prim::FastFill> m_clear[2];
-    psyqo::Color m_clearcolor = {.r = 63, .g = 63, .b = 63};
     psyqo::BumpAllocator<BUMP_ALLOCATOR_SIZE> m_ballocs[2];
+
+    psyqo::Color m_clearcolor = {.r = 0, .g = 0, .b = 0};
+
+    void recursiveSubdivideAndRender(Tri &tri, eastl::array<psyqo::Vertex, 3> &projected, int zIndex,
+      int maxIterations);
 };
 
 }  // namespace psxsplash

src/splashpack.cpp

@@ -1,61 +1,86 @@
 #include "splashpack.hh"
 
-#include <cstdint>
 #include <cstring>
-#include <memory>
 #include <psyqo/primitives/common.hh>
 
 #include "gameobject.hh"
 #include "mesh.hh"
+#include "psyqo/fixed-point.hh"
+#include "psyqo/gte-registers.hh"
 #include "renderer.hh"
 
 namespace psxsplash {
 
-eastl::vector<psxsplash::GameObject *> LoadSplashpack(uint8_t *data) {
+struct SPLASHPACKFileHeader {
+    char magic[2];
+    uint16_t version;
+    uint16_t gameObjectCount;
+    uint16_t navmeshCount;
+    uint16_t textureAtlasCount;
+    uint16_t clutCount;
+    psyqo::GTE::PackedVec3 playerStartPos;
+    psyqo::GTE::PackedVec3 playerStartRot;
+    psyqo::FixedPoint<12, uint16_t> playerHeight;
+    uint16_t pad[1];
+};
+
+struct SPLASHPACKTextureAtlas {
+    uint32_t polygonsOffset;
+    uint16_t width, height;
+    uint16_t x, y;
+};
+
+struct SPLASHPACKClut {
+    uint32_t clutOffset;
+    uint16_t clutPackingX;
+    uint16_t clutPackingY;
+    uint16_t length;
+    uint16_t pad;
+};
+
+void SplashPackLoader::LoadSplashpack(uint8_t *data) {
     psyqo::Kernel::assert(data != nullptr, "Splashpack loading data pointer is null");
     psxsplash::SPLASHPACKFileHeader *header = reinterpret_cast<psxsplash::SPLASHPACKFileHeader *>(data);
     psyqo::Kernel::assert(memcmp(header->magic, "SP", 2) == 0, "Splashpack has incorrect magic");
 
-    eastl::vector<psxsplash::GameObject *> gameObjects;
+    playerStartPos = header->playerStartPos;
+    playerStartRot = header->playerStartRot;
+    playerHeight = header->playerHeight;
+
     gameObjects.reserve(header->gameObjectCount);
+    navmeshes.reserve(header->navmeshCount);
 
     uint8_t *curentPointer = data + sizeof(psxsplash::SPLASHPACKFileHeader);
 
     for (uint16_t i = 0; i < header->gameObjectCount; i++) {
         psxsplash::GameObject *go = reinterpret_cast<psxsplash::GameObject *>(curentPointer);
-
-        int16_t width = 0;
-        switch ((psyqo::Prim::TPageAttr::ColorMode)((std::bit_cast<short>(go->texture) & 0x0180) >> 7)) {
-            case psyqo::Prim::TPageAttr::ColorMode::Tex4Bits:
-                width = 16;
-                break;
-            case psyqo::Prim::TPageAttr::ColorMode::Tex8Bits:
-                width = 256;
-                break;
-            default:
-                width = -1;
-        }
-
-        if (width > 0) {
-            psxsplash::Renderer::getInstance().vramUpload(go->clut, go->clutX * 16, go->clutY, width, 1);
-        }
-
         go->polygons = reinterpret_cast<psxsplash::Tri *>(data + go->polygonsOffset);
-
         gameObjects.push_back(go);
         curentPointer += sizeof(psxsplash::GameObject);
     }
 
+    for (uint16_t i = 0; i < header->navmeshCount; i++) {
+        psxsplash::Navmesh *navmesh = reinterpret_cast<psxsplash::Navmesh *>(curentPointer);
+        navmesh->polygons = reinterpret_cast<psxsplash::NavMeshTri *>(data + navmesh->polygonsOffset);
+        navmeshes.push_back(navmesh);
+        curentPointer += sizeof(psxsplash::Navmesh);
+    }
+
     for (uint16_t i = 0; i < header->textureAtlasCount; i++) {
         psxsplash::SPLASHPACKTextureAtlas *atlas = reinterpret_cast<psxsplash::SPLASHPACKTextureAtlas *>(curentPointer);
-
-        uint8_t *offsetData = data + atlas->polygonsOffset;               // Ensure correct byte offset
-        uint16_t *castedData = reinterpret_cast<uint16_t *>(offsetData);  // Safe cast
-        psxsplash::Renderer::getInstance().vramUpload(castedData, atlas->x, atlas->y, atlas->width, atlas->height);
+        uint8_t *offsetData = data + atlas->polygonsOffset;
+        uint16_t *castedData = reinterpret_cast<uint16_t *>(offsetData);
+        psxsplash::Renderer::GetInstance().VramUpload(castedData, atlas->x, atlas->y, atlas->width, atlas->height);
         curentPointer += sizeof(psxsplash::SPLASHPACKTextureAtlas);
     }
 
-    return gameObjects;
+    for (uint16_t i = 0; i < header->clutCount; i++) {
+        psxsplash::SPLASHPACKClut *clut = reinterpret_cast<psxsplash::SPLASHPACKClut *>(curentPointer);
+        uint8_t *clutOffset = data + clut->clutOffset;
+        psxsplash::Renderer::GetInstance().VramUpload((uint16_t *)clutOffset, clut->clutPackingX * 16,
+                                                      clut->clutPackingY, clut->length, 1);
+        curentPointer += sizeof(psxsplash::SPLASHPACKClut);
+    }
 }
 
 }  // namespace psxsplash

src/splashpack.hh

@@ -2,25 +2,21 @@
 
 #include <EASTL/vector.h>
 
-#include <cstdint>
-
 #include "gameobject.hh"
+#include "navmesh.hh"
+#include "psyqo/fixed-point.hh"
 
 namespace psxsplash {
 
-struct SPLASHPACKFileHeader {
-    char magic[2];
-    uint16_t version;
-    uint16_t gameObjectCount;
-    uint16_t textureAtlasCount;
-};
+class SplashPackLoader {
+  public:
+    eastl::vector<GameObject *> gameObjects;
+    eastl::vector<Navmesh *> navmeshes;
     
-struct SPLASHPACKTextureAtlas {
-    uint32_t polygonsOffset;
-    uint16_t width, height;
-    uint16_t x, y;
-};
+    psyqo::GTE::PackedVec3 playerStartPos, playerStartRot;
+    psyqo::FixedPoint<12, uint16_t> playerHeight;
 
-eastl::vector<GameObject *> LoadSplashpack(uint8_t *data);
+    void LoadSplashpack(uint8_t *data);
+};
 
 };  // namespace psxsplash

src/texture.hh

@@ -1,13 +0,0 @@
-#pragma once
-
-#include <cstdint>
-#include <psyqo/primitives/common.hh>
-
-namespace psxsplash {
-class Texture final {
-  public:
-    psyqo::PrimPieces::TPageAttr m_tpage;
-    uint8_t m_width, m_height;
-};
-
-}  // namespace psxsplash