FPS cam movement, dynamic subdivision, Navmeshes

.vscode/tasks.json

@@ -4,7 +4,7 @@
     {
       "label": "Build Debug",
       "type": "shell",
-      "command": "make BUILD=Debug",
+      "command": "make -j12 BUILD=Debug",
       "group": {
         "kind": "build",
         "isDefault": true
@@ -16,7 +16,7 @@
     {
       "label": "Build Release",
       "type": "shell",
-      "command": "make",
+      "command": "make -j12",
       "group": {
         "kind": "build",
         "isDefault": true

Makefile

@@ -7,6 +7,7 @@ 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

@@ -10,19 +10,19 @@ psxsplash::Camera::Camera() {
     m_rotationMatrix = psyqo::SoftMath::generateRotationMatrix33(0, psyqo::SoftMath::Axis::X, m_trig);
 }
 
-void psxsplash::Camera::moveX(psyqo::FixedPoint<12> x) { m_position.x += -x; }
+void psxsplash::Camera::MoveX(psyqo::FixedPoint<12> x) { m_position.x += x; }
 
-void psxsplash::Camera::moveY(psyqo::FixedPoint<12> y) { m_position.y += -y; }
+void psxsplash::Camera::MoveY(psyqo::FixedPoint<12> y) { m_position.y += y; }
 
-void psxsplash::Camera::moveZ(psyqo::FixedPoint<12> z) { m_position.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) {
+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::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);
@@ -34,4 +34,4 @@ void psxsplash::Camera::setRotation(psyqo::Angle x, psyqo::Angle y, psyqo::Angle
     m_rotationMatrix = rotY;
 }
 
-psyqo::Matrix33& psxsplash::Camera::getRotation() { return m_rotationMatrix; }
+psyqo::Matrix33& psxsplash::Camera::GetRotation() { return m_rotationMatrix; }

src/camera.hh

@@ -10,15 +10,15 @@ class Camera {
   public:
     Camera();
 
-    void moveX(psyqo::FixedPoint<12> x);
+    void MoveX(psyqo::FixedPoint<12> x);
-    void moveY(psyqo::FixedPoint<12> y);
+    void MoveY(psyqo::FixedPoint<12> y);
-    void moveZ(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_position; }
 
-    void setRotation(psyqo::Angle x, psyqo::Angle y, psyqo::Angle z);
+    void SetRotation(psyqo::Angle x, psyqo::Angle y, psyqo::Angle z);
-    psyqo::Matrix33& getRotation();
+    psyqo::Matrix33& GetRotation();
 
   private:
     psyqo::Matrix33 m_rotationMatrix;

src/gtemath.cpp

@@ -5,7 +5,7 @@
 
 using namespace psyqo::GTE;
 
-void psxsplash::matrixMultiplyGTE(const psyqo::Matrix33 &matA, const psyqo::Matrix33 &matB, psyqo::Matrix33 *result) {
+void psxsplash::MatrixMultiplyGTE(const psyqo::Matrix33 &matA, const psyqo::Matrix33 &matB, psyqo::Matrix33 *result) {
     writeSafe<PseudoRegister::Rotation>(matA);
 
     psyqo::Vec3 t;

src/gtemath.hh

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

src/main.cpp

@@ -10,8 +10,10 @@
 #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"
 
@@ -30,6 +32,9 @@ class PSXSplash final : public psyqo::Application {
   public:
     psyqo::Font<> m_font;
     psyqo::AdvancedPad m_input;
+    psxsplash::SplashPackLoader m_loader;
+    static constexpr uint8_t m_stickDeadzone = 0x30;
+
 };
 
 class MainScene final : public psyqo::Scene {
@@ -39,12 +44,14 @@ 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 = 0;
+    static constexpr psyqo::FixedPoint<12> sprintSpeed = 0.003_fp;
+
 };
 
 PSXSplash psxSplash;
@@ -61,7 +68,7 @@ void PSXSplash::prepare() {
     gpu().initialize(config);
 
     // Initialize the Renderer singleton
-    psxsplash::Renderer::init(gpu());
+    psxsplash::Renderer::Init(gpu());
 }
 
 void PSXSplash::createScene() {
@@ -71,10 +78,12 @@ void PSXSplash::createScene() {
 }
 
 void MainScene::start(StartReason reason) {
-    m_objects = psxsplash::LoadSplashpack(_binary_output_bin_start);
+    psxSplash.m_loader.LoadSplashpack(_binary_output_bin_start);
-    psxsplash::Renderer::getInstance().setCamera(m_mainCamera);
+    psxsplash::Renderer::GetInstance().SetCamera(m_mainCamera);
 }
 
+psyqo::FixedPoint<12> pheight = 0.0_fp;
+
 void MainScene::frame() {
     uint32_t beginFrame = gpu().now();
     auto currentFrameCounter = gpu().getFrameCount();
@@ -87,62 +96,64 @@ void MainScene::frame() {
 
     mainScene.m_lastFrameCounter = currentFrameCounter;
     
-    auto& input = psxSplash.m_input;
+    uint8_t rightX = psxSplash.m_input.getAdc(psyqo::AdvancedPad::Pad::Pad1a, 0);
+    uint8_t rightY = psxSplash.m_input.getAdc(psyqo::AdvancedPad::Pad::Pad1a, 1);
 
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Right)) {
+    uint8_t leftX = psxSplash.m_input.getAdc(psyqo::AdvancedPad::Pad::Pad1a, 2);
-        m_mainCamera.moveX((m_trig.cos(camRotY) * moveSpeed * deltaTime));
+    uint8_t leftY = psxSplash.m_input.getAdc(psyqo::AdvancedPad::Pad::Pad1a, 3);
-        m_mainCamera.moveZ(-(m_trig.sin(camRotY) * moveSpeed * deltaTime));
+
+    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) < psxSplash.m_stickDeadzone && 
+       __builtin_abs(leftYOffset) < psxSplash.m_stickDeadzone) {
+        m_sprinting = false;
     }
 
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Left)) {
+    if(psxSplash.m_input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Button::L3)) {
-        m_mainCamera.moveX(-(m_trig.cos(camRotY) * moveSpeed * deltaTime));
+        m_sprinting = true;
-        m_mainCamera.moveZ((m_trig.sin(camRotY) * moveSpeed * deltaTime));
     }
 
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Up)) {
+    psyqo::FixedPoint<12> speed = m_sprinting ? sprintSpeed : moveSpeed;
-        m_mainCamera.moveX((m_trig.sin(camRotY) * m_trig.cos(camRotX)) * moveSpeed * deltaTime);
+
-        m_mainCamera.moveY(-(m_trig.sin(camRotX) * moveSpeed));
+    if (__builtin_abs(rightXOffset) > psxSplash.m_stickDeadzone) {
-        m_mainCamera.moveZ((m_trig.cos(camRotY) * m_trig.cos(camRotX)) * moveSpeed * deltaTime);
+        camRotY += (rightXOffset * rotSpeed * deltaTime) >> 7;  
+    }
+    if (__builtin_abs(rightYOffset) > psxSplash.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) > psxSplash.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) > psxSplash.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)) {
+    if(psxSplash.m_input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Button::L1)) {
-        m_mainCamera.moveX(-((m_trig.sin(camRotY) * m_trig.cos(camRotX)) * moveSpeed * deltaTime));
+        pheight += 0.01_fp;
-        m_mainCamera.moveY((m_trig.sin(camRotX) * moveSpeed * deltaTime));
+    }
-        m_mainCamera.moveZ(-((m_trig.cos(camRotY) * m_trig.cos(camRotX)) * moveSpeed * deltaTime));
+    if(psxSplash.m_input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Button::R1)) {
+        pheight -= 0.01_fp;
     }
 
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::R1)) {
+    psyqo::Vec3 adjustedPosition =
-        m_mainCamera.moveY(-moveSpeed * deltaTime);
+        psxsplash::ComputeNavmeshPosition(m_mainCamera.GetPosition(), *psxSplash.m_loader.navmeshes[0], -0.05_fp);
-    }
+    m_mainCamera.SetPosition(adjustedPosition.x, adjustedPosition.y, adjustedPosition.z);
-
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::L1)) {
-        m_mainCamera.moveY(moveSpeed * deltaTime);
-    }
-
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Cross)) {
-        camRotX -= rotSpeed * deltaTime;
-        m_mainCamera.setRotation(camRotX, camRotY, camRotZ);
-    }
-
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Triangle)) {
-        camRotX += rotSpeed * deltaTime;
-        m_mainCamera.setRotation(camRotX, camRotY, camRotZ);
-    }
-
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Circle)) {
-        camRotY += rotSpeed * deltaTime;
-        m_mainCamera.setRotation(camRotX, camRotY, camRotZ);
-    }
-
-    if (input.isButtonPressed(psyqo::AdvancedPad::Pad::Pad1a, psyqo::AdvancedPad::Square)) {
-        camRotY -= rotSpeed * deltaTime;
-        m_mainCamera.setRotation(camRotX, camRotY, camRotZ);
-    }
-
-    psxsplash::Renderer::getInstance().render(m_objects);
 
