Optimize collision queries with spatial grid and improve movement CCD

include/rendering/camera_controller.hpp

@@ -94,6 +94,8 @@ private:
     static constexpr float PIVOT_HEIGHT = 1.8f;     // Pivot at head height
     static constexpr float CAM_SPHERE_RADIUS = 0.32f;  // Keep camera farther from geometry to avoid clipping-through surfaces
     static constexpr float CAM_EPSILON = 0.22f;        // Extra wall offset to avoid near-plane clipping artifacts
+    static constexpr float COLLISION_FOCUS_RADIUS_THIRD_PERSON = 90.0f;
+    static constexpr float COLLISION_FOCUS_RADIUS_FREE_FLY = 70.0f;
     static constexpr float MIN_PITCH = -88.0f;      // Look almost straight down
     static constexpr float MAX_PITCH = 35.0f;       // Limited upward look
     glm::vec3* followTarget = nullptr;

include/rendering/m2_renderer.hpp

@@ -5,6 +5,7 @@
 #include <glm/glm.hpp>
 #include <memory>
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 #include <string>
 #include <optional>
@@ -58,6 +59,8 @@ struct M2Instance {
     float scale;
     glm::mat4 modelMatrix;
     glm::mat4 invModelMatrix;
+    glm::vec3 worldBoundsMin;
+    glm::vec3 worldBoundsMax;
 
     // Animation state
     float animTime = 0.0f;       // Current animation time
@@ -163,6 +166,16 @@ public:
      */
     float raycastBoundingBoxes(const glm::vec3& origin, const glm::vec3& direction, float maxDistance) const;
 
+    /**
+     * Limit expensive collision/raycast queries to objects near a focus point.
+     */
+    void setCollisionFocus(const glm::vec3& worldPos, float radius);
+    void clearCollisionFocus();
+
+    void resetQueryStats();
+    double getQueryTimeMs() const { return queryTimeMs; }
+    uint32_t getQueryCallCount() const { return queryCallCount; }
+
     // Stats
     uint32_t getModelCount() const { return static_cast<uint32_t>(models.size()); }
     uint32_t getInstanceCount() const { return static_cast<uint32_t>(instances.size()); }
@@ -186,6 +199,42 @@ private:
     // Lighting uniforms
     glm::vec3 lightDir = glm::vec3(0.5f, 0.5f, 1.0f);
     glm::vec3 ambientColor = glm::vec3(0.4f, 0.4f, 0.45f);
+
+    // Optional query-space culling for collision/raycast hot paths.
+    bool collisionFocusEnabled = false;
+    glm::vec3 collisionFocusPos = glm::vec3(0.0f);
+    float collisionFocusRadius = 0.0f;
+    float collisionFocusRadiusSq = 0.0f;
+
+    struct GridCell {
+        int x;
+        int y;
+        int z;
+        bool operator==(const GridCell& other) const {
+            return x == other.x && y == other.y && z == other.z;
+        }
+    };
+    struct GridCellHash {
+        size_t operator()(const GridCell& c) const {
+            size_t h1 = std::hash<int>()(c.x);
+            size_t h2 = std::hash<int>()(c.y);
+            size_t h3 = std::hash<int>()(c.z);
+            return h1 ^ (h2 * 0x9e3779b9u) ^ (h3 * 0x85ebca6bu);
+        }
+    };
+    GridCell toCell(const glm::vec3& p) const;
+    void rebuildSpatialIndex();
+    void gatherCandidates(const glm::vec3& queryMin, const glm::vec3& queryMax, std::vector<size_t>& outIndices) const;
+
+    static constexpr float SPATIAL_CELL_SIZE = 64.0f;
+    std::unordered_map<GridCell, std::vector<uint32_t>, GridCellHash> spatialGrid;
+    std::unordered_map<uint32_t, size_t> instanceIndexById;
+    mutable std::vector<size_t> candidateScratch;
+    mutable std::unordered_set<uint32_t> candidateIdScratch;
+
+    // Collision query profiling (per frame).
+    mutable double queryTimeMs = 0.0;
+    mutable uint32_t queryCallCount = 0;
 };
 
 } // namespace rendering

include/rendering/renderer.hpp

@@ -121,6 +121,14 @@ public:
     void setTargetPosition(const glm::vec3* pos);
     bool isMoving() const;
 
+    // CPU timing stats (milliseconds, last frame).
+    double getLastUpdateMs() const { return lastUpdateMs; }
+    double getLastRenderMs() const { return lastRenderMs; }
+    double getLastCameraUpdateMs() const { return lastCameraUpdateMs; }
+    double getLastTerrainRenderMs() const { return lastTerrainRenderMs; }
+    double getLastWMORenderMs() const { return lastWMORenderMs; }
+    double getLastM2RenderMs() const { return lastM2RenderMs; }
+
 private:
     core::Window* window = nullptr;
     std::unique_ptr<Camera> camera;
@@ -177,6 +185,14 @@ private:
 
     bool terrainEnabled = true;
     bool terrainLoaded = false;
+
+    // CPU timing stats (last frame/update).
+    double lastUpdateMs = 0.0;
+    double lastRenderMs = 0.0;
+    double lastCameraUpdateMs = 0.0;
+    double lastTerrainRenderMs = 0.0;
+    double lastWMORenderMs = 0.0;
+    double lastM2RenderMs = 0.0;
 };
 
 } // namespace rendering

include/rendering/wmo_renderer.hpp

@@ -4,6 +4,7 @@
 #include <glm/glm.hpp>
 #include <memory>
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 #include <string>
 #include <optional>
@@ -158,6 +159,16 @@ public:
      */
     float raycastBoundingBoxes(const glm::vec3& origin, const glm::vec3& direction, float maxDistance) const;
 
