From ffa2d7ef36bb27175c15c56cc5afafe84444cda9 Mon Sep 17 00:00:00 2001 From: Yggdrasil75 Date: Wed, 12 Nov 2025 07:57:43 -0500 Subject: [PATCH] spatc16 (16x16 spatial subgrid for fixed size optimizations) --- tests/g2chromatic.cpp | 8 +- tests/g2chromaticsimd.cpp | 15 +- util/grid/grid2.hpp | 1066 +++++++++++++++++++++++-------------- util/grid/grid2fast.hpp | 316 +++++++++++ util/grid/spatc16.hpp | 233 ++++++++ 5 files changed, 1211 insertions(+), 427 deletions(-) create mode 100644 util/grid/grid2fast.hpp create mode 100644 util/grid/spatc16.hpp diff --git a/tests/g2chromatic.cpp b/tests/g2chromatic.cpp index 195313a..382e7ef 100644 --- a/tests/g2chromatic.cpp +++ b/tests/g2chromatic.cpp @@ -8,9 +8,9 @@ #include "../util/timing_decorator.cpp" struct AnimationConfig { - int width = 512; - int height = 512; - int totalFrames = 240; + int width = 1024; + int height = 1024; + int totalFrames = 480; float fps = 30.0f; int numSeeds = 1; }; @@ -99,7 +99,7 @@ std::vector convertFrameToBGR(Grid2& grid, const AnimationConfig& confi TIME_FUNCTION; int frameWidth, frameHeight; std::vector bgrData; - grid.getGridRegionAsBGR(Vec2(0,0),Vec2(512,512), frameWidth, frameHeight, bgrData); + grid.getGridRegionAsBGR(Vec2(0,0),Vec2(config.width,config.height), frameWidth, frameHeight, bgrData); //grid.getGridRegionAsRGB(0.0f,0.0f,512.0f,512.0f,frameWidth,frameHeight,rgbData); std::vector bgrFrame(frameWidth * frameHeight * 3); diff --git a/tests/g2chromaticsimd.cpp b/tests/g2chromaticsimd.cpp index 366ebb6..ced9d1f 100644 --- a/tests/g2chromaticsimd.cpp +++ b/tests/g2chromaticsimd.cpp @@ -9,8 +9,8 @@ #include "../util/timing_decorator.cpp" struct AnimationConfig { - int width = 512; - int height = 512; + int width = 1024; + int height = 1024; int totalFrames = 240; float fps = 30.0f; int numSeeds = 1; @@ -223,9 +223,7 @@ std::vector convertFrameToBGR(Grid2& grid, const AnimationConfig& confi } -std::vector> createAnimationFrames(Grid2& grid, - const SeedDataSoA& seeds, - const AnimationConfig& config) { +std::vector> createAnimationFrames(Grid2& grid, const SeedDataSoA& seeds, const AnimationConfig& config) { TIME_FUNCTION; std::vector> frames; @@ -241,10 +239,9 @@ std::vector> createAnimationFrames(Grid2& grid, return frames; } -void printSuccessMessage(const std::string& filename, - const std::vector& seedPoints, - const std::vector& seedColors, - const AnimationConfig& config) { + +void printSuccessMessage(const std::string& filename, const std::vector& seedPoints, + const std::vector& seedColors, const AnimationConfig& config) { std::cout << "\nSuccessfully saved chromatic transformation animation to: " << filename << std::endl; std::cout << "Video details:" << std::endl; std::cout << " - Dimensions: " << config.width << " x " << config.height << std::endl; diff --git a/util/grid/grid2.hpp b/util/grid/grid2.hpp index 52a7d78..ac2ab84 100644 --- a/util/grid/grid2.hpp +++ b/util/grid/grid2.hpp @@ -4,6 +4,7 @@ #include "../vectorlogic/vec2.hpp" #include "../vectorlogic/vec4.hpp" #include "../timing_decorator.hpp" +#include "spatc16.hpp" #include #include #include @@ -11,6 +12,7 @@ #include #include #include +#include struct PairHash { template @@ -23,74 +25,372 @@ struct PairHash { class Grid2 { private: - // Changed from multimap to unordered_map - each ID can only have one position/color/size - std::unordered_map positions; - std::unordered_map colors; - std::unordered_map sizes; + // Objects that don't fit in any SpatialCell16x16 + std::unordered_map loosePositions; + std::unordered_map looseColors; + std::unordered_map looseSizes; + + // Spatial partitioning using SpatialCell16x16 cells + std::unordered_map, std::unique_ptr, PairHash> spatialCells; + Vec2 gridMin, gridMax; + bool useSpatialPartitioning; size_t next_id; - std::unordered_map> cellIndices; - std::unordered_map, std::unordered_set, PairHash> spatialGrid; - float cellSize; + // Convert world position to cell coordinates for SpatialCell16x16 placement + std::pair worldToCellCoord(const Vec2& worldPos) const { + if (!useSpatialPartitioning) return {0, 0}; + + float cellWorldSize = getCellWorldSize(); + int cellX = static_cast((worldPos.x - gridMin.x) / cellWorldSize); + int cellY = static_cast((worldPos.y - gridMin.y) / cellWorldSize); + return {cellX, cellY}; + } + + // Get the world size of each SpatialCell16x16 cell + float getCellWorldSize() const { + // SpatialCell16x16 is 16x16 internally, but we need world size + return (gridMax.x - gridMin.x) / getGridCellsX(); + } + + // Calculate how many SpatialCell16x16 cells fit in X direction + int getGridCellsX() const { + if (!useSpatialPartitioning) return 1; + float worldWidth = gridMax.x - gridMin.x; + // Aim for cells that are roughly square in world coordinates + float targetCellSize = std::max(16.0f, worldWidth / 16.0f); + return std::max(1, static_cast(worldWidth / targetCellSize)); + } + + // Calculate how many SpatialCell16x16 cells fit in Y direction + int getGridCellsY() const { + if (!useSpatialPartitioning) return 1; + float worldHeight = gridMax.y - gridMin.y; + float targetCellSize = std::max(16.0f, worldHeight / 16.0f); + return std::max(1, static_cast(worldHeight / targetCellSize)); + } + + // Get cell bounds from coordinates + Vec2 getCellMinCorner(int cellX, int cellY) const { + float cellSizeX = (gridMax.x - gridMin.x) / getGridCellsX(); + float cellSizeY = (gridMax.y - gridMin.y) / getGridCellsY(); + return Vec2( + gridMin.x + cellX * cellSizeX, + gridMin.y + cellY * cellSizeY + ); + } + + Vec2 getCellMaxCorner(int cellX, int cellY) const { + float cellSizeX = (gridMax.x - gridMin.x) / getGridCellsX(); + float cellSizeY = (gridMax.y - gridMin.y) / getGridCellsY(); + return Vec2( + gridMin.x + (cellX + 1) * cellSizeX, + gridMin.y + (cellY + 1) * cellSizeY + ); + } + + // Get or create spatial cell + SpatialCell16x16* getOrCreateCell(int cellX, int cellY) { + auto key = std::make_pair(cellX, cellY); + auto it = spatialCells.find(key); + if (it != spatialCells.end()) { + return it->second.get(); + } + + // Create new SpatialCell16x16 + Vec2 minCorner = getCellMinCorner(cellX, cellY); + Vec2 maxCorner = getCellMaxCorner(cellX, cellY); + spatialCells[key] = std::make_unique(minCorner, maxCorner); + return spatialCells[key].get(); + } + + // Check if an object can fit in SpatialCell16x16 + bool canFitInSpatialCell(const Vec2& position, float size) const { + if (!useSpatialPartitioning) return false; + + auto cellCoord = worldToCellCoord(position); + Vec2 cellMin = getCellMinCorner(cellCoord.first, cellCoord.second); + Vec2 cellMax = getCellMaxCorner(cellCoord.first, cellCoord.second); + + float halfSize = size * 0.5f; + return (position.x - halfSize >= cellMin.x && + position.x + halfSize <= cellMax.x && + position.y - halfSize >= cellMin.y && + position.y + halfSize <= cellMax.y); + } + + // Add object to appropriate storage + bool addToSpatialCell(size_t id, const Vec2& position, const Vec4& color, float size) { + if (!useSpatialPartitioning || !canFitInSpatialCell(position, size)) { + return false; + } + + auto cellCoord = worldToCellCoord(position); + SpatialCell16x16* cell = getOrCreateCell(cellCoord.first, cellCoord.second); + return cell->addObject(id, position, color, size); + } + + // Get all cells that overlap with a circle + std::vector getCellsInRadius(const Vec2& center, float radius) const { + std::vector result; + if (!useSpatialPartitioning) return result; + + Vec2 searchMin(center.x - radius, center.y - radius); + Vec2 searchMax(center.x + radius, center.y + radius); + + auto [minCellX, minCellY] = worldToCellCoord(searchMin); + auto [maxCellX, maxCellY] = worldToCellCoord(searchMax); + + for (int cellY = minCellY; cellY <= maxCellY; ++cellY) { + for (int cellX = minCellX; cellX <= maxCellX; ++cellX) { + auto key = std::make_pair(cellX, cellY); + auto it = spatialCells.find(key); + if (it != spatialCells.end()) { + result.push_back(it->second.get()); + } + } + } + + return result; + } + + // Get all cells that overlap with a region + std::vector getCellsInRegion(const Vec2& minCorner, const Vec2& maxCorner) const { + std::vector result; + if (!useSpatialPartitioning) return result; + + auto [minCellX, minCellY] = worldToCellCoord(minCorner); + auto [maxCellX, maxCellY] = worldToCellCoord(maxCorner); + + for (int cellY = minCellY; cellY <= maxCellY; ++cellY) { + for (int cellX = minCellX; cellX <= maxCellX; ++cellX) { + auto key = std::make_pair(cellX, cellY); + auto it = spatialCells.find(key); + if (it != spatialCells.end()) { + result.push_back(it->second.get()); + } + } + } + + return result; + } + + // Remove object from spatial cells + void removeFromSpatialCells(size_t id, const Vec2& position, float size) { + if (!useSpatialPartitioning) return; + + // Calculate object bounds + float halfSize = size * 0.5f; + Vec2 objMin(position.x - halfSize, position.y - halfSize); + Vec2 objMax(position.x + halfSize, position.y + halfSize); + + // Get affected cells + auto [minCellX, minCellY] = worldToCellCoord(objMin); + auto [maxCellX, maxCellY] = worldToCellCoord(objMax); + + // Remove from all overlapping cells + for (int cellY = minCellY; cellY <= maxCellY; ++cellY) { + for (int cellX = minCellX; cellX <= maxCellX; ++cellX) { + auto key = std::make_pair(cellX, cellY); + auto it = spatialCells.find(key); + if (it != spatialCells.end()) { + it->second->removeObject(id); + } + } + } + } + + // Check if object is in spatial cells + bool isInSpatialCells(size_t id) const { + if (!useSpatialPartitioning) return false; + + for (const auto& cellPair : spatialCells) { + if (cellPair.second->hasObject(id)) { + return true; + } + } + return false; + } + + // Get object position (handles both storage types) + Vec2 getObjectPosition(size_t id) const { + // Check spatial cells first + if (useSpatialPartitioning) { + for (const auto& cellPair : spatialCells) { + if (cellPair.second->hasObject(id)) { + return cellPair.second->getPosition(id); + } + } + } + + // Check loose objects + auto it = loosePositions.find(id); + return it != loosePositions.end() ? it->second : Vec2(); + } + + // Get object color (handles both storage types) + Vec4 getObjectColor(size_t id) const { + // Check spatial cells first + if (useSpatialPartitioning) { + for (const auto& cellPair : spatialCells) { + if (cellPair.second->hasObject(id)) { + return cellPair.second->getColor(id); + } + } + } + + // Check loose objects + auto it = looseColors.find(id); + return it != looseColors.end() ? it->second : Vec4(); + } + + // Get object size (handles both storage types) + float getObjectSize(size_t id) const { + // Check spatial cells first + if (useSpatialPartitioning) { + for (const auto& cellPair : spatialCells) { + if (cellPair.second->hasObject(id)) { + return cellPair.second->getSize(id); + } + } + } + + // Check loose objects + auto it = looseSizes.find(id); + return it != looseSizes.end() ? it->second : 1.0f; + } public: - Grid2() : next_id(0), cellSize(1.0f) {} - Grid2(float cellSize) : next_id(0), cellSize(cellSize) {} + Grid2() : next_id(0), useSpatialPartitioning(false) {} - size_t addObject(const Vec2& position, const Vec4& color, float size = 1.0f) { - size_t id = next_id++; - positions[id] = position; // Direct assignment instead of insert - colors[id] = color; - sizes[id] = size; - std::pair cell = worldToGrid(position); - spatialGrid[cell].insert(id); - cellIndices[id] = cell; - return id; - } - - //gets - much simpler now - Vec2 getPosition(size_t id) const { - auto it = positions.find(id); - if (it != positions.end()) return it->second; - return Vec2(); - } - - Vec4 getColor(size_t id) const { - auto it = colors.find(id); - if (it != colors.end()) return it->second; - return Vec4(); - } - - float getSize(size_t id) const { - auto it = sizes.find(id); - if (it != sizes.end()) return it->second; - return 1.0f; - } - - //sets - simpler too - void setPosition(size_t id, const Vec2& position) { - if (!hasObject(id)) return; + // Initialize with spatial partitioning + void initializeSpatialPartitioning(const Vec2& minCorner, const Vec2& maxCorner) { + gridMin = minCorner; + gridMax = maxCorner; + spatialCells.clear(); + useSpatialPartitioning = true; - Vec2 oldPos = positions[id]; // Direct access - positions[id] = position; // Direct assignment - updateSpatialIndex(id, oldPos, position); - } - - void setColor(size_t id, const Vec4& color) { - colors[id] = color; // Direct assignment - } - - void setSize(size_t id, float size) { - sizes[id] = size; // Direct assignment + // Move existing objects to appropriate storage + std::vector toRemove; + for (const std::pair& pair : loosePositions) { + size_t id = pair.first; + const Vec2& pos = pair.second; + const Vec4& color = looseColors[id]; + float size = looseSizes[id]; + + if (addToSpatialCell(id, pos, color, size)) { + toRemove.push_back(id); + } + } + + // Remove objects that were moved to spatial cells + for (size_t id : toRemove) { + loosePositions.erase(id); + looseColors.erase(id); + looseSizes.erase(id); + } + } + + void initializeSpatialPartitioning(float minX, float minY, float maxX, float maxY) { + initializeSpatialPartitioning(Vec2(minX, minY), Vec2(maxX, maxY)); } - // Batch add/remove operations void addObjects(const std::vector>& objects) { for (const auto& obj : objects) { addObject(std::get<0>(obj), std::get<1>(obj), std::get<2>(obj)); } } + + size_t addObject(const Vec2& position, const Vec4& color, float size = 1.0f) { + size_t id = next_id++; + + // Try to add to spatial cell first + if (!addToSpatialCell(id, position, color, size)) { + // Fall back to loose storage + loosePositions[id] = position; + looseColors[id] = color; + looseSizes[id] = size; + } + + return id; + } + + // Simple getters + Vec2 getPosition(size_t id) const { + return getObjectPosition(id); + } + + Vec4 getColor(size_t id) const { + return getObjectColor(id); + } + + float getSize(size_t id) const { + return getObjectSize(id); + } + + void setPosition(size_t id, const Vec2& position) { + if (!hasObject(id)) return; + + Vec2 oldPos = getObjectPosition(id); + float size = getObjectSize(id); + + // Remove from current storage + if (isInSpatialCells(id)) { + removeFromSpatialCells(id, oldPos, size); + } else { + loosePositions.erase(id); + } + + // Add to appropriate storage + if (!addToSpatialCell(id, position, getObjectColor(id), size)) { + loosePositions[id] = position; + } + } + + void setColor(size_t id, const Vec4& color) { + if (!hasObject(id)) return; + + if (isInSpatialCells(id)) { + // Update in spatial cell + Vec2 pos = getObjectPosition(id); + float size = getObjectSize(id); + removeFromSpatialCells(id, pos, size); + addToSpatialCell(id, pos, color, size); + } else { + // Update in loose storage + looseColors[id] = color; + } + } + + void setSize(size_t id, float size) { + if (!hasObject(id)) return; + + Vec2 pos = getObjectPosition(id); + Vec4 color = getObjectColor(id); + float oldSize = getObjectSize(id); + + // Remove from current storage + if (isInSpatialCells(id)) { + removeFromSpatialCells(id, pos, oldSize); + } else { + loosePositions.erase(id); + looseColors.erase(id); + looseSizes.erase(id); + } + + // Add to appropriate storage with new size + if (!addToSpatialCell(id, pos, color, size)) { + loosePositions[id] = pos; + looseColors[id] = color; + looseSizes[id] = size; + } + } + + bool hasObject(size_t id) const { + if (isInSpatialCells(id)) return true; + return loosePositions.find(id) != loosePositions.end(); + } + void removeObjects(const std::vector& ids) { for (size_t id : ids) { @@ -98,404 +398,342 @@ public: } } - // Batch position updates - void updatePositions(const std::unordered_map& newPositions) { - std::vector> spatialUpdates; - - for (const auto& pair : newPositions) { - if (hasObject(pair.first)) { - Vec2 oldPos = positions[pair.first]; - positions[pair.first] = pair.second; - spatialUpdates.emplace_back(pair.first, oldPos, pair.second); - } - } - - for (const auto& update : spatialUpdates) { - updateSpatialIndex(std::get<0>(update), std::get<1>(update), std::get<2>(update)); - } - } - - std::vector getAllObjectIds() const { - std::vector ids; - ids.reserve(positions.size()); - for (const auto& pair : positions) { - ids.push_back(pair.first); - } - std::sort(ids.begin(), ids.end()); - return ids; - } - - void bulkUpdateColors(const std::function& colorFunc) { - TIME_FUNCTION; - - // Convert to contiguous storage for better performance - std::vector> objectData; - objectData.reserve(positions.size()); - - // Single pass to collect all data - for (const auto& posPair : positions) { - size_t id = posPair.first; - auto colorIt = colors.find(id); - if (colorIt != colors.end()) { - objectData.emplace_back(id, posPair.second, colorIt->second); - } - } - - // Parallel color computation - std::vector> updates; - updates.resize(objectData.size()); - - #pragma omp parallel for - for (size_t i = 0; i < objectData.size(); ++i) { - const auto& [id, pos, currentColor] = objectData[i]; - Vec4 newColor = colorFunc(id, pos, currentColor); - updates[i] = {id, newColor}; - } - - // Batch update colors - much more efficient with unordered_map - for (const auto& update : updates) { - colors[update.first] = update.second; // Direct assignment - } - } - - //other - bool hasObject(size_t id) const { - return positions.find(id) != positions.end(); - } - void removeObject(size_t id) { - // Remove from spatial grid first - 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); - } + if (!hasObject(id)) return; - // Remove from data maps - positions.erase(id); - colors.erase(id); - sizes.erase(id); - } - - std::vector getIndicesAt(float x, float y, float radius = 0.0f) const { - return getIndicesAt(Vec2(x, y), radius); + Vec2 pos = getObjectPosition(id); + float size = getObjectSize(id); + + if (isInSpatialCells(id)) { + removeFromSpatialCells(id, pos, size); + } else { + loosePositions.erase(id); + looseColors.erase(id); + looseSizes.erase(id); + } } std::vector getIndicesAt(const Vec2& position, float radius = 0.0f) const { TIME_FUNCTION; - std::vector result; - if (radius <= 0.0f) { - // Exact position match - for (const auto& pair : positions) { - if (pair.second == position) { - result.push_back(pair.first); - } + std::unordered_set result; + + // Query spatial cells + if (useSpatialPartitioning && radius > 0.0f) { + auto cells = getCellsInRadius(position, radius); + for (SpatialCell16x16* cell : cells) { + auto objects = cell->getObjectsInRadius(position, radius); + result.insert(objects.begin(), objects.end()); } - } 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); + } else if (useSpatialPartitioning && radius == 0.0f) { + auto cellCoord = worldToCellCoord(position); + auto key = std::make_pair(cellCoord.first, cellCoord.second); + auto it = spatialCells.find(key); + if (it != spatialCells.end()) { + auto objects = it->second->getObjectsAt(position); + result.insert(objects.begin(), objects.end()); + } + } + + // Query loose objects + float radius_sq = radius * radius; + for (const auto& pair : loosePositions) { + float dx = pair.second.x - position.x; + float dy = pair.second.y - position.y; + if (dx * dx + dy * dy <= radius_sq) { + result.insert(pair.first); + } + } + + return std::vector(result.begin(), result.end()); + } + + void updatePositions(const std::unordered_map& newPositions) { + TIME_FUNCTION; + + for (const auto& [id, newPos] : newPositions) { + if (!hasObject(id)) continue; + + Vec2 oldPos = getObjectPosition(id); + Vec4 color = getObjectColor(id); + float size = getObjectSize(id); + + // Remove from current storage + if (isInSpatialCells(id)) { + removeFromSpatialCells(id, oldPos, size); + } else { + loosePositions.erase(id); + } + + // Add to appropriate storage with new position + if (!addToSpatialCell(id, newPos, color, size)) { + loosePositions[id] = newPos; + } + } + } + + // Get all object IDs in the grid + std::vector getAllObjectIds() const { + TIME_FUNCTION; + + std::unordered_set result; + + // Get IDs from spatial cells + if (useSpatialPartitioning) { + for (const auto& cellPair : spatialCells) { + auto cellIds = cellPair.second->getAllObjectIds(); + result.insert(cellIds.begin(), cellIds.end()); + } + } + + // Get IDs from loose objects + for (const auto& pair : loosePositions) { + result.insert(pair.first); + } + + return std::vector(result.begin(), result.end()); + } + + // Bulk update colors using a function + void bulkUpdateColors(const std::function& colorFunc) { + TIME_FUNCTION; + + // Update colors in spatial cells + if (useSpatialPartitioning) { + for (auto& cellPair : spatialCells) { + auto cell = cellPair.second.get(); + auto ids = cell->getAllObjectIds(); + + for (size_t id : ids) { + Vec2 pos = cell->getPosition(id); + Vec4 currentColor = cell->getColor(id); + Vec4 newColor = colorFunc(id, pos, currentColor); + + // Remove and re-add with new color + float size = cell->getSize(id); + cell->removeObject(id); + cell->addObject(id, pos, newColor, size); } } } - return result; + // Update colors in loose objects + for (auto& pair : looseColors) { + size_t id = pair.first; + Vec2 pos = loosePositions[id]; + Vec4 currentColor = pair.second; + Vec4 newColor = colorFunc(id, pos, currentColor); + pair.second = newColor; + } } - - void getBoundingBox(Vec2& minCorner, Vec2& maxCorner) const { + + // Get grid region as RGB data (for visualization/export) + void getGridRegionAsRGB(const Vec2& minCorner, const Vec2& maxCorner, + int& width, int& height, std::vector& rgbData) const { TIME_FUNCTION; - if (positions.empty()) { - minCorner = Vec2(0.0f, 0.0f); - maxCorner = Vec2(0.0f, 0.0f); + + // Determine output dimensions + width = static_cast(maxCorner.x - minCorner.x); + height = static_cast(maxCorner.y - minCorner.y); + + if (width <= 0 || height <= 0) { + width = height = 0; + rgbData.clear(); return; } - auto it = positions.begin(); - minCorner = it->second; - maxCorner = it->second; + // Initialize output data (3 channels per pixel) + rgbData.resize(width * height * 3, 0); - for (const auto& pair : positions) { + // Get objects in the region + std::vector objectIds; + + if (useSpatialPartitioning) { + // Use spatial partitioning for efficient query + auto cells = getCellsInRegion(minCorner, maxCorner); + for (SpatialCell16x16* cell : cells) { + auto cellObjects = cell->getObjectsInRegion(minCorner, maxCorner); + objectIds.insert(objectIds.end(), cellObjects.begin(), cellObjects.end()); + } + } else { + // Fall back to checking all loose objects + for (const auto& pair : loosePositions) { + const Vec2& pos = pair.second; + if (pos.x >= minCorner.x && pos.x <= maxCorner.x && + pos.y >= minCorner.y && pos.y <= maxCorner.y) { + objectIds.push_back(pair.first); + } + } + } + + // Rasterize objects to RGB + for (size_t id : objectIds) { + Vec2 pos = getObjectPosition(id); + Vec4 color = getObjectColor(id); + float size = getObjectSize(id); + + // Convert world coordinates to pixel coordinates + int pixelX = static_cast((pos.x - minCorner.x)); + int pixelY = static_cast((pos.y - minCorner.y)); + + // Clamp to image bounds + pixelX = std::clamp(pixelX, 0, width - 1); + pixelY = std::clamp(pixelY, 0, height - 1); + + // Convert normalized color to 8-bit RGB + int r = static_cast(std::clamp(color.x, 0.0f, 1.0f) * 255); + int g = static_cast(std::clamp(color.y, 0.0f, 1.0f) * 255); + int b = static_cast(std::clamp(color.z, 0.0f, 1.0f) * 255); + + // Set pixel color (RGB order) + int index = (pixelY * width + pixelX) * 3; + rgbData[index] = r; + rgbData[index + 1] = g; + rgbData[index + 2] = b; + } + } + + // Get grid region as BGR data (OpenCV compatible) + void getGridRegionAsBGR(const Vec2& minCorner, const Vec2& maxCorner, + int& width, int& height, std::vector& bgrData) const { + TIME_FUNCTION; + + // First get as RGB + std::vector rgbData; + getGridRegionAsRGB(minCorner, maxCorner, width, height, rgbData); + + if (width <= 0 || height <= 0) { + bgrData.clear(); + return; + } + + // Convert RGB to BGR by swapping channels + bgrData.resize(rgbData.size()); + for (int i = 0; i < width * height; ++i) { + int rgbIndex = i * 3; + int bgrIndex = i * 3; + bgrData[bgrIndex] = rgbData[rgbIndex + 2]; // B <- R + bgrData[bgrIndex + 1] = rgbData[rgbIndex + 1]; // G <- G + bgrData[bgrIndex + 2] = rgbData[rgbIndex]; // R <- B + } + } + + void getBoundingBox(Vec2& minCorner, Vec2& maxCorner) const { + TIME_FUNCTION; + + bool hasObjects = false; + minCorner = Vec2(std::numeric_limits::max(), std::numeric_limits::max()); + maxCorner = Vec2(std::numeric_limits::lowest(), std::numeric_limits::lowest()); + + // Process spatial cells + if (useSpatialPartitioning) { + for (const auto& cellPair : spatialCells) { + auto ids = cellPair.second->getAllObjectIds(); + for (size_t id : ids) { + const Vec2& pos = cellPair.second->getPosition(id); + float size = cellPair.second->getSize(id); + float halfSize = size * 0.5f; + + minCorner.x = std::min(minCorner.x, pos.x - halfSize); + minCorner.y = std::min(minCorner.y, pos.y - halfSize); + maxCorner.x = std::max(maxCorner.x, pos.x + halfSize); + maxCorner.y = std::max(maxCorner.y, pos.y + halfSize); + hasObjects = true; + } + } + } + + // Process loose objects + for (const auto& pair : loosePositions) { const Vec2& pos = pair.second; - float size = getSize(pair.first); + float size = looseSizes.at(pair.first); float halfSize = size * 0.5f; minCorner.x = std::min(minCorner.x, pos.x - halfSize); minCorner.y = std::min(minCorner.y, pos.y - halfSize); maxCorner.x = std::max(maxCorner.x, pos.x + halfSize); maxCorner.y = std::max(maxCorner.y, pos.y + halfSize); + hasObjects = true; } + + if (!hasObjects) { + minCorner = Vec2(0.0f, 0.0f); + maxCorner = Vec2(0.0f, 0.0f); + } + } + + // Spatial grid statistics + size_t getSpatialGridCellCount() const { + return useSpatialPartitioning ? spatialCells.size() : 0; } - //to picture - void getGridRegionAsRGB(const Vec2& minCorner, const Vec2& maxCorner, - int& width, int& height, std::vector& rgbData) const { - TIME_FUNCTION; + size_t getSpatialGridObjectCount() const { + if (!useSpatialPartitioning) return 0; - // Ensure valid region - if (minCorner.x >= maxCorner.x || minCorner.y >= maxCorner.y) { - width = 0; - height = 0; - rgbData.clear(); - return; + size_t total = 0; + for (const auto& pair : spatialCells) { + total += pair.second->getObjectCount(); } - - // Calculate grid dimensions - width = static_cast(std::ceil(maxCorner.x - minCorner.x)); - height = static_cast(std::ceil(maxCorner.y - minCorner.y)); - - // Initialize with black (0,0,0) in BGR format - rgbData.resize(width * height * 3, 0); - - // Fill the grid with object colors in the region, accounting for sizes - for (const auto& posPair : positions) { - size_t id = posPair.first; - const Vec2& pos = posPair.second; - float size = getSize(id); - const Vec4& color = getColor(id); - - // Calculate the bounding box of this object in world coordinates - float halfSize = size * 0.5f; - float objMinX = pos.x - halfSize; - float objMinY = pos.y - halfSize; - float objMaxX = pos.x + halfSize; - float objMaxY = pos.y + halfSize; - - // Check if object overlaps with the region - if (objMaxX >= minCorner.x && objMinX <= maxCorner.x && - objMaxY >= minCorner.y && objMinY <= maxCorner.y) { - - int minGridX = static_cast(std::floor(std::max(objMinX, minCorner.x) - minCorner.x)); - int minGridY = static_cast(std::floor(std::max(objMinY, minCorner.y) - minCorner.y)); - int maxGridX = static_cast(std::ceil(std::min(objMaxX, maxCorner.x) - minCorner.x)); - int maxGridY = static_cast(std::ceil(std::min(objMaxY, maxCorner.y) - minCorner.y)); - - // Clamp to grid boundaries - minGridX = std::max(0, minGridX); - minGridY = std::max(0, minGridY); - maxGridX = std::min(width - 1, maxGridX); - maxGridY = std::min(height - 1, maxGridY); - - // Fill all pixels within the object's overlapping region in BGR format - for (int y = minGridY; y <= maxGridY; ++y) { - for (int x = minGridX; x <= maxGridX; ++x) { - int index = (y * width + x) * 3; - - // Convert float color [0,1] to int [0,255] in BGR format - rgbData[index + 2] = static_cast(color.b * 255); // Blue channel - rgbData[index + 1] = static_cast(color.g * 255); // Green channel - rgbData[index] = static_cast(color.r * 255); // Red channel - } - } - } - } - } - - void getGridRegionAsRGB(float minX, float minY, float maxX, float maxY, - int& width, int& height, std::vector& rgbData) const { - getGridRegionAsRGB(Vec2(minX, minY), Vec2(maxX, maxY), width, height, rgbData); - } - - void getGridAsRGB(int& width, int& height, std::vector& rgbData) const { - TIME_FUNCTION; - - // Get the bounding box of all objects - Vec2 minCorner, maxCorner; - getBoundingBox(minCorner, maxCorner); - - // Use the main function to get BGR data for the entire region - getGridRegionAsRGB(minCorner, maxCorner, width, height, rgbData); - } - - void getGridRegionAsBGR(const Vec2& minCorner, const Vec2& maxCorner, - int& width, int& height, std::vector& bgrData) const { - TIME_FUNCTION; - - // Ensure valid region - if (minCorner.x >= maxCorner.x || minCorner.y >= maxCorner.y) { - width = 0; - height = 0; - bgrData.clear(); - return; - } - - // Calculate grid dimensions - width = static_cast(std::ceil(maxCorner.x - minCorner.x)); - height = static_cast(std::ceil(maxCorner.y - minCorner.y)); - - // Initialize with black (0,0,0) in BGR format - bgrData.resize(width * height * 3, 0); - - // Fill the grid with object colors in the region, accounting for sizes - for (const auto& posPair : positions) { - size_t id = posPair.first; - const Vec2& pos = posPair.second; - float size = getSize(id); - const Vec4& color = getColor(id); - - // Calculate the bounding box of this object in world coordinates - float halfSize = size * 0.5f; - float objMinX = pos.x - halfSize; - float objMinY = pos.y - halfSize; - float objMaxX = pos.x + halfSize; - float objMaxY = pos.y + halfSize; - - // Check if object overlaps with the region - if (objMaxX >= minCorner.x && objMinX <= maxCorner.x && - objMaxY >= minCorner.y && objMinY <= maxCorner.y) { - - // Calculate overlapping region in grid coordinates - int minGridX = static_cast(std::floor(std::max(objMinX, minCorner.x) - minCorner.x)); - int minGridY = static_cast(std::floor(std::max(objMinY, minCorner.y) - minCorner.y)); - int maxGridX = static_cast(std::ceil(std::min(objMaxX, maxCorner.x) - minCorner.x)); - int maxGridY = static_cast(std::ceil(std::min(objMaxY, maxCorner.y) - minCorner.y)); - - // Clamp to grid boundaries - minGridX = std::max(0, minGridX); - minGridY = std::max(0, minGridY); - maxGridX = std::min(width - 1, maxGridX); - maxGridY = std::min(height - 1, maxGridY); - - // Fill all pixels within the object's overlapping region in BGR format - for (int y = minGridY; y <= maxGridY; ++y) { - for (int x = minGridX; x <= maxGridX; ++x) { - int index = (y * width + x) * 3; - - // Convert float color [0,1] to int [0,255] in BGR format - bgrData[index] = static_cast(color.b * 255); // Blue channel - bgrData[index + 1] = static_cast(color.g * 255); // Green channel - bgrData[index + 2] = static_cast(color.r * 255); // Red channel - } - } - } - } - } - - // Helper function that takes individual coordinates instead of Vec2 - void getGridRegionAsBGR(float minX, float minY, float maxX, float maxY, - int& width, int& height, std::vector& bgrData) const { - getGridRegionAsBGR(Vec2(minX, minY), Vec2(maxX, maxY), width, height, bgrData); - } - - // Helper function that gets the entire grid bounds and returns as BGR - void getGridAsBGR(int& width, int& height, std::vector& bgrData) const { - TIME_FUNCTION; - - // Get the bounding box of all objects - Vec2 minCorner, maxCorner; - getBoundingBox(minCorner, maxCorner); - - // Use the main function to get BGR data for the entire region - getGridRegionAsBGR(minCorner, maxCorner, width, height, bgrData); - } - - //spatial map - std::pair worldToGrid(const Vec2& pos) const { - return { - static_cast(std::floor(pos.x / cellSize)), - static_cast(std::floor(pos.y / cellSize)) - }; + return total; } - 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; - } + size_t getLooseObjectCount() const { + return loosePositions.size(); } - std::vector getIndicesInRadius(const Vec2& position, float radius) const { - std::vector 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); - } - } - } - } - } - - return result; - } + bool isUsingSpatialPartitioning() const { return useSpatialPartitioning; } - std::vector getIndicesInRegion(const Vec2& minCorner, const Vec2& maxCorner) const { - std::vector result; - - auto minCell = worldToGrid(minCorner); - auto maxCell = worldToGrid(maxCorner); - - 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& pos = getPosition(id); - if (pos.x >= minCorner.x && pos.x <= maxCorner.x && - pos.y >= minCorner.y && pos.y <= maxCorner.y) { - result.push_back(id); - } - } - } - } - } - - return result; - } - - size_t getSpatialGridCellCount() const { return spatialGrid.size(); } - size_t getSpatialGridObjectCount() const { return cellIndices.size(); } - float getCellSize() const { return cellSize; } - - // Additional utility methods - size_t getObjectCount() const { return positions.size(); } void clear() { - positions.clear(); - colors.clear(); - sizes.clear(); - spatialGrid.clear(); - cellIndices.clear(); + loosePositions.clear(); + looseColors.clear(); + looseSizes.clear(); + spatialCells.clear(); next_id = 0; } + +private: + std::unordered_map, std::unique_ptr, PairHash> nestedGrids; + int gridLevel; + int maxLevels; + bool isLeafGrid; + + Grid2* getOrCreateNestedGrid(int cellX, int cellY) { + auto key = std::make_pair(cellX, cellY); + auto it = nestedGrids.find(key); + if (it != nestedGrids.end()) { + return it->second.get(); + } + + if (gridLevel >= maxLevels) { + return nullptr; // Reached maximum depth + } + + // Create new nested grid with subdivided bounds + Vec2 nestedMin = getCellMinCorner(cellX, cellY); + Vec2 nestedMax = getCellMaxCorner(cellX, cellY); + + auto nestedGrid = std::make_unique(); + nestedGrid->initializeSpatialPartitioning(nestedMin, nestedMax); + nestedGrid->setGridLevel(gridLevel + 1); + nestedGrid->setMaxLevels(maxLevels); + + nestedGrids[key] = std::move(nestedGrid); + return nestedGrids[key].get(); + } + + // Check if object should be pushed to nested grid + bool shouldPushToNestedGrid(const Vec2& position, float size) const { + if (gridLevel >= maxLevels) return false; + + // Push to nested grid if object fits completely in a single cell + // and we haven't reached maximum depth + return canFitInSpatialCell(position, size); + } + +public: + void setGridLevel(int level) { gridLevel = level; } + void setMaxLevels(int levels) { maxLevels = levels; } + int getGridLevel() const { return gridLevel; } }; #endif \ No newline at end of file diff --git a/util/grid/grid2fast.hpp b/util/grid/grid2fast.hpp new file mode 100644 index 0000000..13ee764 --- /dev/null +++ b/util/grid/grid2fast.hpp @@ -0,0 +1,316 @@ +#ifndef FIXED_SPATIAL_GRID_2_HPP +#define FIXED_SPATIAL_GRID_2_HPP + +#include "../vectorlogic/vec2.hpp" +#include +#include +#include +#include +#include + +class Grid2Fast { +private: + struct Cell { + std::vector objectIds; + + void add(size_t id) { + objectIds.push_back(id); + } + + void remove(size_t id) { + auto it = std::find(objectIds.begin(), objectIds.end(), id); + if (it != objectIds.end()) { + objectIds.erase(it); + } + } + + bool contains(size_t id) const { + return std::find(objectIds.begin(), objectIds.end(), id) != objectIds.end(); + } + + void clear() { + objectIds.clear(); + } + + size_t size() const { + return objectIds.size(); + } + + bool empty() const { + return objectIds.empty(); + } + }; + + // Fixed grid dimensions + int gridWidth, gridHeight; + float cellSize; + Vec2 worldMin, worldMax; + + // 2D grid storage + std::vector grid; + std::unordered_map> objectToCell; + + // Helper methods + inline int toIndex(int x, int y) const { + return y * gridWidth + x; + } + + inline bool isValidCell(int x, int y) const { + return x >= 0 && x < gridWidth && y >= 0 && y < gridHeight; + } + +public: + Grid2Fast(const Vec2& minCorner, const Vec2& maxCorner, float cellSize) + : cellSize(cellSize), worldMin(minCorner), worldMax(maxCorner) { + + // Calculate grid dimensions + float worldWidth = maxCorner.x - minCorner.x; + float worldHeight = maxCorner.y - minCorner.y; + + gridWidth = static_cast(std::ceil(worldWidth / cellSize)); + gridHeight = static_cast(std::ceil(worldHeight / cellSize)); + + // Initialize grid with empty cells + grid.resize(gridWidth * gridHeight); + } + + Grid2Fast(float minX, float minY, float maxX, float maxY, float cellSize) + : Grid2Fast(Vec2(minX, minY), Vec2(maxX, maxY), cellSize) {} + + // Convert world position to grid coordinates + std::pair worldToGrid(const Vec2& pos) const { + int x = static_cast((pos.x - worldMin.x) / cellSize); + int y = static_cast((pos.y - worldMin.y) / cellSize); + + // Clamp to grid boundaries + x = std::clamp(x, 0, gridWidth - 1); + y = std::clamp(y, 0, gridHeight - 1); + + return {x, y}; + } + + // Convert grid coordinates to world position (center of cell) + Vec2 gridToWorld(int gridX, int gridY) const { + float x = worldMin.x + (gridX + 0.5f) * cellSize; + float y = worldMin.y + (gridY + 0.5f) * cellSize; + return Vec2(x, y); + } + + // Add object to spatial grid + bool addObject(size_t id, const Vec2& position) { + auto [gridX, gridY] = worldToGrid(position); + + if (!isValidCell(gridX, gridY)) { + return false; // Object outside grid bounds + } + + int index = toIndex(gridX, gridY); + grid[index].add(id); + objectToCell[id] = {gridX, gridY}; + return true; + } + + // Remove object from spatial grid + bool removeObject(size_t id) { + auto it = objectToCell.find(id); + if (it == objectToCell.end()) { + return false; + } + + auto [gridX, gridY] = it->second; + if (isValidCell(gridX, gridY)) { + int index = toIndex(gridX, gridY); + grid[index].remove(id); + } + + objectToCell.erase(it); + return true; + } + + // Update object position + bool updateObject(size_t id, const Vec2& oldPos, const Vec2& newPos) { + auto oldCell = worldToGrid(oldPos); + auto newCell = worldToGrid(newPos); + + if (oldCell == newCell) { + // Same cell, no update needed + objectToCell[id] = newCell; + return true; + } + + // Remove from old cell + auto [oldX, oldY] = oldCell; + if (isValidCell(oldX, oldY)) { + int oldIndex = toIndex(oldX, oldY); + grid[oldIndex].remove(id); + } + + // Add to new cell + auto [newX, newY] = newCell; + if (!isValidCell(newX, newY)) { + // Object moved outside grid, remove completely + objectToCell.erase(id); + return false; + } + + int newIndex = toIndex(newX, newY); + grid[newIndex].add(id); + objectToCell[id] = newCell; + return true; + } + + // Get objects in radius (optimized using grid) + std::vector getObjectsInRadius(const Vec2& position, float radius) const { + std::vector result; + + if (radius <= 0.0f) { + return getObjectsAt(position); + } + + 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; + + // Check only relevant cells + for (int y = minCell.second; y <= maxCell.second; ++y) { + for (int x = minCell.first; x <= maxCell.first; ++x) { + if (!isValidCell(x, y)) continue; + + int index = toIndex(x, y); + const Cell& cell = grid[index]; + + for (size_t id : cell.objectIds) { + // We need external position data for distance check + // This assumes the caller will filter results based on actual positions + result.push_back(id); + } + } + } + + return result; + } + + // Get objects at exact position + std::vector getObjectsAt(const Vec2& position) const { + auto [gridX, gridY] = worldToGrid(position); + + if (!isValidCell(gridX, gridY)) { + return {}; + } + + int index = toIndex(gridX, gridY); + return grid[index].objectIds; // Return copy + } + + // Get objects in rectangular region + std::vector getObjectsInRegion(const Vec2& minCorner, const Vec2& maxCorner) const { + std::vector result; + + auto minCell = worldToGrid(minCorner); + auto maxCell = worldToGrid(maxCorner); + + for (int y = minCell.second; y <= maxCell.second; ++y) { + for (int x = minCell.first; x <= maxCell.first; ++x) { + if (!isValidCell(x, y)) continue; + + int index = toIndex(x, y); + const Cell& cell = grid[index]; + + // Add all objects from these cells + // Note: This may include objects outside the exact region due to cell granularity + // Caller should filter based on actual positions if precise region is needed + result.insert(result.end(), cell.objectIds.begin(), cell.objectIds.end()); + } + } + + return result; + } + + // Get all objects in the grid + std::vector getAllObjects() const { + std::vector result; + + for (const auto& pair : objectToCell) { + result.push_back(pair.first); + } + + return result; + } + + // Get cell information + const Cell& getCell(int x, int y) const { + static Cell emptyCell; + if (!isValidCell(x, y)) { + return emptyCell; + } + return grid[toIndex(x, y)]; + } + + const Cell& getCellAtWorldPos(const Vec2& pos) const { + auto [x, y] = worldToGrid(pos); + return getCell(x, y); + } + + // Statistics + size_t getTotalObjectCount() const { + return objectToCell.size(); + } + + size_t getNonEmptyCellCount() const { + size_t count = 0; + for (const auto& cell : grid) { + if (!cell.empty()) { + ++count; + } + } + return count; + } + + size_t getMaxObjectsPerCell() const { + size_t maxCount = 0; + for (const auto& cell : grid) { + maxCount = std::max(maxCount, cell.size()); + } + return maxCount; + } + + float getAverageObjectsPerCell() const { + if (grid.empty()) return 0.0f; + return static_cast(objectToCell.size()) / grid.size(); + } + + // Grid properties + int getGridWidth() const { return gridWidth; } + int getGridHeight() const { return gridHeight; } + float getCellSize() const { return cellSize; } + Vec2 getWorldMin() const { return worldMin; } + Vec2 getWorldMax() const { return worldMax; } + + // Clear all objects + void clear() { + for (auto& cell : grid) { + cell.clear(); + } + objectToCell.clear(); + } + + // Check if object exists in grid + bool contains(size_t id) const { + return objectToCell.find(id) != objectToCell.end(); + } + + // Get cell coordinates for object + std::pair getObjectCell(size_t id) const { + auto it = objectToCell.find(id); + if (it != objectToCell.end()) { + return it->second; + } + return {-1, -1}; + } +}; + +#endif \ No newline at end of file diff --git a/util/grid/spatc16.hpp b/util/grid/spatc16.hpp new file mode 100644 index 0000000..daa484d --- /dev/null +++ b/util/grid/spatc16.hpp @@ -0,0 +1,233 @@ +#ifndef SPATIAL_CELL_16X16_HPP +#define SPATIAL_CELL_16X16_HPP + +#include "../vectorlogic/vec2.hpp" +#include "../vectorlogic/vec4.hpp" +#include +#include +#include +#include +#include + +class SpatialCell16x16 { +private: + static constexpr int CELL_SIZE = 16; + static constexpr int TOTAL_CELLS = CELL_SIZE * CELL_SIZE; + + // Store objects in the cell + std::unordered_map positions; + std::unordered_map colors; + std::unordered_map sizes; + + // Bounds of this cell in world coordinates + Vec2 worldMin, worldMax; + float worldCellSize; // Size of each pixel in world coordinates + +public: + SpatialCell16x16(const Vec2& minCorner, const Vec2& maxCorner) + : worldMin(minCorner), worldMax(maxCorner) { + + // Calculate world size per cell pixel + worldCellSize = std::max( + (worldMax.x - worldMin.x) / CELL_SIZE, + (worldMax.y - worldMin.y) / CELL_SIZE + ); + } + + // Convert world position to cell coordinates [0,15] + std::pair worldToCell(const Vec2& worldPos) const { + float localX = (worldPos.x - worldMin.x) / (worldMax.x - worldMin.x); + float localY = (worldPos.y - worldMin.y) / (worldMax.y - worldMin.y); + + int cellX = static_cast(localX * CELL_SIZE); + int cellY = static_cast(localY * CELL_SIZE); + + // Clamp to valid range + cellX = std::clamp(cellX, 0, CELL_SIZE - 1); + cellY = std::clamp(cellY, 0, CELL_SIZE - 1); + + return {cellX, cellY}; + } + + // Convert cell coordinates to linear index + int cellToIndex(int x, int y) const { + return y * CELL_SIZE + x; + } + + // Convert linear index to cell coordinates + std::pair indexToCell(int index) const { + return {index % CELL_SIZE, index / CELL_SIZE}; + } + + // Convert cell coordinates to world position (center of cell) + Vec2 cellToWorld(int x, int y) const { + float worldX = worldMin.x + (x + 0.5f) * worldCellSize; + float worldY = worldMin.y + (y + 0.5f) * worldCellSize; + return Vec2(worldX, worldY); + } + + // Add object to the spatial cell + bool addObject(size_t id, const Vec2& position, const Vec4& color, float size = 1.0f) { + if (!contains(position)) { + return false; + } + + positions[id] = position; + colors[id] = color; + sizes[id] = size; + + return true; + } + + // Check if world position is within this cell's bounds + bool contains(const Vec2& worldPos) const { + return worldPos.x >= worldMin.x && worldPos.x <= worldMax.x && + worldPos.y >= worldMin.y && worldPos.y <= worldMax.y; + } + + // Update object position + void updateObject(size_t id, const Vec2& oldPos, const Vec2& newPos) { + if (!hasObject(id)) return; + + positions[id] = newPos; + } + + // Remove object + void removeObject(size_t id) { + if (!hasObject(id)) return; + + positions.erase(id); + colors.erase(id); + sizes.erase(id); + } + + // Check if object exists + bool hasObject(size_t id) const { + return positions.find(id) != positions.end(); + } + + // Get object data + Vec2 getPosition(size_t id) const { + auto it = positions.find(id); + return it != positions.end() ? it->second : Vec2(); + } + + Vec4 getColor(size_t id) const { + auto it = colors.find(id); + return it != colors.end() ? it->second : Vec4(); + } + + float getSize(size_t id) const { + auto it = sizes.find(id); + return it != sizes.end() ? it->second : 1.0f; + } + + // Set object data + void setPosition(size_t id, const Vec2& position) { + if (hasObject(id)) { + positions[id] = position; + } + } + + void setColor(size_t id, const Vec4& color) { + colors[id] = color; + } + + void setSize(size_t id, float size) { + if (hasObject(id)) { + sizes[id] = size; + } + } + + // Spatial queries + std::vector getObjectsAt(const Vec2& position) const { + std::vector result; + + // Check all objects since we don't have spatial indexing + for (const auto& pair : positions) { + size_t id = pair.first; + const Vec2& objPos = pair.second; + float size = sizes.at(id); + + // Check if position is within object bounds + if (position.x >= objPos.x - size * 0.5f && position.x <= objPos.x + size * 0.5f && + position.y >= objPos.y - size * 0.5f && position.y <= objPos.y + size * 0.5f) { + result.push_back(id); + } + } + + return result; + } + + std::vector getObjectsInRadius(const Vec2& center, float radius) const { + std::vector result; + float radius_sq = radius * radius; + + // Check all objects since we don't have spatial indexing + for (const auto& pair : positions) { + size_t id = pair.first; + const Vec2& pos = pair.second; + + float dx = pos.x - center.x; + float dy = pos.y - center.y; + if (dx * dx + dy * dy <= radius_sq) { + result.push_back(id); + } + } + + return result; + } + + std::vector getObjectsInRegion(const Vec2& minCorner, const Vec2& maxCorner) const { + std::vector result; + + // Check all objects since we don't have spatial indexing + for (const auto& pair : positions) { + size_t id = pair.first; + const Vec2& pos = pair.second; + + if (pos.x >= minCorner.x && pos.x <= maxCorner.x && + pos.y >= minCorner.y && pos.y <= maxCorner.y) { + result.push_back(id); + } + } + + return result; + } + + // Get all object IDs + std::vector getAllObjectIds() const { + std::vector ids; + ids.reserve(positions.size()); + for (const auto& pair : positions) { + ids.push_back(pair.first); + } + return ids; + } + + // Get cell statistics + size_t getObjectCount() const { return positions.size(); } + size_t getNonEmptyCellCount() const { + // Since we removed cellBuckets, return 1 if there are objects, 0 otherwise + return positions.empty() ? 0 : 1; + } + + // Get bounds + Vec2 getMinCorner() const { return worldMin; } + Vec2 getMaxCorner() const { return worldMax; } + + // Clear all objects + void clear() { + positions.clear(); + colors.clear(); + sizes.clear(); + } + +private: + // Spatial indexing is no longer used + void updateSpatialIndex(size_t id, const Vec2& oldPos, const Vec2& newPos) { + // Empty implementation since we removed spatial indexing + } +}; + +#endif \ No newline at end of file