+    psxsplash::Renderer::GetInstance().Render(psxSplash.m_loader.gameObjects);
+    // psxsplash::Renderer::getInstance().renderNavmeshPreview(*psxSplash.m_loader.navmeshes[0], true);
     psxSplash.m_font.chainprintf(gpu(), {{.x = 2, .y = 2}}, {{.r = 0xff, .g = 0xff, .b = 0xff}}, "FPS: %i",
                                  gpu().getRefreshRate() / deltaTime);
+
     gpu().pumpCallbacks();
     uint32_t endFrame = gpu().now();
     uint32_t spent = endFrame - beginFrame;

src/navmesh.cpp

@@ -0,0 +1,119 @@
+#include "navmesh.hh"
+
+#include <array>
+
+#include "psyqo/fixed-point.hh"
+#include "psyqo/vector.hh"
+
+using namespace psyqo::fixed_point_literals;
+
+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/gte-registers.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

@@ -14,7 +14,9 @@
 #include <psyqo/trigonometry.hh>
 #include <psyqo/vector.hh>
 
+#include "EASTL/array.h"
 #include "gtemath.hh"
+#include "splashpack.hh"
 
 using namespace psyqo::fixed_point_literals;
 using namespace psyqo::trig_literals;
@@ -22,7 +24,7 @@ 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");
 
     clear<Register::TRX, Safe>();
