vibe3_physics/source/spatial_grid.cpp

#include "spatial_grid.hpp"

#include <algorithm>  // for std::max, std::min
#include <cmath>      // for std::floor, std::ceil

#include "ball.hpp"  // for Ball

SpatialGrid::SpatialGrid(int world_width, int world_height, float cell_size)
    : world_width_(world_width)
    , world_height_(world_height)
    , cell_size_(cell_size) {
    // Calcular número de celdas en cada dimensión
    grid_cols_ = static_cast<int>(std::ceil(world_width / cell_size));
    grid_rows_ = static_cast<int>(std::ceil(world_height / cell_size));
}

void SpatialGrid::clear() {
    // Limpiar todos los vectores de celdas (O(n) donde n = número de celdas ocupadas)
    cells_.clear();
}

void SpatialGrid::insert(Ball* ball, float x, float y) {
    // Obtener coordenadas de celda
    int cell_x, cell_y;
    getCellCoords(x, y, cell_x, cell_y);

    // Generar hash key y añadir a la celda
    int key = getCellKey(cell_x, cell_y);
    cells_[key].push_back(ball);
}

std::vector<Ball*> SpatialGrid::queryRadius(float x, float y, float radius) {
    std::vector<Ball*> results;

    // Calcular rango de celdas a revisar (AABB del círculo de búsqueda)
    int min_cell_x, min_cell_y, max_cell_x, max_cell_y;
    getCellCoords(x - radius, y - radius, min_cell_x, min_cell_y);
    getCellCoords(x + radius, y + radius, max_cell_x, max_cell_y);

    // Iterar sobre todas las celdas dentro del AABB
    for (int cy = min_cell_y; cy <= max_cell_y; cy++) {
        for (int cx = min_cell_x; cx <= max_cell_x; cx++) {
            // Verificar que la celda está dentro del grid
            if (cx < 0 || cx >= grid_cols_ || cy < 0 || cy >= grid_rows_) {
                continue;
            }

            // Obtener key de la celda
            int key = getCellKey(cx, cy);

            // Si la celda existe en el mapa, añadir todos sus objetos
            auto it = cells_.find(key);
            if (it != cells_.end()) {
                // Añadir todos los objetos de esta celda al resultado
                results.insert(results.end(), it->second.begin(), it->second.end());
            }
        }
    }

    return results;
}

void SpatialGrid::updateWorldSize(int world_width, int world_height) {
    world_width_ = world_width;
    world_height_ = world_height;

    // Recalcular dimensiones del grid
    grid_cols_ = static_cast<int>(std::ceil(world_width / cell_size_));
    grid_rows_ = static_cast<int>(std::ceil(world_height / cell_size_));

    // Limpiar grid (las posiciones anteriores ya no son válidas)
    clear();
}

// ============================================================================
// MÉTODOS PRIVADOS
// ============================================================================

void SpatialGrid::getCellCoords(float x, float y, int& cell_x, int& cell_y) const {
    // Convertir coordenadas del mundo a coordenadas de celda
    cell_x = static_cast<int>(std::floor(x / cell_size_));
    cell_y = static_cast<int>(std::floor(y / cell_size_));
}

int SpatialGrid::getCellKey(int cell_x, int cell_y) const {
    // Hash espacial 2D → 1D usando codificación por filas
    // Formula: key = y * ancho + x (similar a array 2D aplanado)
    return cell_y * grid_cols_ + cell_x;
}