From 65206162a6abf4c07a2a049714e01eb5c08554bc Mon Sep 17 00:00:00 2001 From: paladin Date: Wed, 9 Sep 2026 09:49:16 +0100 Subject: [PATCH] Initial. Added Loose/Tight Double Grid C++ implementation --- loose_tight_double_grid.cpp | 933 ++++++++++++++++++++++++++++++++++++ loose_tight_double_grid.txt | 933 ++++++++++++++++++++++++++++++++++++ 2 files changed, 1866 insertions(+) create mode 100644 loose_tight_double_grid.cpp create mode 100644 loose_tight_double_grid.txt diff --git a/loose_tight_double_grid.cpp b/loose_tight_double_grid.cpp new file mode 100644 index 0000000..4707629 --- /dev/null +++ b/loose_tight_double_grid.cpp @@ -0,0 +1,933 @@ +// https://stackoverflow.com/questions/41946007/efficient-and-well-explained-implementation-of-a-quadtree-for-2d-collision-det/48330314#48330314 +// https://pastebin.com/swvwTgnd + +// ************************************************************************************ +// SmallList.hpp +// ************************************************************************************ +#ifndef SMALL_LIST_HPP +#define SMALL_LIST_HPP + +#include +#include +#include +#include + +// Stores a random-access sequence of elements similar to vector, but avoids +// heap allocations for small lists. T must be trivially constructible and +// destructible. +template +class SmallList +{ +public: + // Creates an empty list. + SmallList(); + + // Creates a copy of the specified list. + SmallList(const SmallList& other); + + // Copies the specified list. + SmallList& operator=(const SmallList& other); + + // Destroys the list. + ~SmallList(); + + // Returns the number of agents in the list. + int size() const; + + // Returns the nth element. + T& operator[](int n); + + // Returns the nth element in the list. + const T& operator[](int n) const; + + // Returns an index to a matching element in the list or -1 + // if the element is not found. + int find_index(const T& element) const; + + // Clears the list. + void clear(); + + // Reserves space for n elements. + void reserve(int n); + + // Inserts an element to the back of the list. + void push_back(const T& element); + + /// Pops an element off the back of the list. + T pop_back(); + + // Swaps the contents of this list with the other. + void swap(SmallList& other); + + // Returns a pointer to the underlying buffer. + T* data(); + + // Returns a pointer to the underlying buffer. + const T* data() const; + +private: + enum {fixed_cap = 256}; + struct ListData + { + ListData(); + T buf[fixed_cap]; + T* data; + int num; + int cap; + }; + ListData ld; +}; + +/// Provides an indexed free list with constant-time removals from anywhere +/// in the list without invalidating indices. T must be trivially constructible +/// and destructible. +template +class FreeList +{ +public: + /// Creates a new free list. + FreeList(); + + /// Inserts an element to the free list and returns an index to it. + int insert(const T& element); + + // Removes the nth element from the free list. + void erase(int n); + + // Removes all elements from the free list. + void clear(); + + // Returns the range of valid indices. + int range() const; + + // Returns the nth element. + T& operator[](int n); + + // Returns the nth element. + const T& operator[](int n) const; + + // Reserves space for n elements. + void reserve(int n); + + // Swaps the contents of the two lists. + void swap(FreeList& other); + +private: + union FreeElement + { + T element; + int next; + }; + SmallList data; + int first_free; +}; + +// --------------------------------------------------------------------------------- +// SmallList Implementation +// --------------------------------------------------------------------------------- +template +SmallList::ListData::ListData(): data(buf), num(0), cap(fixed_cap) +{ +} + +template +SmallList::SmallList() +{ +} + +template +SmallList::SmallList(const SmallList& other) +{ + if (other.ld.cap == fixed_cap) + { + ld = other.ld; + ld.data = ld.buf; + } + else + { + reserve(other.ld.num); + for (int j=0; j < other.size(); ++j) + ld.data[j] = other.ld.data[j]; + ld.num = other.ld.num; + ld.cap = other.ld.cap; + } +} + +template +SmallList& SmallList::operator=(const SmallList& other) +{ + SmallList(other).swap(*this); + return *this; +} + +template +SmallList::~SmallList() +{ + if (ld.data != ld.buf) + free(ld.data); +} + +template +int SmallList::size() const +{ + return ld.num; +} + +template +T& SmallList::operator[](int n) +{ + assert(n >= 0 && n < ld.num); + return ld.data[n]; +} + +template +const T& SmallList::operator[](int n) const +{ + assert(n >= 0 && n < ld.num); + return ld.data[n]; +} + +template +int SmallList::find_index(const T& element) const +{ + for (int j=0; j < ld.num; ++j) + { + if (ld.data[j] == element) + return j; + } + return -1; +} + +template +void SmallList::clear() +{ + ld.num = 0; +} + +template +void SmallList::reserve(int n) +{ + enum {type_size = sizeof(T)}; + if (n > ld.cap) + { + if (ld.cap == fixed_cap) + { + ld.data = static_cast(malloc(n * type_size)); + memcpy(ld.data, ld.buf, sizeof(ld.buf)); + } + else + ld.data = static_cast(realloc(ld.data, n * type_size)); + ld.cap = n; + } +} + +template +void SmallList::push_back(const T& element) +{ + if (ld.num >= ld.cap) + reserve(ld.cap * 2); + ld.data[ld.num++] = element; +} + +template +T SmallList::pop_back() +{ + return ld.data[--ld.num]; +} + +template +void SmallList::swap(SmallList& other) +{ + ListData& ld1 = ld; + ListData& ld2 = other.ld; + + const int use_fixed1 = ld1.data == ld1.buf; + const int use_fixed2 = ld2.data == ld2.buf; + + const ListData temp = ld1; + ld1 = ld2; + ld2 = temp; + + if (use_fixed1) + ld2.data = ld2.buf; + if (use_fixed2) + ld1.data = ld1.buf; +} + +template +T* SmallList::data() +{ + return ld.data; +} + +template +const T* SmallList::data() const +{ + return ld.data; +} + +// --------------------------------------------------------------------------------- +// FreeList Implementation +// --------------------------------------------------------------------------------- +template +FreeList::FreeList(): first_free(-1) +{ +} + +template +int FreeList::insert(const T& element) +{ + if (first_free != -1) + { + const int index = first_free; + first_free = data[first_free].next; + data[index].element = element; + return index; + } + else + { + FreeElement fe; + fe.element = element; + data.push_back(fe); + return data.size() - 1; + } +} + +template +void FreeList::erase(int n) +{ + assert(n >= 0 && n < data.size()); + data[n].next = first_free; + first_free = n; +} + +template +void FreeList::clear() +{ + data.clear(); + first_free = -1; +} + +template +int FreeList::range() const +{ + return data.size(); +} + +template +T& FreeList::operator[](int n) +{ + return data[n].element; +} + +template +const T& FreeList::operator[](int n) const +{ + return data[n].element; +} + +template +void FreeList::reserve(int n) +{ + data.reserve(n); +} + +template +void FreeList::swap(FreeList& other) +{ + const int temp = first_free; + data.swap(other.data); + first_free = other.first_free; + other.first_free = temp; +} + +#endif + +// ************************************************************************************ +// LGrid.hpp +// ************************************************************************************ +#ifndef LGRID_HPP +#define LGRID_HPP + +#include "SmallList.hpp" + +struct LGridQuery4 +{ + // Stores the resulting elements of the SIMD query. + SmallList elements[4]; +}; + +struct LGridElt +{ + // Stores the index to the next element in the loose cell using an indexed SLL. + int next; + + // Stores the ID of the element. This can be used to associate external + // data to the element. + int id; + + // Stores the center of the element. + float mx, my; + + // Stores the half-size of the element relative to the upper-left corner + // of the grid. + float hx, hy; +}; + +struct LGridLooseCell +{ + // Stores the extents of the grid cell relative to the upper-left corner + // of the grid which expands and shrinks with the elements inserted and + // removed. + float rect[4]; + + // Stores the index to the first element using an indexed SLL. + int head; +}; + +struct LGridLoose +{ + // Stores all the cells in the loose grid. + LGridLooseCell* cells; + + // Stores the number of columns, rows, and cells in the loose grid. + int num_cols, num_rows, num_cells; + + // Stores the inverse size of a loose cell. + float inv_cell_w, inv_cell_h; +}; + +struct LGridTightCell +{ + // Stores the index to the next loose cell in the grid cell. + int next; + + // Stores the position of the loose cell in the grid. + int lcell; +}; + +struct LGridTight +{ + // Stores all the tight cell nodes in the grid. + FreeList cells; + + // Stores the tight cell heads. + int* heads; + + // Stores the number of columns, rows, and cells in the tight grid. + int num_cols, num_rows, num_cells; + + // Stores the inverse size of a tight cell. + float inv_cell_w, inv_cell_h; +}; + +struct LGrid +{ + // Stores the tight cell data for the grid. + LGridTight tight; + + // Stores the loose cell data for the grid. + LGridLoose loose; + + // Stores all the elements in the grid. + FreeList elts; + + // Stores the number of elements in the grid. + int num_elts; + + // Stores the upper-left corner of the grid. + float x, y; + + // Stores the size of the grid. + float w, h; +}; + +// Creates a loose grid encompassing the specified extents using the specified cell +// size. Elements inserted to the loose grid are only inserted in one cell, but the +// extents of each cell are allowed to expand and shrink. To avoid requiring every +// loose cell to be checked during a search, a second grid of tight cells referencing +// the loose cells is stored. +LGrid* lgrid_create(float lcell_w, float lcell_h, float tcell_w, float tcell_h, + float l, float t, float r, float b); + +// Destroys the grid. +void lgrid_destroy(LGrid* grid); + +// Returns the grid cell index for the specified position. +int lgrid_lcell_idx(LGrid* grid, float x, float y); + +// Inserts an element to the grid. +void lgrid_insert(LGrid* grid, int id, float mx, float my, float hx, float hy); + +// Removes an element from the grid. +void lgrid_remove(LGrid* grid, int id, float mx, float my); + +// Moves an element in the grid from the former position to the new one. +void lgrid_move(LGrid* grid, int id, float prev_mx, float prev_my, float mx, float my); + +// Returns all the element IDs that intersect the specified rectangle excluding elements +// with the specified ID to omit. +SmallList lgrid_query(const LGrid* grid, float mx, float my, float hx, float hy, int omit_id); + +// Returns all the element IDs that intersect the specified 4 rectangles excluding elements +// with the specified IDs to omit. +LGridQuery4 lgrid_query4(const LGrid* grid, const SimdVec4f* mx4, const SimdVec4f* my4, + const SimdVec4f* hx4, const SimdVec4f* hy4, const SimdVec4i* omit_id4); + +// Returns true if the specified rectangle is inside the grid boundaries. +bool lgrid_in_bounds(const LGrid* grid, float mx, float my, float hx, float hy); + +// Optimizes the grid, shrinking bounding boxes in response to removed elements and +// rearranging the memory of the grid to allow cache-friendly cell traversal. +void lgrid_optimize(LGrid* grid); + +#endif + +// ************************************************************************************ +// LGrid.cpp +// ************************************************************************************ +#include "LGrid.hpp" +#include +#include +#include + +static int ceil_div(float value, float divisor) +{ + // Returns the value divided by the divisor rounded up. + const float resultf = value / divisor; + const int result = (int)resultf; + return result < resultf ? result+1: result; +} + +static int min_int(int a, int b) +{ + assert(sizeof(int) == 4); + a -= b; + a &= a >> 31; + return a + b; +} + +static int max_int(int a, int b) +{ + assert(sizeof(int) == 4); + a -= b; + a &= (~a) >> 31; + return a + b; +} + +static float min_flt(float a, float b) +{ + return std::min(a, b); +} + +static float max_flt(float a, float b) +{ + return std::max(a, b); +} + +static int to_cell_idx(float val, float inv_cell_size, int num_cells) +{ + const int cell_pos = (int)(val * inv_cell_size); + return min_int(max_int(cell_pos, 0), num_cells - 1); +} + +static SimdVec4i to_tcell_idx4(const LGrid* grid, __m128 rect) +{ + __m128 inv_cell_size_vec = simd_create4f(grid->tight.inv_cell_w, grid->tight.inv_cell_h, + grid->tight.inv_cell_w, grid->tight.inv_cell_h); + __m128 cell_xyf_vec = simd_mul4f(rect, inv_cell_size_vec); + __m128i clamp_vec = simd_create4i(grid->tight.num_cols-1, grid->tight.num_rows-1, + grid->tight.num_cols-1, grid->tight.num_rows-1); + __m128i cell_xy_vec = simd_clamp4i(simd_ftoi4f(cell_xyf_vec), simd_zero4i(), clamp_vec); + return simd_store4i(cell_xy_vec); +} + +static void grid_optimize(LGrid* grid) +{ + FreeList new_elts; + new_elts.reserve(grid->num_elts); + for (int c=0; c < grid->loose.num_cells; ++c) + { + // Replace links to the old elements list to links in the new + // cache-friendly element list. + SmallList new_elt_idxs; + LGridLooseCell* lcell = &grid->loose.cells[c]; + while (lcell->head != -1) + { + const LGridElt* elt = &grid->elts[lcell->head]; + new_elt_idxs.push_back(new_elts.insert(*elt)); + lcell->head = elt->next; + } + for (int j=0; j < new_elt_idxs.size(); ++j) + { + const int new_elt_idx = new_elt_idxs[j]; + new_elts[new_elt_idx].next = lcell->head; + lcell->head = new_elt_idx; + } + } + // Swap the new element list with the old one. + grid->elts.swap(new_elts); +} + +static void expand_aabb(LGrid* grid, int cell_idx, float mx, float my, float hx, float hy) +{ + LGridLooseCell* lcell = &grid->loose.cells[cell_idx]; + const SimdVec4f prev_rect = {lcell->rect[0], lcell->rect[1], lcell->rect[2], lcell->rect[3]}; + lcell->rect[0] = min_flt(lcell->rect[0], mx - hx); + lcell->rect[1] = min_flt(lcell->rect[1], my - hx); + lcell->rect[2] = max_flt(lcell->rect[2], mx + hx); + lcell->rect[3] = max_flt(lcell->rect[3], my + hy); + + // Determine the cells occupied by the loose cell in the tight grid. + const SimdVec4f elt_rect = {mx-hx, my-hy, mx+hx, my+hy}; + const SimdVec4i trect = to_tcell_idx4(grid, simd_load4f(&elt_rect)); + + if (prev_rect.data[0] > prev_rect.data[2]) + { + // If the loose cell was empty, simply insert the loose cell + // to all the tight cells it occupies. We don't need to check + // to see if it was already inserted. + for (int ty = trect.data[1]; ty <= trect.data[3]; ++ty) + { + int* tight_row = grid->tight.heads + ty*grid->tight.num_cols; + for (int tx = trect.data[0]; tx <= trect.data[2]; ++tx) + { + const LGridTightCell new_tcell = {tight_row[tx], cell_idx}; + tight_row[tx] = grid->tight.cells.insert(new_tcell); + } + } + } + else + { + // Only perform the insertion if the loose cell overlaps new tight cells. + const SimdVec4i prev_trect = to_tcell_idx4(grid, simd_load4f(&prev_rect)); + if (trect.data[0] != prev_trect.data[0] || trect.data[1] != prev_trect.data[1] || + trect.data[2] != prev_trect.data[2] || trect.data[3] != prev_trect.data[3]) + { + for (int ty = trect.data[1]; ty <= trect.data[3]; ++ty) + { + int* tight_row = grid->tight.heads + ty*grid->tight.num_cols; + for (int tx = trect.data[0]; tx <= trect.data[2]; ++tx) + { + if (tx < prev_trect.data[0] || tx > prev_trect.data[2] || + ty < prev_trect.data[1] || ty > prev_trect.data[3]) + { + const LGridTightCell new_tcell = {tight_row[tx], cell_idx}; + tight_row[tx] = grid->tight.cells.insert(new_tcell); + } + } + } + } + } +} + +static __m128 element_rect(const LGridElt* elt) +{ + return simd_create4f(elt->mx-elt->hx, elt->my-elt->hy, + elt->mx+elt->hx, elt->my+elt->hy); +} + +LGrid* lgrid_create(float lcell_w, float lcell_h, float tcell_w, float tcell_h, + float l, float t, float r, float b) +{ + const float w = r - l, h = b - t; + const int num_lcols = ceil_div(w, lcell_w), num_lrows = ceil_div(h, lcell_h); + const int num_tcols = ceil_div(w, tcell_w), num_trows = ceil_div(h, tcell_h); + + LGrid* grid = new LGrid; + grid->num_elts = 0; + grid->x = l; + grid->y = t; + grid->h = w; + grid->w = h; + + grid->loose.num_cols = num_lcols; + grid->loose.num_rows = num_lrows; + grid->loose.num_cells = grid->loose.num_cols * grid->loose.num_rows; + grid->loose.inv_cell_w = 1.0f / lcell_w; + grid->loose.inv_cell_h = 1.0f / lcell_h; + + grid->tight.num_cols = num_tcols; + grid->tight.num_rows = num_trows; + grid->tight.num_cells = grid->tight.num_cols * grid->tight.num_rows; + grid->tight.inv_cell_w = 1.0f / tcell_w; + grid->tight.inv_cell_h = 1.0f / tcell_h; + + // Initialize tight cell heads with -1 to indicate empty indexed SLLs. + grid->tight.heads = new int[grid->tight.num_cells]; + for (int j=0; j < grid->tight.num_cells; ++j) + grid->tight.heads[j] = -1; + + // Initialize all the loose cells. + grid->loose.cells = new LGridLooseCell[grid->loose.num_cells]; + for (int c=0; c < grid->loose.num_cells; ++c) + { + grid->loose.cells[c].head = -1; + grid->loose.cells[c].rect[0] = FLT_MAX; + grid->loose.cells[c].rect[1] = FLT_MAX; + grid->loose.cells[c].rect[2] = -FLT_MAX; + grid->loose.cells[c].rect[3] = -FLT_MAX; + } + return grid; +} + +void lgrid_destroy(LGrid* grid) +{ + delete[] grid->loose.cells; + delete[] grid->tight.heads; + delete grid; +} + +int lgrid_lcell_idx(LGrid* grid, float x, float y) +{ + const int cell_x = to_cell_idx(x - grid->x, grid->loose.inv_cell_w, grid->loose.num_cols); + const int cell_y = to_cell_idx(y - grid->y, grid->loose.inv_cell_h, grid->loose.num_rows); + return cell_y * grid->loose.num_cols + cell_x; +} + +void lgrid_insert(LGrid* grid, int id, float mx, float my, float hx, float hy) +{ + const int cell_idx = lgrid_lcell_idx(grid, mx, my); + LGridLooseCell* lcell = &grid->loose.cells[cell_idx]; + + // Insert the element to the appropriate loose cell and row. + const LGridElt new_elt = {lcell->head, id, mx - grid->x, my - grid->y, hx, hy}; + lcell->head = grid->elts.insert(new_elt); + ++grid->num_elts; + + // Expand the loose cell's bounding box to fit the new element. + expand_aabb(grid, cell_idx, mx, my, hx, hy); +} + +void lgrid_remove(LGrid* grid, int id, float mx, float my) +{ + // Find the element in the loose cell. + LGridLooseCell* lcell = &grid->loose.cells[lgrid_lcell_idx(grid, mx, my)]; + int* link = &lcell->head; + while (grid->elts[*link].id != id) + link = &grid->elts[*link].next; + + // Remove the element from the loose cell and row. + const int elt_idx = *link; + *link = grid->elts[elt_idx].next; + grid->elts.erase(elt_idx); + --grid->num_elts; +} + +void lgrid_move(LGrid* grid, int id, float prev_mx, float prev_my, float mx, float my) +{ + const int prev_cell_idx = lgrid_lcell_idx(grid, prev_mx, prev_my); + const int new_cell_idx = lgrid_lcell_idx(grid, mx, my); + LGridLooseCell* lcell = &grid->loose.cells[prev_cell_idx]; + + if (prev_cell_idx == new_cell_idx) + { + // Find the element in the loose cell. + int elt_idx = lcell->head; + while (grid->elts[elt_idx].id != id) + elt_idx = grid->elts[elt_idx].next; + + // Since the element is still inside the same cell, we can simply overwrite + // its position and expand the loose cell's AABB. + mx -= grid->x; + my -= grid->y; + grid->elts[elt_idx].mx = mx; + grid->elts[elt_idx].my = my; + expand_aabb(grid, prev_cell_idx, mx, my, grid->elts[elt_idx].hx, grid->elts[elt_idx].hy); + } + else + { + // Find the element in the loose cell. + int* link = &lcell->head; + while (grid->elts[*link].id != id) + link = &grid->elts[*link].next; + + const int elt_idx = *link; + const float hx = grid->elts[elt_idx].hx; + const float hy = grid->elts[elt_idx].hy; + + // If the element has moved into a different loose cell, remove + // remove the element from the previous loose cell and row. + *link = grid->elts[elt_idx].next; + grid->elts.erase(elt_idx); + --grid->num_elts; + + // Now insert the element to its new position. + lgrid_insert(grid, id, mx, my, hx, hy); + } +} + +SmallList lgrid_query(const LGrid* grid, float mx, float my, float hx, float hy, int omit_id) +{ + mx -= grid->x; + my -= grid->y; + + // Compute the tight cell extents [min_tx, min_ty, max_tx, max_ty]. + const SimdVec4f qrect = {mx-hx, my-hy, mx+hx, my+hy}; + __m128 qrect_vec = simd_load4f(&qrect); + const SimdVec4i trect = to_tcell_idx4(grid, qrect_vec); + + // Gather the intersecting loose cells in the tight cells that intersect. + SmallList lcell_idxs; + for (int ty = trect.data[1]; ty <= trect.data[3]; ++ty) + { + const int* tight_row = grid->tight.heads + ty*grid->tight.num_cols; + for (int tx = trect.data[0]; tx <= trect.data[2]; ++tx) + { + // Iterate through the loose cells that intersect the tight cells. + int tcell_idx = tight_row[tx]; + while (tcell_idx != -1) + { + const LGridTightCell* tcell = &grid->tight.cells[tcell_idx]; + const LGridLooseCell* lcell = &grid->loose.cells[tcell->lcell]; + if (lcell_idxs.find_index(tcell->lcell) == -1 && simd_rect_intersect4f(qrect_vec, simd_loadu4f(lcell->rect))) + lcell_idxs.push_back(tcell->lcell); + tcell_idx = tcell->next; + } + } + } + + // For each loose cell, determine what elements intersect. + SmallList res; + for (int j=0; j < lcell_idxs.size(); ++j) + { + const LGridLooseCell* lcell = &grid->loose.cells[lcell_idxs[j]]; + int elt_idx = lcell->head; + while (elt_idx != -1) + { + // If the element intersects the search rectangle, add it to the + // resulting elements unless it has an ID that should be omitted. + const LGridElt* elt = &grid->elts[elt_idx]; + if (elt->id != omit_id && simd_rect_intersect4f(qrect_vec, element_rect(elt))) + res.push_back(elt->id); + elt_idx = elt->next; + } + } + return res; +} + +LGridQuery4 lgrid_query4(const LGrid* grid, const SimdVec4f* mx4, const SimdVec4f* my4, + const SimdVec4f* hx4, const SimdVec4f* hy4, const SimdVec4i* omit_id4) +{ + __m128 hx_vec = simd_load4f(hx4), hy_vec = simd_load4f(hy4); + __m128 mx_vec = simd_sub4f(simd_load4f(mx4), simd_scalar4f(grid->x)); + __m128 my_vec = simd_sub4f(simd_load4f(my4), simd_scalar4f(grid->y)); + __m128 ql_vec = simd_sub4f(mx_vec, hx_vec), qt_vec = simd_sub4f(my_vec, hy_vec); + __m128 qr_vec = simd_add4f(mx_vec, hx_vec), qb_vec = simd_add4f(my_vec, hy_vec); + + __m128 inv_cell_w_vec = simd_scalar4f(grid->tight.inv_cell_w), inv_cell_h_vec = simd_scalar4f(grid->tight.inv_cell_h); + __m128i max_x_vec = simd_scalar4i(grid->tight.num_cols-1), max_y_vec = simd_scalar4i(grid->tight.num_rows-1); + __m128i tmin_x_vec = simd_clamp4i(simd_ftoi4f(simd_mul4f(ql_vec, inv_cell_w_vec)), simd_zero4i(), max_x_vec); + __m128i tmin_y_vec = simd_clamp4i(simd_ftoi4f(simd_mul4f(qt_vec, inv_cell_h_vec)), simd_zero4i(), max_y_vec); + __m128i tmax_x_vec = simd_clamp4i(simd_ftoi4f(simd_mul4f(qr_vec, inv_cell_w_vec)), simd_zero4i(), max_x_vec); + __m128i tmax_y_vec = simd_clamp4i(simd_ftoi4f(simd_mul4f(qb_vec, inv_cell_h_vec)), simd_zero4i(), max_y_vec); + + const SimdVec4i tmin_x4 = simd_store4i(tmin_x_vec), tmin_y4 = simd_store4i(tmin_y_vec); + const SimdVec4i tmax_x4 = simd_store4i(tmax_x_vec), tmax_y4 = simd_store4i(tmax_y_vec); + const SimdVec4f ql4 = simd_store4f(ql_vec), qt4 = simd_store4f(qt_vec); + const SimdVec4f qr4 = simd_store4f(qr_vec), qb4 = simd_store4f(qb_vec); + + LGridQuery4 res4; + for (int k=0; k < 4; ++k) + { + const int trect[4] = {tmin_x4.data[k], tmin_y4.data[k], tmax_x4.data[k], tmax_y4.data[k]}; + const int omit_id = omit_id4->data[k]; + + // Gather the intersecting loose cells in the tight cells that intersect. + SmallList lcell_idxs; + __m128 qrect_vec = simd_create4f(ql4.data[k], qt4.data[k], qr4.data[k], qb4.data[k]); + for (int ty = trect[1]; ty <= trect[3]; ++ty) + { + const int* tight_row = grid->tight.heads + ty*grid->tight.num_cols; + for (int tx = trect[0]; tx <= trect[2]; ++tx) + { + // Iterate through the loose cells that intersect the tight cells. + int tcell_idx = tight_row[tx]; + while (tcell_idx != -1) + { + const LGridTightCell* tcell = &grid->tight.cells[tcell_idx]; + if (lcell_idxs.find_index(tcell->lcell) && simd_rect_intersect4f(qrect_vec, simd_loadu4f(grid->loose.cells[tcell->lcell].rect))) + lcell_idxs.push_back(tcell->lcell); + tcell_idx = tcell->next; + } + } + } + + // For each loose cell, determine what elements intersect. + for (int j=0; j < lcell_idxs.size(); ++j) + { + const LGridLooseCell* lcell = &grid->loose.cells[lcell_idxs[j]]; + int elt_idx = lcell->head; + while (elt_idx != -1) + { + // If the element intersects the search rectangle, add it to the + // resulting elements unless it has an ID that should be omitted. + const LGridElt* elt = &grid->elts[elt_idx]; + if (elt->id != omit_id && simd_rect_intersect4f(qrect_vec, element_rect(elt))) + res4.elements[k].push_back(elt->id); + elt_idx = elt->next; + } + } + } + return res4; +} + +bool lgrid_in_bounds(const LGrid* grid, float mx, float my, float hx, float hy) +{ + mx -= grid->x; + my -= grid->y; + const float x1 = mx-hx, y1 = my-hy, x2 = mx+hx, y2 = my+hy; + return x1 >= 0.0f && x2 < grid->w && y1 >= 0.0f && y2 < grid->h; +} + +void lgrid_optimize(LGrid* grid) +{ + // Clear all the tight cell data. + for (int j=0; j < grid->tight.num_cells; ++j) + grid->tight.heads[j] = -1; + grid->tight.cells.clear(); + + // Optimize the memory layout of the grid. + grid_optimize(grid); + + #pragma omp parallel for + for (int c=0; c < grid->loose.num_cells; ++c) + { + // Empty the loose cell's bounding box. + LGridLooseCell* lcell = &grid->loose.cells[c]; + lcell->rect[0] = FLT_MAX; + lcell->rect[1] = FLT_MAX; + lcell->rect[2] = -FLT_MAX; + lcell->rect[3] = -FLT_MAX; + + // Expand the bounding box by each element's extents in + // the loose cell. + int elt_idx = lcell->head; + while (elt_idx != -1) + { + const LGridElt* elt = &grid->elts[elt_idx]; + lcell->rect[0] = min_flt(lcell->rect[0], elt->mx - elt->hx); + lcell->rect[1] = min_flt(lcell->rect[1], elt->my - elt->hy); + lcell->rect[2] = max_flt(lcell->rect[2], elt->mx + elt->hx); + lcell->rect[3] = max_flt(lcell->rect[3], elt->my + elt->hy); + elt_idx = elt->next; + } + } + + for (int c=0; c < grid->loose.num_cells; ++c) + { + // Insert the loose cell to all the tight cells in which + // it now belongs. + LGridLooseCell* lcell = &grid->loose.cells[c]; + const SimdVec4i trect = to_tcell_idx4(grid, simd_loadu4f(lcell->rect)); + for (int ty = trect.data[1]; ty <= trect.data[3]; ++ty) + { + int* tight_row = grid->tight.heads + ty*grid->tight.num_cols; + for (int tx = trect.data[0]; tx <= trect.data[2]; ++tx) + { + const LGridTightCell new_tcell = {tight_row[tx], c}; + tight_row[tx] = grid->tight.cells.insert(new_tcell); + } + } + } +} \ No newline at end of file diff --git a/loose_tight_double_grid.txt b/loose_tight_double_grid.txt new file mode 100644 index 0000000..4707629 --- /dev/null +++ b/loose_tight_double_grid.txt @@ -0,0 +1,933 @@ +// https://stackoverflow.com/questions/41946007/efficient-and-well-explained-implementation-of-a-quadtree-for-2d-collision-det/48330314#48330314 +// https://pastebin.com/swvwTgnd + +// ************************************************************************************ +// SmallList.hpp +// ************************************************************************************ +#ifndef SMALL_LIST_HPP +#define SMALL_LIST_HPP + +#include +#include +#include +#include + +// Stores a random-access sequence of elements similar to vector, but avoids +// heap allocations for small lists. T must be trivially constructible and +// destructible. +template +class SmallList +{ +public: + // Creates an empty list. + SmallList(); + + // Creates a copy of the specified list. + SmallList(const SmallList& other); + + // Copies the specified list. + SmallList& operator=(const SmallList& other); + + // Destroys the list. + ~SmallList(); + + // Returns the number of agents in the list. + int size() const; + + // Returns the nth element. + T& operator[](int n); + + // Returns the nth element in the list. + const T& operator[](int n) const; + + // Returns an index to a matching element in the list or -1 + // if the element is not found. + int find_index(const T& element) const; + + // Clears the list. + void clear(); + + // Reserves space for n elements. + void reserve(int n); + + // Inserts an element to the back of the list. + void push_back(const T& element); + + /// Pops an element off the back of the list. + T pop_back(); + + // Swaps the contents of this list with the other. + void swap(SmallList& other); + + // Returns a pointer to the underlying buffer. + T* data(); + + // Returns a pointer to the underlying buffer. + const T* data() const; + +private: + enum {fixed_cap = 256}; + struct ListData + { + ListData(); + T buf[fixed_cap]; + T* data; + int num; + int cap; + }; + ListData ld; +}; + +/// Provides an indexed free list with constant-time removals from anywhere +/// in the list without invalidating indices. T must be trivially constructible +/// and destructible. +template +class FreeList +{ +public: + /// Creates a new free list. + FreeList(); + + /// Inserts an element to the free list and returns an index to it. + int insert(const T& element); + + // Removes the nth element from the free list. + void erase(int n); + + // Removes all elements from the free list. + void clear(); + + // Returns the range of valid indices. + int range() const; + + // Returns the nth element. + T& operator[](int n); + + // Returns the nth element. + const T& operator[](int n) const; + + // Reserves space for n elements. + void reserve(int n); + + // Swaps the contents of the two lists. + void swap(FreeList& other); + +private: + union FreeElement + { + T element; + int next; + }; + SmallList data; + int first_free; +}; + +// --------------------------------------------------------------------------------- +// SmallList Implementation +// --------------------------------------------------------------------------------- +template +SmallList::ListData::ListData(): data(buf), num(0), cap(fixed_cap) +{ +} + +template +SmallList::SmallList() +{ +} + +template +SmallList::SmallList(const SmallList& other) +{ + if (other.ld.cap == fixed_cap) + { + ld = other.ld; + ld.data = ld.buf; + } + else + { + reserve(other.ld.num); + for (int j=0; j < other.size(); ++j) + ld.data[j] = other.ld.data[j]; + ld.num = other.ld.num; + ld.cap = other.ld.cap; + } +} + +template +SmallList& SmallList::operator=(const SmallList& other) +{ + SmallList(other).swap(*this); + return *this; +} + +template +SmallList::~SmallList() +{ + if (ld.data != ld.buf) + free(ld.data); +} + +template +int SmallList::size() const +{ + return ld.num; +} + +template +T& SmallList::operator[](int n) +{ + assert(n >= 0 && n < ld.num); + return ld.data[n]; +} + +template +const T& SmallList::operator[](int n) const +{ + assert(n >= 0 && n < ld.num); + return ld.data[n]; +} + +template +int SmallList::find_index(const T& element) const +{ + for (int j=0; j < ld.num; ++j) + { + if (ld.data[j] == element) + return j; + } + return -1; +} + +template +void SmallList::clear() +{ + ld.num = 0; +} + +template +void SmallList::reserve(int n) +{ + enum {type_size = sizeof(T)}; + if (n > ld.cap) + { + if (ld.cap == fixed_cap) + { + ld.data = static_cast(malloc(n * type_size)); + memcpy(ld.data, ld.buf, sizeof(ld.buf)); + } + else + ld.data = static_cast(realloc(ld.data, n * type_size)); + ld.cap = n; + } +} + +template +void SmallList::push_back(const T& element) +{ + if (ld.num >= ld.cap) + reserve(ld.cap * 2); + ld.data[ld.num++] = element; +} + +template +T SmallList::pop_back() +{ + return ld.data[--ld.num]; +} + +template +void SmallList::swap(SmallList& other) +{ + ListData& ld1 = ld; + ListData& ld2 = other.ld; + + const int use_fixed1 = ld1.data == ld1.buf; + const int use_fixed2 = ld2.data == ld2.buf; + + const ListData temp = ld1; + ld1 = ld2; + ld2 = temp; + + if (use_fixed1) + ld2.data = ld2.buf; + if (use_fixed2) + ld1.data = ld1.buf; +} + +template +T* SmallList::data() +{ + return ld.data; +} + +template +const T* SmallList::data() const +{ + return ld.data; +} + +// --------------------------------------------------------------------------------- +// FreeList Implementation +// --------------------------------------------------------------------------------- +template +FreeList::FreeList(): first_free(-1) +{ +} + +template +int FreeList::insert(const T& element) +{ + if (first_free != -1) + { + const int index = first_free; + first_free = data[first_free].next; + data[index].element = element; + return index; + } + else + { + FreeElement fe; + fe.element = element; + data.push_back(fe); + return data.size() - 1; + } +} + +template +void FreeList::erase(int n) +{ + assert(n >= 0 && n < data.size()); + data[n].next = first_free; + first_free = n; +} + +template +void FreeList::clear() +{ + data.clear(); + first_free = -1; +} + +template +int FreeList::range() const +{ + return data.size(); +} + +template +T& FreeList::operator[](int n) +{ + return data[n].element; +} + +template +const T& FreeList::operator[](int n) const +{ + return data[n].element; +} + +template +void FreeList::reserve(int n) +{ + data.reserve(n); +} + +template +void FreeList::swap(FreeList& other) +{ + const int temp = first_free; + data.swap(other.data); + first_free = other.first_free; + other.first_free = temp; +} + +#endif + +// ************************************************************************************ +// LGrid.hpp +// ************************************************************************************ +#ifndef LGRID_HPP +#define LGRID_HPP + +#include "SmallList.hpp" + +struct LGridQuery4 +{ + // Stores the resulting elements of the SIMD query. + SmallList elements[4]; +}; + +struct LGridElt +{ + // Stores the index to the next element in the loose cell using an indexed SLL. + int next; + + // Stores the ID of the element. This can be used to associate external + // data to the element. + int id; + + // Stores the center of the element. + float mx, my; + + // Stores the half-size of the element relative to the upper-left corner + // of the grid. + float hx, hy; +}; + +struct LGridLooseCell +{ + // Stores the extents of the grid cell relative to the upper-left corner + // of the grid which expands and shrinks with the elements inserted and + // removed. + float rect[4]; + + // Stores the index to the first element using an indexed SLL. + int head; +}; + +struct LGridLoose +{ + // Stores all the cells in the loose grid. + LGridLooseCell* cells; + + // Stores the number of columns, rows, and cells in the loose grid. + int num_cols, num_rows, num_cells; + + // Stores the inverse size of a loose cell. + float inv_cell_w, inv_cell_h; +}; + +struct LGridTightCell +{ + // Stores the index to the next loose cell in the grid cell. + int next; + + // Stores the position of the loose cell in the grid. + int lcell; +}; + +struct LGridTight +{ + // Stores all the tight cell nodes in the grid. + FreeList cells; + + // Stores the tight cell heads. + int* heads; + + // Stores the number of columns, rows, and cells in the tight grid. + int num_cols, num_rows, num_cells; + + // Stores the inverse size of a tight cell. + float inv_cell_w, inv_cell_h; +}; + +struct LGrid +{ + // Stores the tight cell data for the grid. + LGridTight tight; + + // Stores the loose cell data for the grid. + LGridLoose loose; + + // Stores all the elements in the grid. + FreeList elts; + + // Stores the number of elements in the grid. + int num_elts; + + // Stores the upper-left corner of the grid. + float x, y; + + // Stores the size of the grid. + float w, h; +}; + +// Creates a loose grid encompassing the specified extents using the specified cell +// size. Elements inserted to the loose grid are only inserted in one cell, but the +// extents of each cell are allowed to expand and shrink. To avoid requiring every +// loose cell to be checked during a search, a second grid of tight cells referencing +// the loose cells is stored. +LGrid* lgrid_create(float lcell_w, float lcell_h, float tcell_w, float tcell_h, + float l, float t, float r, float b); + +// Destroys the grid. +void lgrid_destroy(LGrid* grid); + +// Returns the grid cell index for the specified position. +int lgrid_lcell_idx(LGrid* grid, float x, float y); + +// Inserts an element to the grid. +void lgrid_insert(LGrid* grid, int id, float mx, float my, float hx, float hy); + +// Removes an element from the grid. +void lgrid_remove(LGrid* grid, int id, float mx, float my); + +// Moves an element in the grid from the former position to the new one. +void lgrid_move(LGrid* grid, int id, float prev_mx, float prev_my, float mx, float my); + +// Returns all the element IDs that intersect the specified rectangle excluding elements +// with the specified ID to omit. +SmallList lgrid_query(const LGrid* grid, float mx, float my, float hx, float hy, int omit_id); + +// Returns all the element IDs that intersect the specified 4 rectangles excluding elements +// with the specified IDs to omit. +LGridQuery4 lgrid_query4(const LGrid* grid, const SimdVec4f* mx4, const SimdVec4f* my4, + const SimdVec4f* hx4, const SimdVec4f* hy4, const SimdVec4i* omit_id4); + +// Returns true if the specified rectangle is inside the grid boundaries. +bool lgrid_in_bounds(const LGrid* grid, float mx, float my, float hx, float hy); + +// Optimizes the grid, shrinking bounding boxes in response to removed elements and +// rearranging the memory of the grid to allow cache-friendly cell traversal. +void lgrid_optimize(LGrid* grid); + +#endif + +// ************************************************************************************ +// LGrid.cpp +// ************************************************************************************ +#include "LGrid.hpp" +#include +#include +#include + +static int ceil_div(float value, float divisor) +{ + // Returns the value divided by the divisor rounded up. + const float resultf = value / divisor; + const int result = (int)resultf; + return result < resultf ? result+1: result; +} + +static int min_int(int a, int b) +{ + assert(sizeof(int) == 4); + a -= b; + a &= a >> 31; + return a + b; +} + +static int max_int(int a, int b) +{ + assert(sizeof(int) == 4); + a -= b; + a &= (~a) >> 31; + return a + b; +} + +static float min_flt(float a, float b) +{ + return std::min(a, b); +} + +static float max_flt(float a, float b) +{ + return std::max(a, b); +} + +static int to_cell_idx(float val, float inv_cell_size, int num_cells) +{ + const int cell_pos = (int)(val * inv_cell_size); + return min_int(max_int(cell_pos, 0), num_cells - 1); +} + +static SimdVec4i to_tcell_idx4(const LGrid* grid, __m128 rect) +{ + __m128 inv_cell_size_vec = simd_create4f(grid->tight.inv_cell_w, grid->tight.inv_cell_h, + grid->tight.inv_cell_w, grid->tight.inv_cell_h); + __m128 cell_xyf_vec = simd_mul4f(rect, inv_cell_size_vec); + __m128i clamp_vec = simd_create4i(grid->tight.num_cols-1, grid->tight.num_rows-1, + grid->tight.num_cols-1, grid->tight.num_rows-1); + __m128i cell_xy_vec = simd_clamp4i(simd_ftoi4f(cell_xyf_vec), simd_zero4i(), clamp_vec); + return simd_store4i(cell_xy_vec); +} + +static void grid_optimize(LGrid* grid) +{ + FreeList new_elts; + new_elts.reserve(grid->num_elts); + for (int c=0; c < grid->loose.num_cells; ++c) + { + // Replace links to the old elements list to links in the new + // cache-friendly element list. + SmallList new_elt_idxs; + LGridLooseCell* lcell = &grid->loose.cells[c]; + while (lcell->head != -1) + { + const LGridElt* elt = &grid->elts[lcell->head]; + new_elt_idxs.push_back(new_elts.insert(*elt)); + lcell->head = elt->next; + } + for (int j=0; j < new_elt_idxs.size(); ++j) + { + const int new_elt_idx = new_elt_idxs[j]; + new_elts[new_elt_idx].next = lcell->head; + lcell->head = new_elt_idx; + } + } + // Swap the new element list with the old one. + grid->elts.swap(new_elts); +} + +static void expand_aabb(LGrid* grid, int cell_idx, float mx, float my, float hx, float hy) +{ + LGridLooseCell* lcell = &grid->loose.cells[cell_idx]; + const SimdVec4f prev_rect = {lcell->rect[0], lcell->rect[1], lcell->rect[2], lcell->rect[3]}; + lcell->rect[0] = min_flt(lcell->rect[0], mx - hx); + lcell->rect[1] = min_flt(lcell->rect[1], my - hx); + lcell->rect[2] = max_flt(lcell->rect[2], mx + hx); + lcell->rect[3] = max_flt(lcell->rect[3], my + hy); + + // Determine the cells occupied by the loose cell in the tight grid. + const SimdVec4f elt_rect = {mx-hx, my-hy, mx+hx, my+hy}; + const SimdVec4i trect = to_tcell_idx4(grid, simd_load4f(&elt_rect)); + + if (prev_rect.data[0] > prev_rect.data[2]) + { + // If the loose cell was empty, simply insert the loose cell + // to all the tight cells it occupies. We don't need to check + // to see if it was already inserted. + for (int ty = trect.data[1]; ty <= trect.data[3]; ++ty) + { + int* tight_row = grid->tight.heads + ty*grid->tight.num_cols; + for (int tx = trect.data[0]; tx <= trect.data[2]; ++tx) + { + const LGridTightCell new_tcell = {tight_row[tx], cell_idx}; + tight_row[tx] = grid->tight.cells.insert(new_tcell); + } + } + } + else + { + // Only perform the insertion if the loose cell overlaps new tight cells. + const SimdVec4i prev_trect = to_tcell_idx4(grid, simd_load4f(&prev_rect)); + if (trect.data[0] != prev_trect.data[0] || trect.data[1] != prev_trect.data[1] || + trect.data[2] != prev_trect.data[2] || trect.data[3] != prev_trect.data[3]) + { + for (int ty = trect.data[1]; ty <= trect.data[3]; ++ty) + { + int* tight_row = grid->tight.heads + ty*grid->tight.num_cols; + for (int tx = trect.data[0]; tx <= trect.data[2]; ++tx) + { + if (tx < prev_trect.data[0] || tx > prev_trect.data[2] || + ty < prev_trect.data[1] || ty > prev_trect.data[3]) + { + const LGridTightCell new_tcell = {tight_row[tx], cell_idx}; + tight_row[tx] = grid->tight.cells.insert(new_tcell); + } + } + } + } + } +} + +static __m128 element_rect(const LGridElt* elt) +{ + return simd_create4f(elt->mx-elt->hx, elt->my-elt->hy, + elt->mx+elt->hx, elt->my+elt->hy); +} + +LGrid* lgrid_create(float lcell_w, float lcell_h, float tcell_w, float tcell_h, + float l, float t, float r, float b) +{ + const float w = r - l, h = b - t; + const int num_lcols = ceil_div(w, lcell_w), num_lrows = ceil_div(h, lcell_h); + const int num_tcols = ceil_div(w, tcell_w), num_trows = ceil_div(h, tcell_h); + + LGrid* grid = new LGrid; + grid->num_elts = 0; + grid->x = l; + grid->y = t; + grid->h = w; + grid->w = h; + + grid->loose.num_cols = num_lcols; + grid->loose.num_rows = num_lrows; + grid->loose.num_cells = grid->loose.num_cols * grid->loose.num_rows; + grid->loose.inv_cell_w = 1.0f / lcell_w; + grid->loose.inv_cell_h = 1.0f / lcell_h; + + grid->tight.num_cols = num_tcols; + grid->tight.num_rows = num_trows; + grid->tight.num_cells = grid->tight.num_cols * grid->tight.num_rows; + grid->tight.inv_cell_w = 1.0f / tcell_w; + grid->tight.inv_cell_h = 1.0f / tcell_h; + + // Initialize tight cell heads with -1 to indicate empty indexed SLLs. + grid->tight.heads = new int[grid->tight.num_cells]; + for (int j=0; j < grid->tight.num_cells; ++j) + grid->tight.heads[j] = -1; + + // Initialize all the loose cells. + grid->loose.cells = new LGridLooseCell[grid->loose.num_cells]; + for (int c=0; c < grid->loose.num_cells; ++c) + { + grid->loose.cells[c].head = -1; + grid->loose.cells[c].rect[0] = FLT_MAX; + grid->loose.cells[c].rect[1] = FLT_MAX; + grid->loose.cells[c].rect[2] = -FLT_MAX; + grid->loose.cells[c].rect[3] = -FLT_MAX; + } + return grid; +} + +void lgrid_destroy(LGrid* grid) +{ + delete[] grid->loose.cells; + delete[] grid->tight.heads; + delete grid; +} + +int lgrid_lcell_idx(LGrid* grid, float x, float y) +{ + const int cell_x = to_cell_idx(x - grid->x, grid->loose.inv_cell_w, grid->loose.num_cols); + const int cell_y = to_cell_idx(y - grid->y, grid->loose.inv_cell_h, grid->loose.num_rows); + return cell_y * grid->loose.num_cols + cell_x; +} + +void lgrid_insert(LGrid* grid, int id, float mx, float my, float hx, float hy) +{ + const int cell_idx = lgrid_lcell_idx(grid, mx, my); + LGridLooseCell* lcell = &grid->loose.cells[cell_idx]; + + // Insert the element to the appropriate loose cell and row. + const LGridElt new_elt = {lcell->head, id, mx - grid->x, my - grid->y, hx, hy}; + lcell->head = grid->elts.insert(new_elt); + ++grid->num_elts; + + // Expand the loose cell's bounding box to fit the new element. + expand_aabb(grid, cell_idx, mx, my, hx, hy); +} + +void lgrid_remove(LGrid* grid, int id, float mx, float my) +{ + // Find the element in the loose cell. + LGridLooseCell* lcell = &grid->loose.cells[lgrid_lcell_idx(grid, mx, my)]; + int* link = &lcell->head; + while (grid->elts[*link].id != id) + link = &grid->elts[*link].next; + + // Remove the element from the loose cell and row. + const int elt_idx = *link; + *link = grid->elts[elt_idx].next; + grid->elts.erase(elt_idx); + --grid->num_elts; +} + +void lgrid_move(LGrid* grid, int id, float prev_mx, float prev_my, float mx, float my) +{ + const int prev_cell_idx = lgrid_lcell_idx(grid, prev_mx, prev_my); + const int new_cell_idx = lgrid_lcell_idx(grid, mx, my); + LGridLooseCell* lcell = &grid->loose.cells[prev_cell_idx]; + + if (prev_cell_idx == new_cell_idx) + { + // Find the element in the loose cell. + int elt_idx = lcell->head; + while (grid->elts[elt_idx].id != id) + elt_idx = grid->elts[elt_idx].next; + + // Since the element is still inside the same cell, we can simply overwrite + // its position and expand the loose cell's AABB. + mx -= grid->x; + my -= grid->y; + grid->elts[elt_idx].mx = mx; + grid->elts[elt_idx].my = my; + expand_aabb(grid, prev_cell_idx, mx, my, grid->elts[elt_idx].hx, grid->elts[elt_idx].hy); + } + else + { + // Find the element in the loose cell. + int* link = &lcell->head; + while (grid->elts[*link].id != id) + link = &grid->elts[*link].next; + + const int elt_idx = *link; + const float hx = grid->elts[elt_idx].hx; + const float hy = grid->elts[elt_idx].hy; + + // If the element has moved into a different loose cell, remove + // remove the element from the previous loose cell and row. + *link = grid->elts[elt_idx].next; + grid->elts.erase(elt_idx); + --grid->num_elts; + + // Now insert the element to its new position. + lgrid_insert(grid, id, mx, my, hx, hy); + } +} + +SmallList lgrid_query(const LGrid* grid, float mx, float my, float hx, float hy, int omit_id) +{ + mx -= grid->x; + my -= grid->y; + + // Compute the tight cell extents [min_tx, min_ty, max_tx, max_ty]. + const SimdVec4f qrect = {mx-hx, my-hy, mx+hx, my+hy}; + __m128 qrect_vec = simd_load4f(&qrect); + const SimdVec4i trect = to_tcell_idx4(grid, qrect_vec); + + // Gather the intersecting loose cells in the tight cells that intersect. + SmallList lcell_idxs; + for (int ty = trect.data[1]; ty <= trect.data[3]; ++ty) + { + const int* tight_row = grid->tight.heads + ty*grid->tight.num_cols; + for (int tx = trect.data[0]; tx <= trect.data[2]; ++tx) + { + // Iterate through the loose cells that intersect the tight cells. + int tcell_idx = tight_row[tx]; + while (tcell_idx != -1) + { + const LGridTightCell* tcell = &grid->tight.cells[tcell_idx]; + const LGridLooseCell* lcell = &grid->loose.cells[tcell->lcell]; + if (lcell_idxs.find_index(tcell->lcell) == -1 && simd_rect_intersect4f(qrect_vec, simd_loadu4f(lcell->rect))) + lcell_idxs.push_back(tcell->lcell); + tcell_idx = tcell->next; + } + } + } + + // For each loose cell, determine what elements intersect. + SmallList res; + for (int j=0; j < lcell_idxs.size(); ++j) + { + const LGridLooseCell* lcell = &grid->loose.cells[lcell_idxs[j]]; + int elt_idx = lcell->head; + while (elt_idx != -1) + { + // If the element intersects the search rectangle, add it to the + // resulting elements unless it has an ID that should be omitted. + const LGridElt* elt = &grid->elts[elt_idx]; + if (elt->id != omit_id && simd_rect_intersect4f(qrect_vec, element_rect(elt))) + res.push_back(elt->id); + elt_idx = elt->next; + } + } + return res; +} + +LGridQuery4 lgrid_query4(const LGrid* grid, const SimdVec4f* mx4, const SimdVec4f* my4, + const SimdVec4f* hx4, const SimdVec4f* hy4, const SimdVec4i* omit_id4) +{ + __m128 hx_vec = simd_load4f(hx4), hy_vec = simd_load4f(hy4); + __m128 mx_vec = simd_sub4f(simd_load4f(mx4), simd_scalar4f(grid->x)); + __m128 my_vec = simd_sub4f(simd_load4f(my4), simd_scalar4f(grid->y)); + __m128 ql_vec = simd_sub4f(mx_vec, hx_vec), qt_vec = simd_sub4f(my_vec, hy_vec); + __m128 qr_vec = simd_add4f(mx_vec, hx_vec), qb_vec = simd_add4f(my_vec, hy_vec); + + __m128 inv_cell_w_vec = simd_scalar4f(grid->tight.inv_cell_w), inv_cell_h_vec = simd_scalar4f(grid->tight.inv_cell_h); + __m128i max_x_vec = simd_scalar4i(grid->tight.num_cols-1), max_y_vec = simd_scalar4i(grid->tight.num_rows-1); + __m128i tmin_x_vec = simd_clamp4i(simd_ftoi4f(simd_mul4f(ql_vec, inv_cell_w_vec)), simd_zero4i(), max_x_vec); + __m128i tmin_y_vec = simd_clamp4i(simd_ftoi4f(simd_mul4f(qt_vec, inv_cell_h_vec)), simd_zero4i(), max_y_vec); + __m128i tmax_x_vec = simd_clamp4i(simd_ftoi4f(simd_mul4f(qr_vec, inv_cell_w_vec)), simd_zero4i(), max_x_vec); + __m128i tmax_y_vec = simd_clamp4i(simd_ftoi4f(simd_mul4f(qb_vec, inv_cell_h_vec)), simd_zero4i(), max_y_vec); + + const SimdVec4i tmin_x4 = simd_store4i(tmin_x_vec), tmin_y4 = simd_store4i(tmin_y_vec); + const SimdVec4i tmax_x4 = simd_store4i(tmax_x_vec), tmax_y4 = simd_store4i(tmax_y_vec); + const SimdVec4f ql4 = simd_store4f(ql_vec), qt4 = simd_store4f(qt_vec); + const SimdVec4f qr4 = simd_store4f(qr_vec), qb4 = simd_store4f(qb_vec); + + LGridQuery4 res4; + for (int k=0; k < 4; ++k) + { + const int trect[4] = {tmin_x4.data[k], tmin_y4.data[k], tmax_x4.data[k], tmax_y4.data[k]}; + const int omit_id = omit_id4->data[k]; + + // Gather the intersecting loose cells in the tight cells that intersect. + SmallList lcell_idxs; + __m128 qrect_vec = simd_create4f(ql4.data[k], qt4.data[k], qr4.data[k], qb4.data[k]); + for (int ty = trect[1]; ty <= trect[3]; ++ty) + { + const int* tight_row = grid->tight.heads + ty*grid->tight.num_cols; + for (int tx = trect[0]; tx <= trect[2]; ++tx) + { + // Iterate through the loose cells that intersect the tight cells. + int tcell_idx = tight_row[tx]; + while (tcell_idx != -1) + { + const LGridTightCell* tcell = &grid->tight.cells[tcell_idx]; + if (lcell_idxs.find_index(tcell->lcell) && simd_rect_intersect4f(qrect_vec, simd_loadu4f(grid->loose.cells[tcell->lcell].rect))) + lcell_idxs.push_back(tcell->lcell); + tcell_idx = tcell->next; + } + } + } + + // For each loose cell, determine what elements intersect. + for (int j=0; j < lcell_idxs.size(); ++j) + { + const LGridLooseCell* lcell = &grid->loose.cells[lcell_idxs[j]]; + int elt_idx = lcell->head; + while (elt_idx != -1) + { + // If the element intersects the search rectangle, add it to the + // resulting elements unless it has an ID that should be omitted. + const LGridElt* elt = &grid->elts[elt_idx]; + if (elt->id != omit_id && simd_rect_intersect4f(qrect_vec, element_rect(elt))) + res4.elements[k].push_back(elt->id); + elt_idx = elt->next; + } + } + } + return res4; +} + +bool lgrid_in_bounds(const LGrid* grid, float mx, float my, float hx, float hy) +{ + mx -= grid->x; + my -= grid->y; + const float x1 = mx-hx, y1 = my-hy, x2 = mx+hx, y2 = my+hy; + return x1 >= 0.0f && x2 < grid->w && y1 >= 0.0f && y2 < grid->h; +} + +void lgrid_optimize(LGrid* grid) +{ + // Clear all the tight cell data. + for (int j=0; j < grid->tight.num_cells; ++j) + grid->tight.heads[j] = -1; + grid->tight.cells.clear(); + + // Optimize the memory layout of the grid. + grid_optimize(grid); + + #pragma omp parallel for + for (int c=0; c < grid->loose.num_cells; ++c) + { + // Empty the loose cell's bounding box. + LGridLooseCell* lcell = &grid->loose.cells[c]; + lcell->rect[0] = FLT_MAX; + lcell->rect[1] = FLT_MAX; + lcell->rect[2] = -FLT_MAX; + lcell->rect[3] = -FLT_MAX; + + // Expand the bounding box by each element's extents in + // the loose cell. + int elt_idx = lcell->head; + while (elt_idx != -1) + { + const LGridElt* elt = &grid->elts[elt_idx]; + lcell->rect[0] = min_flt(lcell->rect[0], elt->mx - elt->hx); + lcell->rect[1] = min_flt(lcell->rect[1], elt->my - elt->hy); + lcell->rect[2] = max_flt(lcell->rect[2], elt->mx + elt->hx); + lcell->rect[3] = max_flt(lcell->rect[3], elt->my + elt->hy); + elt_idx = elt->next; + } + } + + for (int c=0; c < grid->loose.num_cells; ++c) + { + // Insert the loose cell to all the tight cells in which + // it now belongs. + LGridLooseCell* lcell = &grid->loose.cells[c]; + const SimdVec4i trect = to_tcell_idx4(grid, simd_loadu4f(lcell->rect)); + for (int ty = trect.data[1]; ty <= trect.data[3]; ++ty) + { + int* tight_row = grid->tight.heads + ty*grid->tight.num_cols; + for (int tx = trect.data[0]; tx <= trect.data[2]; ++tx) + { + const LGridTightCell new_tcell = {tight_row[tx], c}; + tight_row[tx] = grid->tight.cells.insert(new_tcell); + } + } + } +} \ No newline at end of file