@@ -42,9 +44,10 @@ void psxsplash::Renderer::init(psyqo::GPU &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();
@@ -64,8 +67,8 @@ void psxsplash::Renderer::render(eastl::vector<GameObject *> &objects) {
         ::clear<Register::TRZ, Safe>();
 
         // Rotate the camera Translation vector by the camera rotation
-        writeSafe<PseudoRegister::Rotation>(m_currentCamera->getRotation());
+        writeSafe<PseudoRegister::Rotation>(m_currentCamera->GetRotation());
-        writeSafe<PseudoRegister::V0>(m_currentCamera->getPosition());
+        writeSafe<PseudoRegister::V0>(-m_currentCamera->GetPosition());
 
         Kernels::mvmva<Kernels::MX::RT, Kernels::MV::V0, Kernels::TV::TR>();
         cameraPosition = readSafe<PseudoRegister::SV>();
@@ -80,7 +83,7 @@ void psxsplash::Renderer::render(eastl::vector<GameObject *> &objects) {
         objectPosition.z += cameraPosition.z;
 
         // Combine object and camera rotations
-        matrixMultiplyGTE(m_currentCamera->getRotation(), obj->rotation, &finalMatrix);
+        MatrixMultiplyGTE(m_currentCamera->GetRotation(), obj->rotation, &finalMatrix);
 
         psyqo::GTE::writeSafe<psyqo::GTE::PseudoRegister::Translation>(objectPosition);
         psyqo::GTE::writeSafe<psyqo::GTE::PseudoRegister::Rotation>(finalMatrix);
@@ -101,10 +104,19 @@ void psxsplash::Renderer::render(eastl::vector<GameObject *> &objects) {
             if (mac0 <= 0) continue;
 
             int32_t zIndex = 0;
-            uint32_t sz0, sz1, sz2;
+            uint32_t u0, u1, u2;
-            read<Register::SZ0>(&sz0);
+
-            read<Register::SZ1>(&sz1);
+            read<Register::SZ1>(&u0);
-            read<Register::SZ2>(&sz2);
+            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);
             if (zIndex < 0 || zIndex >= ORDERING_TABLE_SIZE) continue;
@@ -113,7 +125,7 @@ void psxsplash::Renderer::render(eastl::vector<GameObject *> &objects) {
             read<Register::SXY1>(&projected[1].packed);
             read<Register::SXY2>(&projected[2].packed);
 
-            iterativeSubdivideAndRender(tri, projected, zIndex, 3);
+            recursiveSubdivideAndRender(tri, projected, zIndex, 1);
         }
     }
     m_gpu.getNextClear(clear.primitive, m_clearcolor);
@@ -121,101 +133,236 @@ void psxsplash::Renderer::render(eastl::vector<GameObject *> &objects) {
     m_gpu.chain(ot);
 }
 
-static inline psyqo::Color averageColor(const psyqo::Color &c1, const psyqo::Color &c2) {
+void psxsplash::Renderer::RenderNavmeshPreview(psxsplash::Navmesh navmesh, bool isOnMesh) {
-    uint8_t r = (c1.r + c2.r) >> 1;
+    uint8_t parity = m_gpu.getParity();
-    uint8_t g = (c1.g + c2.g) >> 1;
+    eastl::array<psyqo::Vertex, 3> projected;
-    uint8_t b = (c1.b + c2.b) >> 1;
-    psyqo::Color c;
 
-    c.r = r;
+    auto &ot = m_ots[parity];
-    c.g = g;
+    auto &clear = m_clear[parity];
-    c.b = b;
+    auto &balloc = m_ballocs[parity];
+    balloc.reset();
 
-    return c;
+    psyqo::Vec3 cameraPosition;
-}
 
+    ::clear<Register::TRX, Safe>();
+    ::clear<Register::TRY, Safe>();
+    ::clear<Register::TRZ, Safe>();
 
-// Temporary subdivision code. I'm told this is terrible.
+    // Rotate the camera Translation vector by the camera rotation
-void psxsplash::Renderer::iterativeSubdivideAndRender(const Tri &initialTri,
+    writeSafe<PseudoRegister::Rotation>(m_currentCamera->GetRotation());
-                                                      const eastl::array<psyqo::Vertex, 3> &initialProj, int zIndex,
+    writeSafe<PseudoRegister::V0>(m_currentCamera->GetPosition());
-                                                      int maxIterations) {
-    struct Subdiv {
-        Tri tri;
-        eastl::array<psyqo::Vertex, 3> proj;
-        int iterations;
-    };
 
-    // Reserve space knowing the max subdivisions (for maxIterations=3, max elements are small)
+    Kernels::mvmva<Kernels::MX::RT, Kernels::MV::V0, Kernels::TV::TR>();
-    eastl::vector<Subdiv> stack;
+    cameraPosition = readSafe<PseudoRegister::SV>();
-    stack.reserve(16);
-    stack.push_back({initialTri, initialProj, maxIterations});
 
-    while (!stack.empty()) {
+    write<Register::TRX, Safe>(-cameraPosition.x.raw());
-        Subdiv s = stack.back();
+    write<Register::TRY, Safe>(-cameraPosition.y.raw());
-        stack.pop_back();
+    write<Register::TRZ, Safe>(-cameraPosition.z.raw());
 
-        uint16_t minX = eastl::min({s.proj[0].x, s.proj[1].x, s.proj[2].x});
+    psyqo::GTE::writeSafe<psyqo::GTE::PseudoRegister::Rotation>(m_currentCamera->GetRotation());
-        uint16_t maxX = eastl::max({s.proj[0].x, s.proj[1].x, s.proj[2].x});
-        uint16_t minY = eastl::min({s.proj[0].y, s.proj[1].y, s.proj[2].y});
-        uint16_t maxY = eastl::max({s.proj[0].y, s.proj[1].y, s.proj[2].y});
-        uint16_t width = maxX - minX;
-        uint16_t height = maxY - minY;
 
-        // Base case: small enough or no iterations left.
+    for (int i = 0; i < navmesh.triangleCount; i++) {
-        if (s.iterations == 0 || (width < 2048 && height < 1024)) {
+        NavMeshTri &tri = navmesh.polygons[i];
-            auto &balloc = m_ballocs[m_gpu.getParity()];
+        psyqo::Vec3 result;
-            auto &prim = balloc.allocateFragment<psyqo::Prim::GouraudTexturedTriangle>();
 
-            prim.primitive.pointA = s.proj[0];
+        writeSafe<PseudoRegister::V0>(tri.v0);
-            prim.primitive.pointB = s.proj[1];
+        writeSafe<PseudoRegister::V1>(tri.v1);
-            prim.primitive.pointC = s.proj[2];
+        writeSafe<PseudoRegister::V2>(tri.v2);
-            prim.primitive.uvA = s.tri.uvA;
+
-            prim.primitive.uvB = s.tri.uvB;
+        Kernels::rtpt();
-            prim.primitive.uvC = s.tri.uvC;
+        Kernels::nclip();
-            prim.primitive.tpage = s.tri.tpage;
+
-            psyqo::PrimPieces::ClutIndex clut(s.tri.clutX, s.tri.clutY);
+        int32_t mac0 = 0;
-            prim.primitive.clutIndex = clut;
+        read<Register::MAC0>(reinterpret_cast<uint32_t *>(&mac0));
-            prim.primitive.setColorA(s.tri.colorA);
+        if (mac0 <= 0) continue;
-            prim.primitive.setColorB(s.tri.colorB);
+
-            prim.primitive.setColorC(s.tri.colorC);
+        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;
+
+        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_ots[m_gpu.getParity()].insert(prim, zIndex);
-            continue;
-        }
-
-        // Compute midpoint between projected[0] and projected[1].
-        psyqo::Vertex mid;
-        mid.x = (s.proj[0].x + s.proj[1].x) >> 1;
-        mid.y = (s.proj[0].y + s.proj[1].y) >> 1;
-
-        // Interpolate UV and color.
-        psyqo::PrimPieces::UVCoords newUV;
-        newUV.u = (s.tri.uvA.u + s.tri.uvB.u) / 2;
-        newUV.v = (s.tri.uvA.v + s.tri.uvB.v) / 2;
-        psyqo::Color newColor = averageColor(s.tri.colorA, s.tri.colorB);
-
-        // Prepare new projected vertices.
-        eastl::array<psyqo::Vertex, 3> projA = {s.proj[0], mid, s.proj[2]};
-        eastl::array<psyqo::Vertex, 3> projB = {mid, s.proj[1], s.proj[2]};
-
-        // Construct new Tris
-        Tri triA = s.tri;
-        triA.uvB = newUV;
-        triA.colorB = newColor;
-
-        Tri triB = s.tri;
-        triB.uvA = newUV;
-        triB.colorA = newColor;
-
-        // Push new subdivisions on stack.
-        stack.push_back({triA, projA, s.iterations - 1});
-        stack.push_back({triB, projB, s.iterations - 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::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()];
+        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;  // uvC remains UVCoordsPadded.
+        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;
+    }
+
+    // Subdivide the triangle
+    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

@@ -16,6 +16,7 @@
 
 #include "camera.hh"
 #include "gameobject.hh"
+#include "splashpack.hh"
 
 namespace psxsplash {
 
@@ -25,18 +26,19 @@ class Renderer final {
     Renderer& operator=(const Renderer&) = delete;
 
     static constexpr size_t ORDERING_TABLE_SIZE = 2048 * 3;
-    static constexpr size_t BUMP_ALLOCATOR_SIZE = 8096 * 16;
+    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 iterativeSubdivideAndRender(const Tri& initialTri, const eastl::array<psyqo::Vertex, 3>& initialProj,
-                                     int zIndex, int maxIterations);
-    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;
     }
@@ -56,7 +58,10 @@ class Renderer final {
     psyqo::Fragments::SimpleFragment<psyqo::Prim::FastFill> m_clear[2];
     psyqo::BumpAllocator<BUMP_ALLOCATOR_SIZE> m_ballocs[2];
 
-    psyqo::Color m_clearcolor = {.r = 63, .g = 63, .b = 100};
+    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

@@ -10,13 +10,37 @@
 
 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;
+    uint16_t pad[2];
+};
+
+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;
     gameObjects.reserve(header->gameObjectCount);
+    navmeshes.reserve(header->navmeshCount);
 
     uint8_t *curentPointer = data + sizeof(psxsplash::SPLASHPACKFileHeader);
 
@@ -27,23 +51,28 @@ eastl::vector<psxsplash::GameObject *> LoadSplashpack(uint8_t *data) {
         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;
         uint16_t *castedData = reinterpret_cast<uint16_t *>(offsetData);
-        psxsplash::Renderer::getInstance().vramUpload(castedData, atlas->x, atlas->y, atlas->width, atlas->height);
+        psxsplash::Renderer::GetInstance().VramUpload(castedData, atlas->x, atlas->y, atlas->width, atlas->height);
         curentPointer += sizeof(psxsplash::SPLASHPACKTextureAtlas);
     }
 
     for (uint16_t i = 0; i < header->clutCount; i++) {
         psxsplash::SPLASHPACKClut *clut = reinterpret_cast<psxsplash::SPLASHPACKClut *>(curentPointer);
-        uint8_t* clutOffset = data + clut->clutOffset;
+        uint8_t *clutOffset = data + clut->clutOffset;
-        psxsplash::Renderer::getInstance().vramUpload((uint16_t*) clutOffset, clut->clutPackingX * 16, clut->clutPackingY, clut->length, 1);
+        psxsplash::Renderer::GetInstance().VramUpload((uint16_t *)clutOffset, clut->clutPackingX * 16,
+                                                      clut->clutPackingY, clut->length, 1);
         curentPointer += sizeof(psxsplash::SPLASHPACKClut);
     }
-
-    return gameObjects;
 }
 
 }  // namespace psxsplash

src/splashpack.hh

@@ -5,32 +5,15 @@
 #include <cstdint>
 
 #include "gameobject.hh"
+#include "navmesh.hh"
 
 namespace psxsplash {
 
-struct SPLASHPACKFileHeader {
+class SplashPackLoader {
-    char magic[2];
+  public:
-    uint16_t version;
+    eastl::vector<GameObject *> gameObjects;
-    uint16_t gameObjectCount;
+    eastl::vector<Navmesh *> navmeshes;
-    uint16_t textureAtlasCount;
+    void LoadSplashpack(uint8_t *data);
-    uint16_t clutCount;
-    uint16_t pad[3];
 };
 
-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;
-};
-
-eastl::vector<GameObject *> LoadSplashpack(uint8_t *data);
-
 };  // namespace psxsplash