+    /**
+     * Limit expensive collision/raycast queries to objects near a focus point.
+     */
+    void setCollisionFocus(const glm::vec3& worldPos, float radius);
+    void clearCollisionFocus();
+
+    void resetQueryStats();
+    double getQueryTimeMs() const { return queryTimeMs; }
+    uint32_t getQueryCallCount() const { return queryCallCount; }
+
 private:
     /**
      * WMO group GPU resources
@@ -222,6 +233,8 @@ private:
         float scale;
         glm::mat4 modelMatrix;
         glm::mat4 invModelMatrix;  // Cached inverse for collision
+        glm::vec3 worldBoundsMin;
+        glm::vec3 worldBoundsMax;
 
         void updateModelMatrix();
     };
@@ -249,6 +262,27 @@ private:
      */
     GLuint loadTexture(const std::string& path);
 
+    struct GridCell {
+        int x;
+        int y;
+        int z;
+        bool operator==(const GridCell& other) const {
+            return x == other.x && y == other.y && z == other.z;
+        }
+    };
+    struct GridCellHash {
+        size_t operator()(const GridCell& c) const {
+            size_t h1 = std::hash<int>()(c.x);
+            size_t h2 = std::hash<int>()(c.y);
+            size_t h3 = std::hash<int>()(c.z);
+            return h1 ^ (h2 * 0x9e3779b9u) ^ (h3 * 0x85ebca6bu);
+        }
+    };
+
+    GridCell toCell(const glm::vec3& p) const;
+    void rebuildSpatialIndex();
+    void gatherCandidates(const glm::vec3& queryMin, const glm::vec3& queryMax, std::vector<size_t>& outIndices) const;
+
     // Shader
     std::unique_ptr<Shader> shader;
 
@@ -272,6 +306,23 @@ private:
     bool wireframeMode = false;
     bool frustumCulling = true;
     uint32_t lastDrawCalls = 0;
+
+    // Optional query-space culling for collision/raycast hot paths.
+    bool collisionFocusEnabled = false;
+    glm::vec3 collisionFocusPos = glm::vec3(0.0f);
+    float collisionFocusRadius = 0.0f;
+    float collisionFocusRadiusSq = 0.0f;
+
+    // Uniform grid for fast local collision queries.
+    static constexpr float SPATIAL_CELL_SIZE = 64.0f;
+    std::unordered_map<GridCell, std::vector<uint32_t>, GridCellHash> spatialGrid;
+    std::unordered_map<uint32_t, size_t> instanceIndexById;
+    mutable std::vector<size_t> candidateScratch;
+    mutable std::unordered_set<uint32_t> candidateIdScratch;
+
+    // Collision query profiling (per frame).
+    mutable double queryTimeMs = 0.0;
+    mutable uint32_t queryCallCount = 0;
 };
 
 } // namespace rendering

src/rendering/camera_controller.cpp

