fancy grid stuff

.vscode/settings.json

@@ -84,7 +84,8 @@
         "span": "cpp",
         "cinttypes": "cpp",
         "variant": "cpp",
-        "__nullptr": "cpp"
+        "__nullptr": "cpp",
+        "unordered_set": "cpp"
     },
     "files.exclude": {
         "**/*.rpyc": true,

util/grid/grid2.hpp

@@ -7,289 +7,352 @@
 #include <unordered_map>
 #include <string>
 #include <algorithm>
+#include <map>
+#include <unordered_set>
 
 class Grid2 {
+private:
+    //size_t is index.
+    //vec2 is x,y position of the sparse value
+    std::multimap<size_t, Vec2> positions;
+    //vec4 is rgba color at the position
+    std::multimap<size_t, Vec4> colors;
+    //size is a floating size to assign to a "pixel" (or voxel for grid3) to allow larger or smaller assignments in this map
+    std::multimap<size_t, float> sizes;
+    //others will be added later
+    size_t next_id;
+    
+    std::unordered_map<size_t, std::pair<int, int>> cellIndices; // object ID -> grid cell
+    std::unordered_map<std::pair<int, int>, std::unordered_set<size_t>> spatialGrid; // cell -> object IDs
+    float cellSize;
+    
 public:
-    int width, height;
+    Grid2() : next_id(0), cellSize(1.0f) {}
+    Grid2(float cellSize = 1.0f) : next_id(0), cellSize(cellSize) {}
     
-    Grid2() : width(0), height(0) {}
+    size_t addObject(const Vec2& position, const Vec4& color, float size = 1.0f) {
-    Grid2(int size) : width(size), height(size) {
+        size_t id = next_id++;
-        positions.reserve(size);
+        positions.insert({id, position});
-        colors.reserve(size);
+        colors.insert({id, color});
-        sizes.reserve(size);
+        sizes.insert({id, size});
-    }
+        std::pair<int,int> cell = worldToGrid(position);
-    Grid2(int width, int height) : width(width), height(height) {
+        spatialGrid[cell].insert(id);
-        positions.reserve(width * height);
+        cellIndices[id] = cell;
-        colors.reserve(width * height);
+        return id;
-        sizes.reserve(width * height);
     }
 
-    // Add a pixel at specific position
+    //gets
-    int addPixel(const Vec2& position, const Vec4& color, float size = 1.0f) {
+    Vec2 getPosition(size_t id) const {
-        int index = positions.size();
+        auto it = positions.find(id);
-        positions.push_back(position);
+        if (it != positions.end()) return it->second;
-        colors.push_back(color);
+        return Vec2();
-        sizes.push_back(size);
-        positionToIndex[position] = index;
-        return index;
     }
 
-    // Add a pixel with integer coordinates
+    Vec4 getColor(size_t id) const {
-    int addPixel(int x, int y, const Vec4& color, float size = 1.0f) {
+        auto it = colors.find(id);
-        return addPixel(Vec2(static_cast<float>(x), static_cast<float>(y)), color, size);
+        if (it != colors.end()) return it->second;
+        return Vec4();
     }
 
-    // Check if position is occupied
+    float getSize(size_t id) const {
-    bool isOccupied(const Vec2& position) const {
+        auto it = sizes.find(id);
-        return positionToIndex.find(position) != positionToIndex.end();
+        if (it != sizes.end()) return it->second;
+        return 1.0f;
     }
 
-    bool isOccupied(int x, int y) const {
+    //sets
-        return isOccupied(Vec2(static_cast<float>(x), static_cast<float>(y)));
+    void setPosition(size_t id, const Vec2& position) {
+        if (!hasObject(id)) return;
+        
+        Vec2 oldPos = getPosition(id);
+        positions.erase(id);
+        positions.insert({id, position});
+        updateSpatialIndex(id, oldPos, position);
     }
 
-    // Get pixel index at position, returns -1 if not found
+    void setColor(size_t id, const Vec4& color) {
-    int getPixelIndex(const Vec2& position) const {
+        colors.erase(id);
-        auto it = positionToIndex.find(position);
+        colors.insert({id, color});
-        return (it != positionToIndex.end()) ? it->second : -1;
     }
 
-    int getPixelIndex(int x, int y) const {
+    void setSize(size_t id, float size) {
-        return getPixelIndex(Vec2(static_cast<float>(x), static_cast<float>(y)));
+        sizes.erase(id);
+        sizes.insert({id, size});
     }
     
-    // Remove pixel at position
+    // Batch add/remove operations
-    bool removePixel(const Vec2& position) {
+    void addObjects(const std::vector<std::tuple<Vec2, Vec4, float>>& objects) {
-        int index = getPixelIndex(position);
+        for (const auto& obj : objects) {
-        if (index == -1) return false;
+            addObject(std::get<0>(obj), std::get<1>(obj), std::get<2>(obj));
-        
+        }
-        // Swap with last element and update map
-        if (index != positions.size() - 1) {
-            positions[index] = positions.back();
-            colors[index] = colors.back();
-            sizes[index] = sizes.back();
-            
-            // Update mapping for the moved element
-            positionToIndex[positions[index]] = index;
     }
     
-        // Remove last element
+    void removeObjects(const std::vector<size_t>& ids) {
-        positions.pop_back();
+        for (size_t id : ids) {
-        colors.pop_back();
+            removeObject(id);
-        sizes.pop_back();
+        }
-        positionToIndex.erase(position);
-        
-        return true;
     }
     
-    bool removePixel(int x, int y) {
+    // Batch position updates
-        return removePixel(Vec2(static_cast<float>(x), static_cast<float>(y)));
+    void updatePositions(const std::unordered_map<size_t, Vec2>& newPositions) {
+        // Bulk update spatial grid - collect all changes first
+        std::vector<std::tuple<size_t, Vec2, Vec2>> spatialUpdates;
+        
+        for (const auto& pair : newPositions) {
+            if (hasObject(pair.first)) {
+                Vec2 oldPos = getPosition(pair.first);
+                positions.erase(pair.first);
+                positions.insert({pair.first, pair.second});
+                spatialUpdates.emplace_back(pair.first, oldPos, pair.second);
+            }
         }
         
-    // Clear all pixels
+        // Apply all spatial updates at once
-    void clear() {
+        for (const auto& update : spatialUpdates) {
-        positions.clear();
+            updateSpatialIndex(std::get<0>(update), std::get<1>(update), std::get<2>(update));
-        colors.clear();
+        }
-        sizes.clear();
-        positionToIndex.clear();
     }
     
-    // Get pixel count
+    //other
-    size_t getPixelCount() const {
+    bool hasObject(size_t id) const {
-        return positions.size();
+        return positions.find(id) != positions.end();
     }
     
-    // Check if grid is empty
+    void removeObject(size_t id) {
-    bool isEmpty() const {
+        // Remove from spatial grid first
-        return positions.empty();
+        auto cellIt = cellIndices.find(id);
+        if (cellIt != cellIndices.end()) {
+            auto& cellObjects = spatialGrid[cellIt->second];
+            cellObjects.erase(id);
+            if (cellObjects.empty()) {
+                spatialGrid.erase(cellIt->second);
+            }
+            cellIndices.erase(id);
+        }
+        
+        // Remove from data maps
+        positions.erase(id);
+        colors.erase(id);
+        sizes.erase(id);
+    }
+    
+    std::vector<size_t> getIndicesAt(float x, float y, float radius = 0.0f) const {
+        return getIndicesAt(Vec2(x, y), radius);
+    }
+    
+    std::vector<size_t> getIndicesAt(const Vec2& position, float radius = 0.0f) const {
+        std::vector<size_t> result;
+        
+        if (radius <= 0.0f) {
+            // Exact position match
+            for (const auto& pair : positions) {
+                if (pair.second == position) {
+                    result.push_back(pair.first);
+                }
+            }
+        } else {
+            // Radius-based search
+            float radius_sq = radius * radius;
+            for (const auto& pair : positions) {
+                float dx = pair.second.x - position.x;
+                float dy = pair.second.y - position.y;
+                if (dx * dx + dy * dy <= radius_sq) {
+                    result.push_back(pair.first);
+                }
+            }
+        }
+        
+        return result;
     }
     
-    // Get bounding box of occupied pixels
     void getBoundingBox(Vec2& minCorner, Vec2& maxCorner) const {
         if (positions.empty()) {
-            minCorner = Vec2(0, 0);
+            minCorner = Vec2(0.0f, 0.0f);
-            maxCorner = Vec2(0, 0);
+            maxCorner = Vec2(0.0f, 0.0f);
             return;
         }
         
-        minCorner = positions[0];
+        auto it = positions.begin();
-        maxCorner = positions[0];
+        minCorner = it->second;
+        maxCorner = it->second;
         
-        for (const auto& pos : positions) {
+        for (const auto& pair : positions) {
-            minCorner = minCorner.min(pos);
+            const Vec2& pos = pair.second;
-            maxCorner = maxCorner.max(pos);
+            minCorner.x = std::min(minCorner.x, pos.x);
+            minCorner.y = std::min(minCorner.y, pos.y);
+            maxCorner.x = std::max(maxCorner.x, pos.x);
+            maxCorner.y = std::max(maxCorner.y, pos.y);
         }
     }
     
-    // Fill a rectangular region
+    //to picture
-    void fillRectangle(const Vec2& start, const Vec2& end, const Vec4& color, float size = 1.0f) {
+    void getGridAsRGB(int& width, int& height, std::vector<int>& rgbData) const {
-        int startX = static_cast<int>(std::min(start.x, end.x));
+        Vec2 minCorner, maxCorner;
-        int endX = static_cast<int>(std::max(start.x, end.x));
+        getBoundingBox(minCorner, maxCorner);
-        int startY = static_cast<int>(std::min(start.y, end.y));
-        int endY = static_cast<int>(std::max(start.y, end.y));
         
-        for (int y = startY; y <= endY; ++y) {
+        // Calculate grid dimensions (adding 1 to include both ends)
-            for (int x = startX; x <= endX; ++x) {
+        width = static_cast<int>(std::ceil(maxCorner.x - minCorner.x)) + 1;
-                if (!isOccupied(x, y)) {
+        height = static_cast<int>(std::ceil(maxCorner.y - minCorner.y)) + 1;
-                    addPixel(x, y, color, size);
+        
+        // Initialize with black (0,0,0)
+        rgbData.resize(width * height * 3, 0);
+        
+        // Fill the grid with object colors
+        for (const auto& posPair : positions) {
+            size_t id = posPair.first;
+            const Vec2& pos = posPair.second;
+            
+            // Convert world position to grid coordinates
+            int gridX = static_cast<int>(pos.x - minCorner.x);
+            int gridY = static_cast<int>(pos.y - minCorner.y);
+            
+            if (gridX >= 0 && gridX < width && gridY >= 0 && gridY < height) {
+                const Vec4& color = getColor(id);
+                int index = (gridY * width + gridX) * 3;
+                
+                // Convert float color [0,1] to int [0,255]
+                rgbData[index] = static_cast<int>(color.r * 255);
+                rgbData[index + 1] = static_cast<int>(color.g * 255);
+                rgbData[index + 2] = static_cast<int>(color.b * 255);
+            }
+        }
+    }
+    
+    void getRegionAsRGB(float minX, float minY, float maxX, float maxY, 
+                       int& width, int& height, std::vector<int>& rgbData) const {
+        // Ensure valid region
+        if (minX >= maxX || minY >= maxY) {
+            width = 0;
+            height = 0;
+            rgbData.clear();
+            return;
+        }
+        
+        // Calculate grid dimensions
+        width = static_cast<int>(std::ceil(maxX - minX));
+        height = static_cast<int>(std::ceil(maxY - minY));
+        
+        // Initialize with black (0,0,0)
+        rgbData.resize(width * height * 3, 0);
+        
+        // Fill the grid with object colors in the region
+        for (const auto& posPair : positions) {
+            size_t id = posPair.first;
+            const Vec2& pos = posPair.second;
+            
+            // Check if position is within the region
+            if (pos.x >= minX && pos.x < maxX && pos.y >= minY && pos.y < maxY) {
+                // Convert world position to grid coordinates
+                int gridX = static_cast<int>(pos.x - minX);
+                int gridY = static_cast<int>(pos.y - minY);
+                
+                if (gridX >= 0 && gridX < width && gridY >= 0 && gridY < height) {
+                    const Vec4& color = getColor(id);
+                    int index = (gridY * width + gridX) * 3;
+                    
+                    // Convert float color [0,1] to int [0,255]
+                    rgbData[index] = static_cast<int>(color.r * 255);
+                    rgbData[index + 1] = static_cast<int>(color.g * 255);
+                    rgbData[index + 2] = static_cast<int>(color.b * 255);
                 }
             }
         }
     }
     
-    // Create a circle pattern
+    void getRegionAsRGB(const Vec2& minCorner, const Vec2& maxCorner,
-    void fillCircle(const Vec2& center, float radius, const Vec4& color, float size = 1.0f) {
+                       int& width, int& height, std::vector<int>& rgbData) const {
-        int centerX = static_cast<int>(center.x);
+        getRegionAsRGB(minCorner.x, minCorner.y, maxCorner.x, maxCorner.y, 
-        int centerY = static_cast<int>(center.y);
+                      width, height, rgbData);
-        int radiusInt = static_cast<int>(radius);
+    }
 
-        for (int y = centerY - radiusInt; y <= centerY + radiusInt; ++y) {
+    //spatial map
-            for (int x = centerX - radiusInt; x <= centerX + radiusInt; ++x) {
+    std::pair<int, int> worldToGrid(const Vec2& pos) const {
-                float dx = x - center.x;
+        return {
-                float dy = y - center.y;
+            static_cast<int>(std::floor(pos.x / cellSize)),
-                if (dx * dx + dy * dy <= radius * radius) {
+            static_cast<int>(std::floor(pos.y / cellSize))
-                    if (!isOccupied(x, y)) {
+        };
-                        addPixel(x, y, color, size);
+    }
+    
+    void updateSpatialIndex(size_t id, const Vec2& oldPos, const Vec2& newPos) {
+        auto oldCell = worldToGrid(oldPos);
+        auto newCell = worldToGrid(newPos);
+        
+        if (oldCell != newCell) {
+            // Remove from old cell
+            auto oldIt = spatialGrid.find(oldCell);
+            if (oldIt != spatialGrid.end()) {
+                oldIt->second.erase(id);
+                if (oldIt->second.empty()) {
+                    spatialGrid.erase(oldIt);
+                }
+            }
+            
+            // Add to new cell
+            spatialGrid[newCell].insert(id);
+            cellIndices[id] = newCell;
+        }
+    }
+    
+    std::vector<size_t> getIndicesInRadius(const Vec2& position, float radius) const {
+        std::vector<size_t> result;
+        
+        Vec2 minPos(position.x - radius, position.y - radius);
+        Vec2 maxPos(position.x + radius, position.y + radius);
+        
+        auto minCell = worldToGrid(minPos);
+        auto maxCell = worldToGrid(maxPos);
+        
+        float radiusSq = radius * radius;
+        
+        // Only check relevant cells
+        for (int x = minCell.first; x <= maxCell.first; ++x) {
+            for (int y = minCell.second; y <= maxCell.second; ++y) {
+                auto cell = std::make_pair(x, y);
+                auto it = spatialGrid.find(cell);
+                if (it != spatialGrid.end()) {
+                    for (size_t id : it->second) {
+                        const Vec2& objPos = getPosition(id);
+                        float dx = objPos.x - position.x;
+                        float dy = objPos.y - position.y;
+                        if (dx * dx + dy * dy <= radiusSq) {
+                            result.push_back(id);
                         }
                     }
                 }
             }
         }
         
-    // Create a line between two points
+        return result;
-    void drawLine(const Vec2& start, const Vec2& end, const Vec4& color, float size = 1.0f) {
-        // Bresenham's line algorithm
-        int x0 = static_cast<int>(start.x);
-        int y0 = static_cast<int>(start.y);
-        int x1 = static_cast<int>(end.x);
-        int y1 = static_cast<int>(end.y);
-        
-        int dx = std::abs(x1 - x0);
-        int dy = std::abs(y1 - y0);
-        int sx = (x0 < x1) ? 1 : -1;
-        int sy = (y0 < y1) ? 1 : -1;
-        int err = dx - dy;
-        
-        while (true) {
-            if (!isOccupied(x0, y0)) {
-                addPixel(x0, y0, color, size);
     }
     
-            if (x0 == x1 && y0 == y1) break;
+    std::vector<size_t> getIndicesInRegion(const Vec2& minCorner, const Vec2& maxCorner) const {
+        std::vector<size_t> result;
         
-            int e2 = 2 * err;
+        auto minCell = worldToGrid(minCorner);
-            if (e2 > -dy) {
+        auto maxCell = worldToGrid(maxCorner);
-                err -= dy;
-                x0 += sx;
-            }
-            if (e2 < dx) {
-                err += dx;
-                y0 += sy;
-            }
-        }
-    }
         
-    // Get neighbors of a pixel (4-connected)
+        for (int x = minCell.first; x <= maxCell.first; ++x) {
-    std::vector<int> getNeighbors4(const Vec2& position) const {
+            for (int y = minCell.second; y <= maxCell.second; ++y) {
-        std::vector<int> neighbors;
+                auto cell = std::make_pair(x, y);
-        Vec2 offsets[] = {Vec2(1, 0), Vec2(-1, 0), Vec2(0, 1), Vec2(0, -1)};
+                auto it = spatialGrid.find(cell);
-        
+                if (it != spatialGrid.end()) {
-        for (const auto& offset : offsets) {
+                    for (size_t id : it->second) {
-            Vec2 neighborPos = position + offset;
+                        const Vec2& pos = getPosition(id);
-            int index = getPixelIndex(neighborPos);
+                        if (pos.x >= minCorner.x && pos.x <= maxCorner.x &&
-            if (index != -1) {
+                            pos.y >= minCorner.y && pos.y <= maxCorner.y) {
-                neighbors.push_back(index);
+                            result.push_back(id);
-            }
-        }
-        
-        return neighbors;
-    }
-    
-    // Get neighbors of a pixel (8-connected)
-    std::vector<int> getNeighbors8(const Vec2& position) const {
-        std::vector<int> neighbors;
-        
-        for (int dy = -1; dy <= 1; ++dy) {
-            for (int dx = -1; dx <= 1; ++dx) {
-                if (dx == 0 && dy == 0) continue;
-                
-                Vec2 neighborPos = position + Vec2(dx, dy);
-                int index = getPixelIndex(neighborPos);
-                if (index != -1) {
-                    neighbors.push_back(index);
-                }
-            }
-        }
-        
-        return neighbors;
-    }
-    
-    // Find connected components
-    std::vector<std::vector<int>> findConnectedComponents() const {
-        std::vector<std::vector<int>> components;
-        std::unordered_map<Vec2, bool, std::hash<Vec2>> visited;
-        
-        for (size_t i = 0; i < positions.size(); ++i) {
-            const Vec2& pos = positions[i];
-            if (visited.find(pos) == visited.end()) {
-                std::vector<int> component;
-                floodFill(pos, visited, component);
-                components.push_back(component);
-            }
-        }
-        
-        return components;
-    }
-    
-    // Getters
-    const std::vector<Vec2>& getPositions() const { return positions; }
-    const std::vector<Vec4>& getColors() const { return colors; }
-    const std::vector<float>& getSizes() const { return sizes; }
-    
-    Vec2 getPosition(int index) const { return positions[index]; }
-    Vec4 getColor(int index) const { return colors[index]; }
-    float getSize(int index) const { return sizes[index]; }
-    
-    void setColor(int index, const Vec4& color) { colors[index] = color; }
-    void setSize(int index, float size) { sizes[index] = size; }
-    
-    int getWidth() const { return width; }
-    int getHeight() const { return height; }
-
-private:
-    std::vector<Vec2> positions;
-    std::vector<Vec4> colors;
-    std::vector<float> sizes;
-    std::unordered_map<Vec2, int, std::hash<Vec2>> positionToIndex;
-    
-    void floodFill(const Vec2& start, std::unordered_map<Vec2, bool, std::hash<Vec2>>& visited, 
-                   std::vector<int>& component) const {
-        std::vector<Vec2> stack;
-        stack.push_back(start);
-        
-        while (!stack.empty()) {
-            Vec2 current = stack.back();
-            stack.pop_back();
-            
-            if (visited.find(current) != visited.end()) continue;
-            
-            visited[current] = true;
-            int index = getPixelIndex(current);
-            if (index != -1) {
-                component.push_back(index);
-                
-                // Add 4-connected neighbors
-                Vec2 neighbors[] = {current + Vec2(1, 0), current + Vec2(-1, 0), 
-                                   current + Vec2(0, 1), current + Vec2(0, -1)};
-                
-                for (const auto& neighbor : neighbors) {
-                    if (isOccupied(neighbor) && visited.find(neighbor) == visited.end()) {
-                        stack.push_back(neighbor);
                         }
                     }
                 }
             }
         }
+        
+        return result;
+    }
+
+    size_t getSpatialGridCellCount() const { return spatialGrid.size(); }
+    size_t getSpatialGridObjectCount() const { return cellIndices.size(); }
+    float getCellSize() const { return cellSize; }
 };
 
 #endif

util/grid/grid3.hpp

@@ -1,296 +1,354 @@
 #ifndef GRID3_HPP
 #define GRID3_HPP
 
-#include "../vec3.hpp"
+#include "vec3.hpp"
-#include "../vec4.hpp"
+#include "vec4.hpp"
 #include <vector>
 #include <unordered_map>
 #include <string>
 #include <algorithm>
-#include <functional>
+#include <map>
+#include <unordered_set>
 
 class Grid3 {
+private:
+    // size_t is index
+    // Vec3 is x,y,z position of the sparse voxel
+    std::multimap<size_t, Vec3> positions;
+    // Vec4 is rgba color at the position
+    std::multimap<size_t, Vec4> colors;
+    // size is a floating size to assign to a voxel to allow larger or smaller assignments
+    std::multimap<size_t, float> sizes;
+    size_t next_id;
+    
+    std::unordered_map<size_t, std::tuple<int, int, int>> cellIndices; // object ID -> grid cell
+    std::unordered_map<std::tuple<int, int, int>, std::unordered_set<size_t>> spatialGrid; // cell -> object IDs
+    float cellSize;
+    
 public:
-    // Constructors
+    Grid3() : next_id(0), cellSize(1.0f) {}
-    Grid3() : width(0), height(0), depth(0) {}
+    Grid3(float cellSize) : next_id(0), cellSize(cellSize) {}
-    Grid3(int size) : width(size), height(size), depth(size) {
+    
-        positions.reserve(size * size * size);
+    size_t addObject(const Vec3& position, const Vec4& color, float size = 1.0f) {
-        colors.reserve(size * size * size);
+        size_t id = next_id++;
-        sizes.reserve(size * size * size);
+        positions.insert({id, position});
-    }
+        colors.insert({id, color});
-    Grid3(int width, int height, int depth) : width(width), height(height), depth(depth) {
+        sizes.insert({id, size});
-        positions.reserve(width * height * depth);
+        auto cell = worldToGrid(position);
-        colors.reserve(width * height * depth);
+        spatialGrid[cell].insert(id);
-        sizes.reserve(width * height * depth);
+        cellIndices[id] = cell;
+        return id;
     }
 
-    // Add a voxel at specific position
+    // Gets
-    int addVoxel(const Vec3& position, const Vec4& color, float size = 1.0f) {
+    Vec3 getPosition(size_t id) const {
-        int index = positions.size();
+        auto it = positions.find(id);
-        positions.push_back(position);
+        if (it != positions.end()) return it->second;
-        colors.push_back(color);
+        return Vec3();
-        sizes.push_back(size);
-        positionToIndex[position] = index;
-        return index;
     }
 
-    // Add a voxel with integer coordinates
+    Vec4 getColor(size_t id) const {
-    int addVoxel(int x, int y, int z, const Vec4& color, float size = 1.0f) {
+        auto it = colors.find(id);
-        return addVoxel(Vec3(static_cast<float>(x), static_cast<float>(y), static_cast<float>(z)), color, size);
+        if (it != colors.end()) return it->second;
+        return Vec4();
     }
 
-    // Check if position is occupied
+    float getSize(size_t id) const {
-    bool isOccupied(const Vec3& position) const {
+        auto it = sizes.find(id);
-        return positionToIndex.find(position) != positionToIndex.end();
+        if (it != sizes.end()) return it->second;
+        return 1.0f;
     }
 
-    bool isOccupied(int x, int y, int z) const {
+    // Sets
-        return isOccupied(Vec3(static_cast<float>(x), static_cast<float>(y), static_cast<float>(z)));
+    void setPosition(size_t id, const Vec3& position) {
+        if (!hasObject(id)) return;
+        
+        Vec3 oldPos = getPosition(id);
+        positions.erase(id);
+        positions.insert({id, position});
+        updateSpatialIndex(id, oldPos, position);
     }
 
-    // Get voxel index at position, returns -1 if not found
+    void setColor(size_t id, const Vec4& color) {
-    int getVoxelIndex(const Vec3& position) const {
+        colors.erase(id);
-        auto it = positionToIndex.find(position);
+        colors.insert({id, color});
-        return (it != positionToIndex.end()) ? it->second : -1;
     }
 
-    int getVoxelIndex(int x, int y, int z) const {
+    void setSize(size_t id, float size) {
-        return getVoxelIndex(Vec3(static_cast<float>(x), static_cast<float>(y), static_cast<float>(z)));
+        sizes.erase(id);
+        sizes.insert({id, size});
     }
     
-    // Remove voxel at position
+    // Batch add/remove operations
-    bool removeVoxel(const Vec3& position) {
+    void addObjects(const std::vector<std::tuple<Vec3, Vec4, float>>& objects) {
-        int index = getVoxelIndex(position);
+        for (const auto& obj : objects) {
-        if (index == -1) return false;
+            addObject(std::get<0>(obj), std::get<1>(obj), std::get<2>(obj));
-        
+        }
-        // Swap with last element and update map
-        if (index != positions.size() - 1) {
-            positions[index] = positions.back();
-            colors[index] = colors.back();
-            sizes[index] = sizes.back();
-            
-            // Update mapping for the moved element
-            positionToIndex[positions[index]] = index;
     }
     
-        // Remove last element
+    void removeObjects(const std::vector<size_t>& ids) {
-        positions.pop_back();
+        for (size_t id : ids) {
-        colors.pop_back();
+            removeObject(id);
-        sizes.pop_back();
+        }
-        positionToIndex.erase(position);
-        
-        return true;
     }
     
-    bool removeVoxel(int x, int y, int z) {
+    // Batch position updates
-        return removeVoxel(Vec3(static_cast<float>(x), static_cast<float>(y), static_cast<float>(z)));
+    void updatePositions(const std::unordered_map<size_t, Vec3>& newPositions) {
+        // Bulk update spatial grid - collect all changes first
+        std::vector<std::tuple<size_t, Vec3, Vec3>> spatialUpdates;
+        
+        for (const auto& pair : newPositions) {
+            if (hasObject(pair.first)) {
+                Vec3 oldPos = getPosition(pair.first);
+                positions.erase(pair.first);
+                positions.insert({pair.first, pair.second});
+                spatialUpdates.emplace_back(pair.first, oldPos, pair.second);
+            }
         }
         
-    // Clear all voxels
+        // Apply all spatial updates at once
-    void clear() {
+        for (const auto& update : spatialUpdates) {
-        positions.clear();
+            updateSpatialIndex(std::get<0>(update), std::get<1>(update), std::get<2>(update));
-        colors.clear();
+        }
-        sizes.clear();
-        positionToIndex.clear();
     }
     
-    // Get voxel count
+    // Other
-    size_t getVoxelCount() const {
+    bool hasObject(size_t id) const {
-        return positions.size();
+        return positions.find(id) != positions.end();
     }
     
-    // Check if grid is empty
+    void removeObject(size_t id) {
-    bool isEmpty() const {
+        // Remove from spatial grid first
-        return positions.empty();
+        auto cellIt = cellIndices.find(id);
+        if (cellIt != cellIndices.end()) {
+            auto& cellObjects = spatialGrid[cellIt->second];
+            cellObjects.erase(id);
+            if (cellObjects.empty()) {
+                spatialGrid.erase(cellIt->second);
+            }
+            cellIndices.erase(id);
+        }
+        
+        // Remove from data maps
+        positions.erase(id);
+        colors.erase(id);
+        sizes.erase(id);
+    }
+    
+    std::vector<size_t> getIndicesAt(float x, float y, float z, float radius = 0.0f) const {
+        return getIndicesAt(Vec3(x, y, z), radius);
+    }
+    
+    std::vector<size_t> getIndicesAt(const Vec3& position, float radius = 0.0f) const {
+        std::vector<size_t> result;
+        
+        if (radius <= 0.0f) {
+            // Exact position match
+            for (const auto& pair : positions) {
+                if (pair.second == position) {
+                    result.push_back(pair.first);
+                }
+            }
+        } else {
+            // Radius-based search
+            float radius_sq = radius * radius;
+            for (const auto& pair : positions) {
+                float dx = pair.second.x - position.x;
+                float dy = pair.second.y - position.y;
+                float dz = pair.second.z - position.z;
+                if (dx * dx + dy * dy + dz * dz <= radius_sq) {
+                    result.push_back(pair.first);
+                }
+            }
+        }
+        
+        return result;
     }
     
-    // Get bounding box of occupied voxels
     void getBoundingBox(Vec3& minCorner, Vec3& maxCorner) const {
         if (positions.empty()) {
-            minCorner = Vec3(0, 0, 0);
+            minCorner = Vec3(0.0f, 0.0f, 0.0f);
-            maxCorner = Vec3(0, 0, 0);
+            maxCorner = Vec3(0.0f, 0.0f, 0.0f);
             return;
         }
         
-        minCorner = positions[0];
+        auto it = positions.begin();
-        maxCorner = positions[0];
+        minCorner = it->second;
+        maxCorner = it->second;
         
-        for (const auto& pos : positions) {
+        for (const auto& pair : positions) {
-            minCorner = minCorner.min(pos);
+            const Vec3& pos = pair.second;
-            maxCorner = maxCorner.max(pos);
+            minCorner.x = std::min(minCorner.x, pos.x);
+            minCorner.y = std::min(minCorner.y, pos.y);
+            minCorner.z = std::min(minCorner.z, pos.z);
+            maxCorner.x = std::max(maxCorner.x, pos.x);
+            maxCorner.y = std::max(maxCorner.y, pos.y);
+            maxCorner.z = std::max(maxCorner.z, pos.z);
         }
     }
     
-    // Fill a rectangular prism region
+    // Get 2D slice of the 3D grid (useful for visualization)
-    void fillCuboid(const Vec3& start, const Vec3& end, const Vec4& color, float size = 1.0f) {
+    void getSliceAsRGB(int axis, float slicePos, 
-        int startX = static_cast<int>(std::min(start.x, end.x));
+                      int& width, int& height, std::vector<int>& rgbData) const {
-        int endX = static_cast<int>(std::max(start.x, end.x));
+        Vec3 minCorner, maxCorner;
-        int startY = static_cast<int>(std::min(start.y, end.y));
+        getBoundingBox(minCorner, maxCorner);
-        int endY = static_cast<int>(std::max(start.y, end.y));
-        int startZ = static_cast<int>(std::min(start.z, end.z));
-        int endZ = static_cast<int>(std::max(start.z, end.z));
         
-        for (int z = startZ; z <= endZ; ++z) {
+        // Determine slice dimensions based on axis (0=x, 1=y, 2=z)
-            for (int y = startY; y <= endY; ++y) {
+        if (axis == 0) { // X-slice
-                for (int x = startX; x <= endX; ++x) {
+            width = static_cast<int>(std::ceil(maxCorner.z - minCorner.z)) + 1;
-                    if (!isOccupied(x, y, z)) {
+            height = static_cast<int>(std::ceil(maxCorner.y - minCorner.y)) + 1;
-                        addVoxel(x, y, z, color, size);
+        } else if (axis == 1) { // Y-slice
+            width = static_cast<int>(std::ceil(maxCorner.z - minCorner.z)) + 1;
+            height = static_cast<int>(std::ceil(maxCorner.x - minCorner.x)) + 1;
+        } else { // Z-slice
+            width = static_cast<int>(std::ceil(maxCorner.x - minCorner.x)) + 1;
+            height = static_cast<int>(std::ceil(maxCorner.y - minCorner.y)) + 1;
         }
+        
+        // Initialize with black (0,0,0)
+        rgbData.resize(width * height * 3, 0);
+        
+        // Fill the slice with object colors
+        for (const auto& posPair : positions) {
+            size_t id = posPair.first;
+            const Vec3& pos = posPair.second;
+            
+            // Check if position is within slice tolerance
+            float tolerance = 0.5f; // Half voxel tolerance
+            bool inSlice = false;
+            int gridX = 0, gridY = 0;
+            
+            if (axis == 0 && std::abs(pos.x - slicePos) <= tolerance) { // X-slice
+                gridX = static_cast<int>(pos.z - minCorner.z);
+                gridY = static_cast<int>(pos.y - minCorner.y);
+                inSlice = true;
+            } else if (axis == 1 && std::abs(pos.y - slicePos) <= tolerance) { // Y-slice
+                gridX = static_cast<int>(pos.z - minCorner.z);
+                gridY = static_cast<int>(pos.x - minCorner.x);
+                inSlice = true;
+            } else if (axis == 2 && std::abs(pos.z - slicePos) <= tolerance) { // Z-slice
+                gridX = static_cast<int>(pos.x - minCorner.x);
+                gridY = static_cast<int>(pos.y - minCorner.y);
+                inSlice = true;
+            }
+            
+            if (inSlice && gridX >= 0 && gridX < width && gridY >= 0 && gridY < height) {
+                const Vec4& color = getColor(id);
+                int index = (gridY * width + gridX) * 3;
+                
+                // Convert float color [0,1] to int [0,255]
+                rgbData[index] = static_cast<int>(color.r * 255);
+                rgbData[index + 1] = static_cast<int>(color.g * 255);
+                rgbData[index + 2] = static_cast<int>(color.b * 255);
+            }
+        }
+    }
+    
+    void getRegionAsRGB(float minX, float minY, float minZ, float maxX, float maxY, float maxZ,
+                       int& width, int& height, std::vector<int>& rgbData) const {
+        // For 3D, this creates a 2D projection (XY plane at average Z)
+        if (minX >= maxX || minY >= maxY || minZ >= maxZ) {
+            width = 0;
+            height = 0;
+            rgbData.clear();
+            return;
+        }
+        
+        // Calculate grid dimensions for XY projection
+        width = static_cast<int>(std::ceil(maxX - minX));
+        height = static_cast<int>(std::ceil(maxY - minY));
+        
+        // Initialize with black (0,0,0)
+        rgbData.resize(width * height * 3, 0);
+        
+        // Fill the grid with object colors in the region (XY projection)
+        for (const auto& posPair : positions) {
+            size_t id = posPair.first;
+            const Vec3& pos = posPair.second;
+            
+            // Check if position is within the region
+            if (pos.x >= minX && pos.x < maxX && 
+                pos.y >= minY && pos.y < maxY &&
+                pos.z >= minZ && pos.z < maxZ) {
+                
+                // Convert world position to grid coordinates (XY projection)
+                int gridX = static_cast<int>(pos.x - minX);
+                int gridY = static_cast<int>(pos.y - minY);
+                
+                if (gridX >= 0 && gridX < width && gridY >= 0 && gridY < height) {
+                    const Vec4& color = getColor(id);
+                    int index = (gridY * width + gridX) * 3;
+                    
+                    // Convert float color [0,1] to int [0,255]
+                    rgbData[index] = static_cast<int>(color.r * 255);
+                    rgbData[index + 1] = static_cast<int>(color.g * 255);
+                    rgbData[index + 2] = static_cast<int>(color.b * 255);
                 }
             }
         }
     }
     
-    // Create a sphere
+    void getRegionAsRGB(const Vec3& minCorner, const Vec3& maxCorner,
-    void fillSphere(const Vec3& center, float radius, const Vec4& color, float size = 1.0f) {
+                       int& width, int& height, std::vector<int>& rgbData) const {
-        int centerX = static_cast<int>(center.x);
+        getRegionAsRGB(minCorner.x, minCorner.y, minCorner.z, 
-        int centerY = static_cast<int>(center.y);
+                      maxCorner.x, maxCorner.y, maxCorner.z,
-        int centerZ = static_cast<int>(center.z);
+                      width, height, rgbData);
-        int radiusInt = static_cast<int>(radius);
-        
-        for (int z = centerZ - radiusInt; z <= centerZ + radiusInt; ++z) {
-            for (int y = centerY - radiusInt; y <= centerY + radiusInt; ++y) {
-                for (int x = centerX - radiusInt; x <= centerX + radiusInt; ++x) {
-                    float dx = x - center.x;
-                    float dy = y - center.y;
-                    float dz = z - center.z;
-                    if (dx * dx + dy * dy + dz * dz <= radius * radius) {
-                        if (!isOccupied(x, y, z)) {
-                            addVoxel(x, y, z, color, size);
-                        }
-                    }
-                }
-            }
-        }
     }
 
-    // Create a hollow sphere (just the surface)
+    // Spatial grid methods for 3D
-    void fillHollowSphere(const Vec3& center, float radius, const Vec4& color, float thickness = 1.0f, float size = 1.0f) {
+    std::tuple<int, int, int> worldToGrid(const Vec3& pos) const {
-        int centerX = static_cast<int>(center.x);
+        return {
-        int centerY = static_cast<int>(center.y);
+            static_cast<int>(std::floor(pos.x / cellSize)),
-        int centerZ = static_cast<int>(center.z);
+            static_cast<int>(std::floor(pos.y / cellSize)),
-        int radiusInt = static_cast<int>(radius);
+            static_cast<int>(std::floor(pos.z / cellSize))
-        
-        for (int z = centerZ - radiusInt; z <= centerZ + radiusInt; ++z) {
-            for (int y = centerY - radiusInt; y <= centerY + radiusInt; ++y) {
-                for (int x = centerX - radiusInt; x <= centerX + radiusInt; ++x) {
-                    float dx = x - center.x;
-                    float dy = y - center.y;
-                    float dz = z - center.z;
-                    float distance = std::sqrt(dx * dx + dy * dy + dz * dz);
-                    
-                    if (std::abs(distance - radius) <= thickness) {
-                        if (!isOccupied(x, y, z)) {
-                            addVoxel(x, y, z, color, size);
-                        }
-                    }
-                }
-            }
-        }
-    }
-    
-    // Create a cylinder
-    void fillCylinder(const Vec3& baseCenter, const Vec3& axis, float radius, float height, 
-                     const Vec4& color, float size = 1.0f) {
-        // Simplified cylinder aligned with Y-axis
-        Vec3 normalizedAxis = axis.normalized();
-        
-        for (int h = 0; h < static_cast<int>(height); ++h) {
-            Vec3 center = baseCenter + normalizedAxis * static_cast<float>(h);
-            
-            for (int y = -static_cast<int>(radius); y <= static_cast<int>(radius); ++y) {
-                for (int x = -static_cast<int>(radius); x <= static_cast<int>(radius); ++x) {
-                    if (x * x + y * y <= radius * radius) {
-                        Vec3 pos = center + Vec3(x, y, 0);
-                        if (!isOccupied(pos)) {
-                            addVoxel(pos, color, size);
-                        }
-                    }
-                }
-            }
-        }
-    }
-    
-    // Get neighbors of a voxel (6-connected)
-    std::vector<int> getNeighbors6(const Vec3& position) const {
-        std::vector<int> neighbors;
-        Vec3 offsets[] = {
-            Vec3(1, 0, 0), Vec3(-1, 0, 0),
-            Vec3(0, 1, 0), Vec3(0, -1, 0),
-            Vec3(0, 0, 1), Vec3(0, 0, -1)
         };
+    }
     
-        for (const auto& offset : offsets) {
+    void updateSpatialIndex(size_t id, const Vec3& oldPos, const Vec3& newPos) {
-            Vec3 neighborPos = position + offset;
+        auto oldCell = worldToGrid(oldPos);
-            int index = getVoxelIndex(neighborPos);
+        auto newCell = worldToGrid(newPos);
-            if (index != -1) {
+        
-                neighbors.push_back(index);
+        if (oldCell != newCell) {
+            // Remove from old cell
+            auto oldIt = spatialGrid.find(oldCell);
+            if (oldIt != spatialGrid.end()) {
+                oldIt->second.erase(id);
+                if (oldIt->second.empty()) {
+                    spatialGrid.erase(oldIt);
                 }
             }
             
-        return neighbors;
+            // Add to new cell
-    }
+            spatialGrid[newCell].insert(id);
-    
+            cellIndices[id] = newCell;
-    // Get neighbors of a voxel (26-connected)
-    std::vector<int> getNeighbors26(const Vec3& position) const {
-        std::vector<int> neighbors;
-        
-        for (int dz = -1; dz <= 1; ++dz) {
-            for (int dy = -1; dy <= 1; ++dy) {
-                for (int dx = -1; dx <= 1; ++dx) {
-                    if (dx == 0 && dy == 0 && dz == 0) continue;
-                    
-                    Vec3 neighborPos = position + Vec3(dx, dy, dz);
-                    int index = getVoxelIndex(neighborPos);
-                    if (index != -1) {
-                        neighbors.push_back(index);
-                    }
-                }
         }
     }
     
-        return neighbors;
+    std::vector<size_t> getIndicesInRadius(const Vec3& position, float radius) const {
-    }
+        std::vector<size_t> result;
         
-    // Create a simple teapot model (simplified)
+        Vec3 minPos(position.x - radius, position.y - radius, position.z - radius);
-    void createTeapot(const Vec3& position, float scale, const Vec4& color, float size = 1.0f) {
+        Vec3 maxPos(position.x + radius, position.y + radius, position.z + radius);
-        // This is a very simplified teapot representation
-        // In practice, you'd load a proper voxel model
         
-        // Teapot body (ellipsoid)
+        auto minCell = worldToGrid(minPos);
-        fillEllipsoid(position + Vec3(0, scale * 0.3f, 0), 
+        auto maxCell = worldToGrid(maxPos);
-                     Vec3(scale * 0.4f, scale * 0.3f, scale * 0.4f), color, size);
         
-        // Teapot lid (smaller ellipsoid on top)
+        float radiusSq = radius * radius;
-        fillEllipsoid(position + Vec3(0, scale * 0.6f, 0), 
-                     Vec3(scale * 0.3f, scale * 0.1f, scale * 0.3f), color, size);
         
-        // Teapot spout (cylinder)
+        // Only check relevant cells
-        fillCylinder(position + Vec3(scale * 0.3f, scale * 0.2f, 0), 
+        for (int x = std::get<0>(minCell); x <= std::get<0>(maxCell); ++x) {
-                    Vec3(1, 0.2f, 0), scale * 0.05f, scale * 0.3f, color, size);
+            for (int y = std::get<1>(minCell); y <= std::get<1>(maxCell); ++y) {
-        
+                for (int z = std::get<2>(minCell); z <= std::get<2>(maxCell); ++z) {
-        // Teapot handle (torus segment)
+                    auto cell = std::make_tuple(x, y, z);
-        fillTorusSegment(position + Vec3(-scale * 0.3f, scale * 0.3f, 0), 
+                    auto it = spatialGrid.find(cell);
-                        Vec3(0, 1, 0), scale * 0.1f, scale * 0.2f, color, size);
+                    if (it != spatialGrid.end()) {
-    }
+                        for (size_t id : it->second) {
-    
+                            const Vec3& objPos = getPosition(id);
-    // Fill an ellipsoid
+                            float dx = objPos.x - position.x;
-    void fillEllipsoid(const Vec3& center, const Vec3& radii, const Vec4& color, float size = 1.0f) {
+                            float dy = objPos.y - position.y;
-        int radiusX = static_cast<int>(radii.x);
+                            float dz = objPos.z - position.z;
-        int radiusY = static_cast<int>(radii.y);
+                            if (dx * dx + dy * dy + dz * dz <= radiusSq) {
-        int radiusZ = static_cast<int>(radii.z);
+                                result.push_back(id);
-        
-        for (int z = -radiusZ; z <= radiusZ; ++z) {
-            for (int y = -radiusY; y <= radiusY; ++y) {
-                for (int x = -radiusX; x <= radiusX; ++x) {
-                    float normalizedX = static_cast<float>(x) / radii.x;
-                    float normalizedY = static_cast<float>(y) / radii.y;
-                    float normalizedZ = static_cast<float>(z) / radii.z;
-                    
-                    if (normalizedX * normalizedX + normalizedY * normalizedY + normalizedZ * normalizedZ <= 1.0f) {
-                        Vec3 pos = center + Vec3(x, y, z);
-                        if (!isOccupied(pos)) {
-                            addVoxel(pos, color, size);
                             }
                         }
                     }
@@ -298,89 +356,73 @@ public:
             }
         }
         
-    // Fill a torus segment
+        return result;
-    void fillTorusSegment(const Vec3& center, const Vec3& axis, float majorRadius, float minorRadius, 
-                         const Vec4& color, float size = 1.0f) {
-        Vec3 normalizedAxis = axis.normalized();
-        
-        // Simplified torus - in practice you'd use proper torus equation
-        for (float angle = 0; angle < 2 * M_PI; angle += 0.2f) {
-            Vec3 circleCenter = center + Vec3(std::cos(angle) * majorRadius, 0, std::sin(angle) * majorRadius);
-            fillSphere(circleCenter, minorRadius, color, size);
-        }
     }
     
-    // Find connected components in 3D
+    std::vector<size_t> getIndicesInRegion(const Vec3& minCorner, const Vec3& maxCorner) const {
-    std::vector<std::vector<int>> findConnectedComponents() const {
+        std::vector<size_t> result;
-        std::vector<std::vector<int>> components;
-        std::unordered_map<Vec3, bool, std::hash<Vec3>> visited;
         
-        for (size_t i = 0; i < positions.size(); ++i) {
+        auto minCell = worldToGrid(minCorner);
-            const Vec3& pos = positions[i];
+        auto maxCell = worldToGrid(maxCorner);
-            if (visited.find(pos) == visited.end()) {
-                std::vector<int> component;
-                floodFill3D(pos, visited, component);
-                components.push_back(component);
-            }
-        }
         
-        return components;
+        for (int x = std::get<0>(minCell); x <= std::get<0>(maxCell); ++x) {
-    }
+            for (int y = std::get<1>(minCell); y <= std::get<1>(maxCell); ++y) {
-    
+                for (int z = std::get<2>(minCell); z <= std::get<2>(maxCell); ++z) {
-    // Getters
+                    auto cell = std::make_tuple(x, y, z);
-    const std::vector<Vec3>& getPositions() const { return positions; }
+                    auto it = spatialGrid.find(cell);
-    const std::vector<Vec4>& getColors() const { return colors; }
+                    if (it != spatialGrid.end()) {
-    const std::vector<float>& getSizes() const { return sizes; }
+                        for (size_t id : it->second) {
-    
+                            const Vec3& pos = getPosition(id);
-    Vec3 getPosition(int index) const { return positions[index]; }
+                            if (pos.x >= minCorner.x && pos.x <= maxCorner.x &&
-    Vec4 getColor(int index) const { return colors[index]; }
+                                pos.y >= minCorner.y && pos.y <= maxCorner.y &&
-    float getSize(int index) const { return sizes[index]; }
+                                pos.z >= minCorner.z && pos.z <= maxCorner.z) {
-    
+                                result.push_back(id);
-    void setColor(int index, const Vec4& color) { colors[index] = color; }
-    void setSize(int index, float size) { sizes[index] = size; }
-    
-    int getWidth() const { return width; }
-    int getHeight() const { return height; }
-    int getDepth() const { return depth; }
-
-private:
-    std::vector<Vec3> positions;
-    std::vector<Vec4> colors;
-    std::vector<float> sizes;
-    std::unordered_map<Vec3, int, std::hash<Vec3>> positionToIndex;
-    int width, height, depth;
-    
-    void floodFill3D(const Vec3& start, std::unordered_map<Vec3, bool, std::hash<Vec3>>& visited, 
-                     std::vector<int>& component) const {
-        std::vector<Vec3> stack;
-        stack.push_back(start);
-        
-        while (!stack.empty()) {
-            Vec3 current = stack.back();
-            stack.pop_back();
-            
-            if (visited.find(current) != visited.end()) continue;
-            
-            visited[current] = true;
-            int index = getVoxelIndex(current);
-            if (index != -1) {
-                component.push_back(index);
-                
-                // Add 6-connected neighbors
-                Vec3 neighbors[] = {
-                    current + Vec3(1, 0, 0), current + Vec3(-1, 0, 0),
-                    current + Vec3(0, 1, 0), current + Vec3(0, -1, 0),
-                    current + Vec3(0, 0, 1), current + Vec3(0, 0, -1)
-                };
-                
-                for (const auto& neighbor : neighbors) {
-                    if (isOccupied(neighbor) && visited.find(neighbor) == visited.end()) {
-                        stack.push_back(neighbor);
                             }
                         }
                     }
                 }
             }
+        }
+        
+        return result;
+    }
+
+    size_t getSpatialGridCellCount() const { return spatialGrid.size(); }
+    size_t getSpatialGridObjectCount() const { return cellIndices.size(); }
+    float getCellSize() const { return cellSize; }
+    
+    // 3D-specific utility methods
+    size_t getVoxelCount() const { return positions.size(); }
+    
+    // Get density information (useful for volume rendering)
+    std::vector<float> getDensityGrid(int resX, int resY, int resZ) const {
+        std::vector<float> density(resX * resY * resZ, 0.0f);
+        
+        Vec3 minCorner, maxCorner;
+        getBoundingBox(minCorner, maxCorner);
+        
+        Vec3 gridSize = maxCorner - minCorner;
+        if (gridSize.x <= 0 || gridSize.y <= 0 || gridSize.z <= 0) {
+            return density;
+        }
+        
+        Vec3 voxelSize(gridSize.x / resX, gridSize.y / resY, gridSize.z / resZ);
+        
+        for (const auto& posPair : positions) {
+            const Vec3& pos = posPair.second;
+            
+            // Convert to grid coordinates
+            int gx = static_cast<int>((pos.x - minCorner.x) / gridSize.x * resX);
+            int gy = static_cast<int>((pos.y - minCorner.y) / gridSize.y * resY);
+            int gz = static_cast<int>((pos.z - minCorner.z) / gridSize.z * resZ);
+            
+            if (gx >= 0 && gx < resX && gy >= 0 && gy < resY && gz >= 0 && gz < resZ) {
+                density[gz * resX * resY + gy * resX + gx] += 1.0f;
+            }
+        }
+        
+        return density;
+    }
 };
 
 #endif