@@ -165,6 +165,12 @@ void CameraController::update(float deltaTime) {
     if (thirdPerson && followTarget) {
         // Move the follow target (character position) instead of the camera
         glm::vec3 targetPos = *followTarget;
+        if (wmoRenderer) {
+            wmoRenderer->setCollisionFocus(targetPos, COLLISION_FOCUS_RADIUS_THIRD_PERSON);
+        }
+        if (m2Renderer) {
+            m2Renderer->setCollisionFocus(targetPos, COLLISION_FOCUS_RADIUS_THIRD_PERSON);
+        }
 
         // Check for water at current position
         std::optional<float> waterH;
@@ -227,8 +233,12 @@ void CameraController::update(float deltaTime) {
         {
             glm::vec3 startPos = *followTarget;
             glm::vec3 desiredPos = targetPos;
-            float moveDistXY = glm::length(glm::vec2(desiredPos.x - startPos.x, desiredPos.y - startPos.y));
+            float moveDist = glm::length(desiredPos - startPos);
-            int sweepSteps = std::max(1, std::min(6, static_cast<int>(std::ceil(moveDistXY / 0.4f))));
+            // Adaptive CCD: keep per-step movement short, especially on low FPS spikes.
+            int sweepSteps = std::max(1, std::min(24, static_cast<int>(std::ceil(moveDist / 0.18f))));
+            if (deltaTime > 0.04f) {
+                sweepSteps = std::min(28, std::max(sweepSteps, static_cast<int>(std::ceil(deltaTime / 0.016f)) * 3));
+            }
             glm::vec3 stepPos = startPos;
             glm::vec3 stepDelta = (desiredPos - startPos) / static_cast<float>(sweepSteps);
 
@@ -452,8 +462,8 @@ void CameraController::update(float deltaTime) {
 
         // Check floor collision along the camera path
         // Sample a few points to find where camera would go underground
-        for (int i = 1; i <= 4; i++) {
+        for (int i = 1; i <= 2; i++) {
-            float testDist = collisionDistance * (float(i) / 4.0f);
+            float testDist = collisionDistance * (float(i) / 2.0f);
             glm::vec3 testPos = pivot + camDir * testDist;
             auto floorH = getFloorAt(testPos.x, testPos.y, testPos.z);
 
@@ -490,8 +500,9 @@ void CameraController::update(float deltaTime) {
         constexpr float FLOOR_SAMPLE_R = 0.35f;
         std::optional<float> finalFloorH;
         const glm::vec2 floorOffsets[] = {
-            {0.0f, 0.0f}, {FLOOR_SAMPLE_R, 0.0f}, {-FLOOR_SAMPLE_R, 0.0f},
+            {0.0f, 0.0f},
-            {0.0f, FLOOR_SAMPLE_R}, {0.0f, -FLOOR_SAMPLE_R}
+            {FLOOR_SAMPLE_R * 0.7f, FLOOR_SAMPLE_R * 0.7f},
+            {-FLOOR_SAMPLE_R * 0.7f, -FLOOR_SAMPLE_R * 0.7f}
         };
         for (const auto& o : floorOffsets) {
             auto h = getFloorAt(smoothedCamPos.x + o.x, smoothedCamPos.y + o.y, smoothedCamPos.z);
@@ -517,6 +528,12 @@ void CameraController::update(float deltaTime) {
     } else {
         // Free-fly camera mode (original behavior)
         glm::vec3 newPos = camera->getPosition();
+        if (wmoRenderer) {
+            wmoRenderer->setCollisionFocus(newPos, COLLISION_FOCUS_RADIUS_FREE_FLY);
+        }
+        if (m2Renderer) {
+            m2Renderer->setCollisionFocus(newPos, COLLISION_FOCUS_RADIUS_FREE_FLY);
+        }
         float feetZ = newPos.z - eyeHeight;
 
         // Check for water at feet position
@@ -577,8 +594,11 @@ void CameraController::update(float deltaTime) {
         if (wmoRenderer) {
             glm::vec3 startFeet = camera->getPosition() - glm::vec3(0, 0, eyeHeight);
             glm::vec3 desiredFeet = newPos - glm::vec3(0, 0, eyeHeight);
-            float moveDistXY = glm::length(glm::vec2(desiredFeet.x - startFeet.x, desiredFeet.y - startFeet.y));
+            float moveDist = glm::length(desiredFeet - startFeet);
-            int sweepSteps = std::max(1, std::min(6, static_cast<int>(std::ceil(moveDistXY / 0.4f))));
+            int sweepSteps = std::max(1, std::min(24, static_cast<int>(std::ceil(moveDist / 0.18f))));
+            if (deltaTime > 0.04f) {
+                sweepSteps = std::min(28, std::max(sweepSteps, static_cast<int>(std::ceil(deltaTime / 0.016f)) * 3));
+            }
             glm::vec3 stepPos = startFeet;
             glm::vec3 stepDelta = (desiredFeet - startFeet) / static_cast<float>(sweepSteps);
 

src/rendering/m2_renderer.cpp

@@ -5,6 +5,7 @@
 #include "pipeline/asset_manager.hpp"
 #include "pipeline/blp_loader.hpp"
 #include "core/logger.hpp"
+#include <chrono>
 #include <glm/gtc/matrix_transform.hpp>
 #include <glm/gtc/type_ptr.hpp>
 #include <unordered_set>
@@ -59,6 +60,53 @@ bool segmentIntersectsAABB(const glm::vec3& from, const glm::vec3& to,
     return tExit >= 0.0f && tEnter <= 1.0f;
 }
 
+void transformAABB(const glm::mat4& modelMatrix,
+                   const glm::vec3& localMin,
+                   const glm::vec3& localMax,
+                   glm::vec3& outMin,
+                   glm::vec3& outMax) {
+    const glm::vec3 corners[8] = {
+        {localMin.x, localMin.y, localMin.z},
+        {localMin.x, localMin.y, localMax.z},
+        {localMin.x, localMax.y, localMin.z},
+        {localMin.x, localMax.y, localMax.z},
+        {localMax.x, localMin.y, localMin.z},
+        {localMax.x, localMin.y, localMax.z},
+        {localMax.x, localMax.y, localMin.z},
+        {localMax.x, localMax.y, localMax.z}
+    };
+
+    outMin = glm::vec3(std::numeric_limits<float>::max());
+    outMax = glm::vec3(-std::numeric_limits<float>::max());
+    for (const auto& c : corners) {
+        glm::vec3 wc = glm::vec3(modelMatrix * glm::vec4(c, 1.0f));
+        outMin = glm::min(outMin, wc);
+        outMax = glm::max(outMax, wc);
+    }
+}
+
+float pointAABBDistanceSq(const glm::vec3& p, const glm::vec3& bmin, const glm::vec3& bmax) {
+    glm::vec3 q = glm::clamp(p, bmin, bmax);
+    glm::vec3 d = p - q;
+    return glm::dot(d, d);
+}
+
+struct QueryTimer {
+    double* totalMs = nullptr;
+    uint32_t* callCount = nullptr;
+    std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();
+    QueryTimer(double* total, uint32_t* calls) : totalMs(total), callCount(calls) {}
+    ~QueryTimer() {
+        if (callCount) {
+            (*callCount)++;
+        }
+        if (totalMs) {
+            auto end = std::chrono::steady_clock::now();
+            *totalMs += std::chrono::duration<double, std::milli>(end - start).count();
+        }
+    }
+};
+
 } // namespace
 
 void M2Instance::updateModelMatrix() {
@@ -195,6 +243,8 @@ void M2Renderer::shutdown() {
     }
     models.clear();
     instances.clear();
+    spatialGrid.clear();
+    instanceIndexById.clear();
 
     // Delete cached textures
     for (auto& [path, texId] : textureCache) {
@@ -351,8 +401,22 @@ uint32_t M2Renderer::createInstance(uint32_t modelId, const glm::vec3& position,
     instance.rotation = rotation;
     instance.scale = scale;
     instance.updateModelMatrix();
+    glm::vec3 localMin, localMax;
+    getTightCollisionBounds(models[modelId], localMin, localMax);
+    transformAABB(instance.modelMatrix, localMin, localMax, instance.worldBoundsMin, instance.worldBoundsMax);
 
     instances.push_back(instance);
+    size_t idx = instances.size() - 1;
+    instanceIndexById[instance.id] = idx;
+    GridCell minCell = toCell(instance.worldBoundsMin);
+    GridCell maxCell = toCell(instance.worldBoundsMax);
+    for (int z = minCell.z; z <= maxCell.z; z++) {
+        for (int y = minCell.y; y <= maxCell.y; y++) {
+            for (int x = minCell.x; x <= maxCell.x; x++) {
+                spatialGrid[GridCell{x, y, z}].push_back(instance.id);
+            }
+        }
+    }
 
     return instance.id;
 }
@@ -372,9 +436,23 @@ uint32_t M2Renderer::createInstanceWithMatrix(uint32_t modelId, const glm::mat4&
     instance.scale = 1.0f;
     instance.modelMatrix = modelMatrix;
     instance.invModelMatrix = glm::inverse(modelMatrix);
+    glm::vec3 localMin, localMax;
+    getTightCollisionBounds(models[modelId], localMin, localMax);
+    transformAABB(instance.modelMatrix, localMin, localMax, instance.worldBoundsMin, instance.worldBoundsMax);
     instance.animTime = static_cast<float>(rand()) / RAND_MAX * 10.0f;  // Random start time
 
     instances.push_back(instance);
+    size_t idx = instances.size() - 1;
+    instanceIndexById[instance.id] = idx;
+    GridCell minCell = toCell(instance.worldBoundsMin);
+    GridCell maxCell = toCell(instance.worldBoundsMax);
+    for (int z = minCell.z; z <= maxCell.z; z++) {
+        for (int y = minCell.y; y <= maxCell.y; y++) {
+            for (int x = minCell.x; x <= maxCell.x; x++) {
+                spatialGrid[GridCell{x, y, z}].push_back(instance.id);
+            }
+        }
+    }
 
     return instance.id;
 }
@@ -496,6 +574,7 @@ void M2Renderer::removeInstance(uint32_t instanceId) {
     for (auto it = instances.begin(); it != instances.end(); ++it) {
         if (it->id == instanceId) {
             instances.erase(it);
+            rebuildSpatialIndex();
             return;
         }
     }
@@ -509,6 +588,86 @@ void M2Renderer::clear() {
     }
     models.clear();
     instances.clear();
+    spatialGrid.clear();
+    instanceIndexById.clear();
+}
+
+void M2Renderer::setCollisionFocus(const glm::vec3& worldPos, float radius) {
+    collisionFocusEnabled = (radius > 0.0f);
+    collisionFocusPos = worldPos;
+    collisionFocusRadius = std::max(0.0f, radius);
+    collisionFocusRadiusSq = collisionFocusRadius * collisionFocusRadius;
+}
+
+void M2Renderer::clearCollisionFocus() {
+    collisionFocusEnabled = false;
+}
+
+void M2Renderer::resetQueryStats() {
+    queryTimeMs = 0.0;
+    queryCallCount = 0;
+}
+
+M2Renderer::GridCell M2Renderer::toCell(const glm::vec3& p) const {
+    return GridCell{
+        static_cast<int>(std::floor(p.x / SPATIAL_CELL_SIZE)),
+        static_cast<int>(std::floor(p.y / SPATIAL_CELL_SIZE)),
+        static_cast<int>(std::floor(p.z / SPATIAL_CELL_SIZE))
+    };
+}
+
+void M2Renderer::rebuildSpatialIndex() {
+    spatialGrid.clear();
+    instanceIndexById.clear();
+    instanceIndexById.reserve(instances.size());
+
+    for (size_t i = 0; i < instances.size(); i++) {
+        const auto& inst = instances[i];
+        instanceIndexById[inst.id] = i;
+
+        GridCell minCell = toCell(inst.worldBoundsMin);
+        GridCell maxCell = toCell(inst.worldBoundsMax);
+        for (int z = minCell.z; z <= maxCell.z; z++) {
+            for (int y = minCell.y; y <= maxCell.y; y++) {
+                for (int x = minCell.x; x <= maxCell.x; x++) {
+                    spatialGrid[GridCell{x, y, z}].push_back(inst.id);
+                }
+            }
+        }
+    }
+}
+
+void M2Renderer::gatherCandidates(const glm::vec3& queryMin, const glm::vec3& queryMax,
+                                  std::vector<size_t>& outIndices) const {
+    outIndices.clear();
+    candidateIdScratch.clear();
+
+    GridCell minCell = toCell(queryMin);
+    GridCell maxCell = toCell(queryMax);
+    for (int z = minCell.z; z <= maxCell.z; z++) {
+        for (int y = minCell.y; y <= maxCell.y; y++) {
+            for (int x = minCell.x; x <= maxCell.x; x++) {
+                auto it = spatialGrid.find(GridCell{x, y, z});
+                if (it == spatialGrid.end()) continue;
+                for (uint32_t id : it->second) {
+                    if (!candidateIdScratch.insert(id).second) continue;
+                    auto idxIt = instanceIndexById.find(id);
+                    if (idxIt != instanceIndexById.end()) {
+                        outIndices.push_back(idxIt->second);
+                    }
+                }
+            }
+        }
+    }
+
+    // Safety fallback to preserve collision correctness if the spatial index
+    // misses candidates (e.g. during streaming churn).
+    if (outIndices.empty() && !instances.empty()) {
+        outIndices.reserve(instances.size());
+        for (size_t i = 0; i < instances.size(); i++) {
+            outIndices.push_back(i);
+        }
+    }
 }
 
 void M2Renderer::cleanupUnusedModels() {
@@ -591,9 +750,26 @@ uint32_t M2Renderer::getTotalTriangleCount() const {
 }
 
 std::optional<float> M2Renderer::getFloorHeight(float glX, float glY, float glZ) const {
+    QueryTimer timer(&queryTimeMs, &queryCallCount);
     std::optional<float> bestFloor;
 
-    for (const auto& instance : instances) {
+    glm::vec3 queryMin(glX - 2.0f, glY - 2.0f, glZ - 6.0f);
+    glm::vec3 queryMax(glX + 2.0f, glY + 2.0f, glZ + 8.0f);
+    gatherCandidates(queryMin, queryMax, candidateScratch);
+
+    for (size_t idx : candidateScratch) {
+        const auto& instance = instances[idx];
+        if (collisionFocusEnabled &&
+            pointAABBDistanceSq(collisionFocusPos, instance.worldBoundsMin, instance.worldBoundsMax) > collisionFocusRadiusSq) {
+            continue;
+        }
+
+        if (glX < instance.worldBoundsMin.x || glX > instance.worldBoundsMax.x ||
+            glY < instance.worldBoundsMin.y || glY > instance.worldBoundsMax.y ||
+            glZ < instance.worldBoundsMin.z - 2.0f || glZ > instance.worldBoundsMax.z + 2.0f) {
+            continue;
+        }
+
         auto it = models.find(instance.modelId);
         if (it == models.end()) continue;
         if (instance.scale <= 0.001f) continue;
@@ -627,11 +803,30 @@ std::optional<float> M2Renderer::getFloorHeight(float glX, float glY, float glZ)
 
 bool M2Renderer::checkCollision(const glm::vec3& from, const glm::vec3& to,
                                  glm::vec3& adjustedPos, float playerRadius) const {
+    QueryTimer timer(&queryTimeMs, &queryCallCount);
     adjustedPos = to;
     bool collided = false;
 
+    glm::vec3 queryMin = glm::min(from, to) - glm::vec3(7.0f, 7.0f, 5.0f);
+    glm::vec3 queryMax = glm::max(from, to) + glm::vec3(7.0f, 7.0f, 5.0f);
+    gatherCandidates(queryMin, queryMax, candidateScratch);
+
     // Check against all M2 instances in local space (rotation-aware).
-    for (const auto& instance : instances) {
+    for (size_t idx : candidateScratch) {
+        const auto& instance = instances[idx];
+        if (collisionFocusEnabled &&
+            pointAABBDistanceSq(collisionFocusPos, instance.worldBoundsMin, instance.worldBoundsMax) > collisionFocusRadiusSq) {
+            continue;
+        }
+
+        const float broadMargin = playerRadius + 1.0f;
+        if (from.x < instance.worldBoundsMin.x - broadMargin && adjustedPos.x < instance.worldBoundsMin.x - broadMargin) continue;
+        if (from.x > instance.worldBoundsMax.x + broadMargin && adjustedPos.x > instance.worldBoundsMax.x + broadMargin) continue;
+        if (from.y < instance.worldBoundsMin.y - broadMargin && adjustedPos.y < instance.worldBoundsMin.y - broadMargin) continue;
+        if (from.y > instance.worldBoundsMax.y + broadMargin && adjustedPos.y > instance.worldBoundsMax.y + broadMargin) continue;
+        if (from.z > instance.worldBoundsMax.z + 2.5f && adjustedPos.z > instance.worldBoundsMax.z + 2.5f) continue;
+        if (from.z + 2.5f < instance.worldBoundsMin.z && adjustedPos.z + 2.5f < instance.worldBoundsMin.z) continue;
+
         auto it = models.find(instance.modelId);
         if (it == models.end()) continue;
 
@@ -709,9 +904,29 @@ bool M2Renderer::checkCollision(const glm::vec3& from, const glm::vec3& to,
 }
 
 float M2Renderer::raycastBoundingBoxes(const glm::vec3& origin, const glm::vec3& direction, float maxDistance) const {
+    QueryTimer timer(&queryTimeMs, &queryCallCount);
     float closestHit = maxDistance;
 
-    for (const auto& instance : instances) {
+    glm::vec3 rayEnd = origin + direction * maxDistance;
+    glm::vec3 queryMin = glm::min(origin, rayEnd) - glm::vec3(1.0f);
+    glm::vec3 queryMax = glm::max(origin, rayEnd) + glm::vec3(1.0f);
+    gatherCandidates(queryMin, queryMax, candidateScratch);
+
+    for (size_t idx : candidateScratch) {
+        const auto& instance = instances[idx];
+        if (collisionFocusEnabled &&
+            pointAABBDistanceSq(collisionFocusPos, instance.worldBoundsMin, instance.worldBoundsMax) > collisionFocusRadiusSq) {
+            continue;
+        }
+
+        // Cheap world-space broad-phase.
+        float tEnter = 0.0f;
+        glm::vec3 worldMin = instance.worldBoundsMin - glm::vec3(0.35f);
+        glm::vec3 worldMax = instance.worldBoundsMax + glm::vec3(0.35f);
+        if (!segmentIntersectsAABB(origin, origin + direction * maxDistance, worldMin, worldMax, tEnter)) {
+            continue;
+        }
+
         auto it = models.find(instance.modelId);
         if (it == models.end()) continue;
 

src/rendering/performance_hud.cpp

@@ -11,6 +11,7 @@
 #include "rendering/weather.hpp"
 #include "rendering/character_renderer.hpp"
 #include "rendering/wmo_renderer.hpp"
+#include "rendering/m2_renderer.hpp"
 #include "rendering/camera.hpp"
 #include <imgui.h>
 #include <algorithm>
@@ -153,6 +154,28 @@ void PerformanceHUD::render(const Renderer* renderer, const Camera* camera) {
         ImGui::Text("Max: %.1f", maxFPS);
         ImGui::Text("Frame: %.2f ms", frameTime * 1000.0f);
 
+        ImGui::Spacing();
+        ImGui::TextColored(ImVec4(0.9f, 0.8f, 0.6f, 1.0f), "CPU TIMINGS (ms)");
+        ImGui::Text("Update: %.2f (Camera: %.2f)", renderer->getLastUpdateMs(), renderer->getLastCameraUpdateMs());
+        ImGui::Text("Render: %.2f (Terrain: %.2f, WMO: %.2f, M2: %.2f)",
+                    renderer->getLastRenderMs(),
+                    renderer->getLastTerrainRenderMs(),
+                    renderer->getLastWMORenderMs(),
+                    renderer->getLastM2RenderMs());
+        auto* wmoRenderer = renderer->getWMORenderer();
+        auto* m2Renderer = renderer->getM2Renderer();
+        if (wmoRenderer || m2Renderer) {
+            ImGui::Text("Collision queries:");
+            if (wmoRenderer) {
+                ImGui::Text("  WMO: %.2f ms (%u calls)",
+                            wmoRenderer->getQueryTimeMs(), wmoRenderer->getQueryCallCount());
+            }
+            if (m2Renderer) {
+                ImGui::Text("  M2:  %.2f ms (%u calls)",
+                            m2Renderer->getQueryTimeMs(), m2Renderer->getQueryCallCount());
+            }
+        }
+
         // Frame time graph
         if (!frameTimeHistory.empty()) {
             std::vector<float> frameTimesMs;

src/rendering/renderer.cpp

@@ -34,6 +34,7 @@
 #include <glm/gtc/quaternion.hpp>
 #include <cctype>
 #include <cmath>
+#include <chrono>
 #include <optional>
 #include <unordered_map>
 #include <unordered_set>
@@ -582,8 +583,17 @@ audio::FootstepSurface Renderer::resolveFootstepSurface() const {
 }
 
 void Renderer::update(float deltaTime) {
+    auto updateStart = std::chrono::steady_clock::now();
+    if (wmoRenderer) wmoRenderer->resetQueryStats();
+    if (m2Renderer) m2Renderer->resetQueryStats();
+
     if (cameraController) {
+        auto cameraStart = std::chrono::steady_clock::now();
         cameraController->update(deltaTime);
+        auto cameraEnd = std::chrono::steady_clock::now();
+        lastCameraUpdateMs = std::chrono::duration<double, std::milli>(cameraEnd - cameraStart).count();
+    } else {
+        lastCameraUpdateMs = 0.0;
     }
 
     // Sync character model position/rotation and animation with follow target
@@ -722,9 +732,17 @@ void Renderer::update(float deltaTime) {
     if (performanceHUD) {
         performanceHUD->update(deltaTime);
     }
+
+    auto updateEnd = std::chrono::steady_clock::now();
+    lastUpdateMs = std::chrono::duration<double, std::milli>(updateEnd - updateStart).count();
 }
 
 void Renderer::renderWorld(game::World* world) {
+    auto renderStart = std::chrono::steady_clock::now();
+    lastTerrainRenderMs = 0.0;
+    lastWMORenderMs = 0.0;
+    lastM2RenderMs = 0.0;
+
     (void)world;  // Unused for now
 
     // Get time of day for sky-related rendering
@@ -780,7 +798,10 @@ void Renderer::renderWorld(game::World* world) {
             terrainRenderer->setFog(fogColorArray, 400.0f, 1200.0f);
         }
 
+        auto terrainStart = std::chrono::steady_clock::now();
         terrainRenderer->render(*camera);
+        auto terrainEnd = std::chrono::steady_clock::now();
+        lastTerrainRenderMs = std::chrono::duration<double, std::milli>(terrainEnd - terrainStart).count();
 
         // Render water after terrain (transparency requires back-to-front rendering)
         if (waterRenderer) {
@@ -812,20 +833,29 @@ void Renderer::renderWorld(game::World* world) {
     if (wmoRenderer && camera) {
         glm::mat4 view = camera->getViewMatrix();
         glm::mat4 projection = camera->getProjectionMatrix();
+        auto wmoStart = std::chrono::steady_clock::now();
         wmoRenderer->render(*camera, view, projection);
+        auto wmoEnd = std::chrono::steady_clock::now();
+        lastWMORenderMs = std::chrono::duration<double, std::milli>(wmoEnd - wmoStart).count();
     }
 
     // Render M2 doodads (trees, rocks, etc.)
     if (m2Renderer && camera) {
         glm::mat4 view = camera->getViewMatrix();
         glm::mat4 projection = camera->getProjectionMatrix();
+        auto m2Start = std::chrono::steady_clock::now();
         m2Renderer->render(*camera, view, projection);
+        auto m2End = std::chrono::steady_clock::now();
+        lastM2RenderMs = std::chrono::duration<double, std::milli>(m2End - m2Start).count();
     }
 
     // Render minimap overlay
     if (minimap && camera && window) {
         minimap->render(*camera, window->getWidth(), window->getHeight());
     }
+
+    auto renderEnd = std::chrono::steady_clock::now();
+    lastRenderMs = std::chrono::duration<double, std::milli>(renderEnd - renderStart).count();
 }
 
 bool Renderer::loadTestTerrain(pipeline::AssetManager* assetManager, const std::string& adtPath) {

src/rendering/wmo_renderer.cpp

@@ -9,6 +9,7 @@
 #include <glm/gtc/matrix_transform.hpp>
 #include <glm/gtc/type_ptr.hpp>
 #include <algorithm>
+#include <chrono>
 #include <cmath>
 #include <limits>
 #include <unordered_set>
@@ -155,6 +156,8 @@ void WMORenderer::shutdown() {
 
     loadedModels.clear();
     instances.clear();
+    spatialGrid.clear();
+    instanceIndexById.clear();
     shader.reset();
 }
 
@@ -309,8 +312,22 @@ uint32_t WMORenderer::createInstance(uint32_t modelId, const glm::vec3& position
     instance.rotation = rotation;
     instance.scale = scale;
     instance.updateModelMatrix();
+    const ModelData& model = loadedModels[modelId];
+    transformAABB(instance.modelMatrix, model.boundingBoxMin, model.boundingBoxMax,
+                  instance.worldBoundsMin, instance.worldBoundsMax);
 
     instances.push_back(instance);
+    size_t idx = instances.size() - 1;
+    instanceIndexById[instance.id] = idx;
+    GridCell minCell = toCell(instance.worldBoundsMin);
+    GridCell maxCell = toCell(instance.worldBoundsMax);
+    for (int z = minCell.z; z <= maxCell.z; z++) {
+        for (int y = minCell.y; y <= maxCell.y; y++) {
+            for (int x = minCell.x; x <= maxCell.x; x++) {
+                spatialGrid[GridCell{x, y, z}].push_back(instance.id);
+            }
+        }
+    }
     core::Logger::getInstance().info("Created WMO instance ", instance.id, " (model ", modelId, ")");
     return instance.id;
 }
@@ -320,15 +337,96 @@ void WMORenderer::removeInstance(uint32_t instanceId) {
                           [instanceId](const WMOInstance& inst) { return inst.id == instanceId; });
     if (it != instances.end()) {
         instances.erase(it);
+        rebuildSpatialIndex();
         core::Logger::getInstance().info("Removed WMO instance ", instanceId);
     }
 }
 
 void WMORenderer::clearInstances() {
     instances.clear();
+    spatialGrid.clear();
+    instanceIndexById.clear();
     core::Logger::getInstance().info("Cleared all WMO instances");
 }
 
+void WMORenderer::setCollisionFocus(const glm::vec3& worldPos, float radius) {
+    collisionFocusEnabled = (radius > 0.0f);
+    collisionFocusPos = worldPos;
+    collisionFocusRadius = std::max(0.0f, radius);
+    collisionFocusRadiusSq = collisionFocusRadius * collisionFocusRadius;
+}
+
+void WMORenderer::clearCollisionFocus() {
+    collisionFocusEnabled = false;
+}
+
+void WMORenderer::resetQueryStats() {
+    queryTimeMs = 0.0;
+    queryCallCount = 0;
+}
+
+WMORenderer::GridCell WMORenderer::toCell(const glm::vec3& p) const {
+    return GridCell{
+        static_cast<int>(std::floor(p.x / SPATIAL_CELL_SIZE)),
+        static_cast<int>(std::floor(p.y / SPATIAL_CELL_SIZE)),
+        static_cast<int>(std::floor(p.z / SPATIAL_CELL_SIZE))
+    };
+}
+
+void WMORenderer::rebuildSpatialIndex() {
+    spatialGrid.clear();
+    instanceIndexById.clear();
+    instanceIndexById.reserve(instances.size());
+
+    for (size_t i = 0; i < instances.size(); i++) {
+        const auto& inst = instances[i];
+        instanceIndexById[inst.id] = i;
+
+        GridCell minCell = toCell(inst.worldBoundsMin);
+        GridCell maxCell = toCell(inst.worldBoundsMax);
+        for (int z = minCell.z; z <= maxCell.z; z++) {
+            for (int y = minCell.y; y <= maxCell.y; y++) {
+                for (int x = minCell.x; x <= maxCell.x; x++) {
+                    spatialGrid[GridCell{x, y, z}].push_back(inst.id);
+                }
+            }
+        }
+    }
+}
+
+void WMORenderer::gatherCandidates(const glm::vec3& queryMin, const glm::vec3& queryMax,
+                                   std::vector<size_t>& outIndices) const {
+    outIndices.clear();
+    candidateIdScratch.clear();
+
+    GridCell minCell = toCell(queryMin);
+    GridCell maxCell = toCell(queryMax);
+    for (int z = minCell.z; z <= maxCell.z; z++) {
+        for (int y = minCell.y; y <= maxCell.y; y++) {
+            for (int x = minCell.x; x <= maxCell.x; x++) {
+                auto it = spatialGrid.find(GridCell{x, y, z});
+                if (it == spatialGrid.end()) continue;
+                for (uint32_t id : it->second) {
+                    if (!candidateIdScratch.insert(id).second) continue;
+                    auto idxIt = instanceIndexById.find(id);
+                    if (idxIt != instanceIndexById.end()) {
+                        outIndices.push_back(idxIt->second);
+                    }
+                }
+            }
+        }
+    }
+
+    // Safety fallback: if the grid misses due streaming/index drift, avoid
+    // tunneling by scanning all instances instead of returning no candidates.
+    if (outIndices.empty() && !instances.empty()) {
+        outIndices.reserve(instances.size());
+        for (size_t i = 0; i < instances.size(); i++) {
+            outIndices.push_back(i);
+        }
+    }
+}
+
 void WMORenderer::render(const Camera& camera, const glm::mat4& view, const glm::mat4& projection) {
     if (!shader || instances.empty()) {
         lastDrawCalls = 0;
@@ -378,6 +476,14 @@ void WMORenderer::render(const Camera& camera, const glm::mat4& view, const glm:
             continue;
         }
 
+        if (frustumCulling) {
+            glm::vec3 instMin = instance.worldBoundsMin - glm::vec3(0.5f);
+            glm::vec3 instMax = instance.worldBoundsMax + glm::vec3(0.5f);
+            if (!frustum.intersectsAABB(instMin, instMax)) {
+                continue;
+            }
+        }
+
         const ModelData& model = modelIt->second;
         shader->setUniform("uModel", instance.modelMatrix);
 
@@ -727,6 +833,28 @@ static void transformAABB(const glm::mat4& modelMatrix,
     }
 }
 
+static float pointAABBDistanceSq(const glm::vec3& p, const glm::vec3& bmin, const glm::vec3& bmax) {
+    glm::vec3 q = glm::clamp(p, bmin, bmax);
+    glm::vec3 d = p - q;
+    return glm::dot(d, d);
+}
+
+struct QueryTimer {
+    double* totalMs = nullptr;
+    uint32_t* callCount = nullptr;
+    std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();
+    QueryTimer(double* total, uint32_t* calls) : totalMs(total), callCount(calls) {}
+    ~QueryTimer() {
+        if (callCount) {
+            (*callCount)++;
+        }
+        if (totalMs) {
+            auto end = std::chrono::steady_clock::now();
+            *totalMs += std::chrono::duration<double, std::milli>(end - start).count();
+        }
+    }
+};
+
 // Möller–Trumbore ray-triangle intersection
 // Returns distance along ray if hit, or negative if miss
 static float rayTriangleIntersect(const glm::vec3& origin, const glm::vec3& dir,
@@ -796,6 +924,7 @@ static glm::vec3 closestPointOnTriangle(const glm::vec3& p, const glm::vec3& a,
 }
 
 std::optional<float> WMORenderer::getFloorHeight(float glX, float glY, float glZ) const {
+    QueryTimer timer(&queryTimeMs, &queryCallCount);
     std::optional<float> bestFloor;
 
     // World-space ray: from high above, pointing straight down
@@ -808,7 +937,24 @@ std::optional<float> WMORenderer::getFloorHeight(float glX, float glY, float glZ
         core::Logger::getInstance().warning("WMO getFloorHeight: no instances loaded!");
     }
 
-    for (const auto& instance : instances) {
+    glm::vec3 queryMin(glX - 2.0f, glY - 2.0f, glZ - 8.0f);
+    glm::vec3 queryMax(glX + 2.0f, glY + 2.0f, glZ + 10.0f);
+    gatherCandidates(queryMin, queryMax, candidateScratch);
+
+    for (size_t idx : candidateScratch) {
+        const auto& instance = instances[idx];
+        if (collisionFocusEnabled &&
+            pointAABBDistanceSq(collisionFocusPos, instance.worldBoundsMin, instance.worldBoundsMax) > collisionFocusRadiusSq) {
+            continue;
+        }
+
+        // Broad-phase reject in world space to avoid expensive matrix transforms.
+        if (glX < instance.worldBoundsMin.x || glX > instance.worldBoundsMax.x ||
+            glY < instance.worldBoundsMin.y || glY > instance.worldBoundsMax.y ||
+            glZ < instance.worldBoundsMin.z - 2.0f || glZ > instance.worldBoundsMax.z + 4.0f) {
+            continue;
+        }
+
         auto it = loadedModels.find(instance.modelId);
         if (it == loadedModels.end()) continue;
 
@@ -875,6 +1021,7 @@ std::optional<float> WMORenderer::getFloorHeight(float glX, float glY, float glZ
 }
 
 bool WMORenderer::checkWallCollision(const glm::vec3& from, const glm::vec3& to, glm::vec3& adjustedPos) const {
+    QueryTimer timer(&queryTimeMs, &queryCallCount);
     adjustedPos = to;
     bool blocked = false;
 
@@ -892,7 +1039,25 @@ bool WMORenderer::checkWallCollision(const glm::vec3& from, const glm::vec3& to,
     int groupsChecked = 0;
     int wallsHit = 0;
 
-    for (const auto& instance : instances) {
+    glm::vec3 queryMin = glm::min(from, to) - glm::vec3(8.0f, 8.0f, 5.0f);
+    glm::vec3 queryMax = glm::max(from, to) + glm::vec3(8.0f, 8.0f, 5.0f);
+    gatherCandidates(queryMin, queryMax, candidateScratch);
+
+    for (size_t idx : candidateScratch) {
+        const auto& instance = instances[idx];
+        if (collisionFocusEnabled &&
+            pointAABBDistanceSq(collisionFocusPos, instance.worldBoundsMin, instance.worldBoundsMax) > collisionFocusRadiusSq) {
+            continue;
+        }
+
+        const float broadMargin = PLAYER_RADIUS + 1.5f;
+        if (from.x < instance.worldBoundsMin.x - broadMargin && to.x < instance.worldBoundsMin.x - broadMargin) continue;
+        if (from.x > instance.worldBoundsMax.x + broadMargin && to.x > instance.worldBoundsMax.x + broadMargin) continue;
+        if (from.y < instance.worldBoundsMin.y - broadMargin && to.y < instance.worldBoundsMin.y - broadMargin) continue;
+        if (from.y > instance.worldBoundsMax.y + broadMargin && to.y > instance.worldBoundsMax.y + broadMargin) continue;
+        if (from.z > instance.worldBoundsMax.z + PLAYER_HEIGHT && to.z > instance.worldBoundsMax.z + PLAYER_HEIGHT) continue;
+        if (from.z + PLAYER_HEIGHT < instance.worldBoundsMin.z && to.z + PLAYER_HEIGHT < instance.worldBoundsMin.z) continue;
+
         auto it = loadedModels.find(instance.modelId);
         if (it == loadedModels.end()) continue;
 
@@ -1012,7 +1177,24 @@ bool WMORenderer::checkWallCollision(const glm::vec3& from, const glm::vec3& to,
 }
 
 bool WMORenderer::isInsideWMO(float glX, float glY, float glZ, uint32_t* outModelId) const {
-    for (const auto& instance : instances) {
+    QueryTimer timer(&queryTimeMs, &queryCallCount);
+    glm::vec3 queryMin(glX - 0.5f, glY - 0.5f, glZ - 0.5f);
+    glm::vec3 queryMax(glX + 0.5f, glY + 0.5f, glZ + 0.5f);
+    gatherCandidates(queryMin, queryMax, candidateScratch);
+
+    for (size_t idx : candidateScratch) {
+        const auto& instance = instances[idx];
+        if (collisionFocusEnabled &&
+            pointAABBDistanceSq(collisionFocusPos, instance.worldBoundsMin, instance.worldBoundsMax) > collisionFocusRadiusSq) {
+            continue;
+        }
+
+        if (glX < instance.worldBoundsMin.x || glX > instance.worldBoundsMax.x ||
+            glY < instance.worldBoundsMin.y || glY > instance.worldBoundsMax.y ||
+            glZ < instance.worldBoundsMin.z || glZ > instance.worldBoundsMax.z) {
+            continue;
+        }
+
         auto it = loadedModels.find(instance.modelId);
         if (it == loadedModels.end()) continue;
 
@@ -1033,6 +1215,7 @@ bool WMORenderer::isInsideWMO(float glX, float glY, float glZ, uint32_t* outMode
 }
 
 float WMORenderer::raycastBoundingBoxes(const glm::vec3& origin, const glm::vec3& direction, float maxDistance) const {
+    QueryTimer timer(&queryTimeMs, &queryCallCount);
     float closestHit = maxDistance;
     // Camera collision should primarily react to walls.
     // Treat near-horizontal triangles as floor/ceiling and ignore them here so
@@ -1042,7 +1225,30 @@ float WMORenderer::raycastBoundingBoxes(const glm::vec3& origin, const glm::vec3
     constexpr float MAX_HIT_ABOVE_ORIGIN = 0.80f;
     constexpr float MIN_SURFACE_ALIGNMENT = 0.25f;
 
-    for (const auto& instance : instances) {
+    glm::vec3 rayEnd = origin + direction * maxDistance;
+    glm::vec3 queryMin = glm::min(origin, rayEnd) - glm::vec3(1.0f);
+    glm::vec3 queryMax = glm::max(origin, rayEnd) + glm::vec3(1.0f);
+    gatherCandidates(queryMin, queryMax, candidateScratch);
+
+    for (size_t idx : candidateScratch) {
+        const auto& instance = instances[idx];
+        if (collisionFocusEnabled &&
+            pointAABBDistanceSq(collisionFocusPos, instance.worldBoundsMin, instance.worldBoundsMax) > collisionFocusRadiusSq) {
+            continue;
+        }
+
+        glm::vec3 center = (instance.worldBoundsMin + instance.worldBoundsMax) * 0.5f;
+        float radius = glm::length(instance.worldBoundsMax - center);
+        if (glm::length(center - origin) > (maxDistance + radius + 1.0f)) {
+            continue;
+        }
+
+        glm::vec3 worldMin = instance.worldBoundsMin - glm::vec3(0.5f);
+        glm::vec3 worldMax = instance.worldBoundsMax + glm::vec3(0.5f);
+        if (!rayIntersectsAABB(origin, direction, worldMin, worldMax)) {
+            continue;
+        }
+
         auto it = loadedModels.find(instance.modelId);
         if (it == loadedModels.end()) continue;