Advertisement
Guest User

Untitled

a guest
Mar 11th, 2020
3,284
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 28.76 KB | None | 0 0
  1. // ************************************************************************************
  2. // SmallList.hpp
  3. // ************************************************************************************
  4. #ifndef SMALL_LIST_HPP
  5. #define SMALL_LIST_HPP
  6.  
  7. #include <cstdlib>
  8. #include <cstring>
  9. #include <cassert>
  10. #include <vector>
  11.  
  12. // Stores a random-access sequence of elements similar to vector, but avoids
  13. // heap allocations for small lists. T must be trivially constructible and
  14. // destructible.
  15. template <class T>
  16. class SmallList
  17. {
  18. public:
  19.     // Creates an empty list.
  20.     SmallList();
  21.  
  22.     // Creates a copy of the specified list.
  23.     SmallList(const SmallList& other);
  24.  
  25.     // Copies the specified list.
  26.     SmallList& operator=(const SmallList& other);
  27.  
  28.     // Destroys the list.
  29.     ~SmallList();
  30.  
  31.     // Returns the number of agents in the list.
  32.     int size() const;
  33.  
  34.     // Returns the nth element.
  35.     T& operator[](int n);
  36.  
  37.     // Returns the nth element in the list.
  38.     const T& operator[](int n) const;
  39.  
  40.     // Returns an index to a matching element in the list or -1
  41.     // if the element is not found.
  42.     int find_index(const T& element) const;
  43.  
  44.     // Clears the list.
  45.     void clear();
  46.  
  47.     // Reserves space for n elements.
  48.     void reserve(int n);
  49.  
  50.     // Inserts an element to the back of the list.
  51.     void push_back(const T& element);
  52.  
  53.     /// Pops an element off the back of the list.
  54.     T pop_back();
  55.  
  56.     // Swaps the contents of this list with the other.
  57.     void swap(SmallList& other);
  58.  
  59.     // Returns a pointer to the underlying buffer.
  60.     T* data();
  61.  
  62.     // Returns a pointer to the underlying buffer.
  63.     const T* data() const;
  64.  
  65. private:
  66.     enum {fixed_cap = 256};
  67.     struct ListData
  68.     {
  69.         ListData();
  70.         T buf[fixed_cap];
  71.         T* data;
  72.         int num;
  73.         int cap;
  74.     };
  75.     ListData ld;
  76. };
  77.  
  78. /// Provides an indexed free list with constant-time removals from anywhere
  79. /// in the list without invalidating indices. T must be trivially constructible
  80. /// and destructible.
  81. template <class T>
  82. class FreeList
  83. {
  84. public:
  85.     /// Creates a new free list.
  86.     FreeList();
  87.  
  88.     /// Inserts an element to the free list and returns an index to it.
  89.     int insert(const T& element);
  90.  
  91.     // Removes the nth element from the free list.
  92.     void erase(int n);
  93.  
  94.     // Removes all elements from the free list.
  95.     void clear();
  96.  
  97.     // Returns the range of valid indices.
  98.     int range() const;
  99.  
  100.     // Returns the nth element.
  101.     T& operator[](int n);
  102.  
  103.     // Returns the nth element.
  104.     const T& operator[](int n) const;
  105.  
  106.     // Reserves space for n elements.
  107.     void reserve(int n);
  108.  
  109.     // Swaps the contents of the two lists.
  110.     void swap(FreeList& other);
  111.  
  112. private:
  113.     union FreeElement
  114.     {
  115.         T element;
  116.         int next;
  117.     };
  118.     SmallList<FreeElement> data;
  119.     int first_free;
  120. };
  121.  
  122. // ---------------------------------------------------------------------------------
  123. // SmallList Implementation
  124. // ---------------------------------------------------------------------------------
  125. template <class T>
  126. SmallList<T>::ListData::ListData(): data(buf), num(0), cap(fixed_cap)
  127. {
  128. }
  129.  
  130. template <class T>
  131. SmallList<T>::SmallList()
  132. {
  133. }
  134.  
  135. template <class T>
  136. SmallList<T>::SmallList(const SmallList& other)
  137. {
  138.     if (other.ld.cap == fixed_cap)
  139.     {
  140.         ld = other.ld;
  141.         ld.data = ld.buf;
  142.     }
  143.     else
  144.     {
  145.         reserve(other.ld.num);
  146.         for (int j=0; j < other.size(); ++j)
  147.             ld.data[j] = other.ld.data[j];
  148.         ld.num = other.ld.num;
  149.         ld.cap = other.ld.cap;
  150.     }
  151. }
  152.  
  153. template <class T>
  154. SmallList<T>& SmallList<T>::operator=(const SmallList<T>& other)
  155. {
  156.     SmallList(other).swap(*this);
  157.     return *this;
  158. }
  159.  
  160. template <class T>
  161. SmallList<T>::~SmallList()
  162. {
  163.     if (ld.data != ld.buf)
  164.         free(ld.data);
  165. }
  166.  
  167. template <class T>
  168. int SmallList<T>::size() const
  169. {
  170.     return ld.num;
  171. }
  172.  
  173. template <class T>
  174. T& SmallList<T>::operator[](int n)
  175. {
  176.     assert(n >= 0 && n < ld.num);
  177.     return ld.data[n];
  178. }
  179.  
  180. template <class T>
  181. const T& SmallList<T>::operator[](int n) const
  182. {
  183.     assert(n >= 0 && n < ld.num);
  184.     return ld.data[n];
  185. }
  186.  
  187. template <class T>
  188. int SmallList<T>::find_index(const T& element) const
  189. {
  190.     for (int j=0; j < ld.num; ++j)
  191.     {
  192.         if (ld.data[j] == element)
  193.             return j;
  194.     }
  195.     return -1;
  196. }
  197.  
  198. template <class T>
  199. void SmallList<T>::clear()
  200. {
  201.     ld.num = 0;
  202. }
  203.  
  204. template <class T>
  205. void SmallList<T>::reserve(int n)
  206. {
  207.     enum {type_size = sizeof(T)};
  208.     if (n > ld.cap)
  209.     {
  210.         if (ld.cap == fixed_cap)
  211.         {
  212.             ld.data = static_cast<T*>(malloc(n * type_size));
  213.             memcpy(ld.data, ld.buf, sizeof(ld.buf));
  214.         }
  215.         else
  216.             ld.data = static_cast<T*>(realloc(ld.data, n * type_size));
  217.         ld.cap = n;
  218.     }
  219. }
  220.  
  221. template <class T>
  222. void SmallList<T>::push_back(const T& element)
  223. {
  224.     if (ld.num >= ld.cap)
  225.         reserve(ld.cap * 2);
  226.     ld.data[ld.num++] = element;
  227. }
  228.  
  229. template <class T>
  230. T SmallList<T>::pop_back()
  231. {
  232.     return ld.data[--ld.num];
  233. }
  234.  
  235. template <class T>
  236. void SmallList<T>::swap(SmallList& other)
  237. {
  238.     ListData& ld1 = ld;
  239.     ListData& ld2 = other.ld;
  240.  
  241.     const int use_fixed1 = ld1.data == ld1.buf;
  242.     const int use_fixed2 = ld2.data == ld2.buf;
  243.  
  244.     const ListData temp = ld1;
  245.     ld1 = ld2;
  246.     ld2 = temp;
  247.  
  248.     if (use_fixed1)
  249.         ld2.data = ld2.buf;
  250.     if (use_fixed2)
  251.         ld1.data = ld1.buf;
  252. }
  253.  
  254. template <class T>
  255. T* SmallList<T>::data()
  256. {
  257.     return ld.data;
  258. }
  259.  
  260. template <class T>
  261. const T* SmallList<T>::data() const
  262. {
  263.     return ld.data;
  264. }
  265.  
  266. // ---------------------------------------------------------------------------------
  267. // FreeList Implementation
  268. // ---------------------------------------------------------------------------------
  269. template <class T>
  270. FreeList<T>::FreeList(): first_free(-1)
  271. {
  272. }
  273.  
  274. template <class T>
  275. int FreeList<T>::insert(const T& element)
  276. {
  277.     if (first_free != -1)
  278.     {
  279.         const int index = first_free;
  280.         first_free = data[first_free].next;
  281.         data[index].element = element;
  282.         return index;
  283.     }
  284.     else
  285.     {
  286.         FreeElement fe;
  287.         fe.element = element;
  288.         data.push_back(fe);
  289.         return data.size() - 1;
  290.     }
  291. }
  292.  
  293. template <class T>
  294. void FreeList<T>::erase(int n)
  295. {
  296.     assert(n >= 0 && n < data.size());
  297.     data[n].next = first_free;
  298.     first_free = n;
  299. }
  300.  
  301. template <class T>
  302. void FreeList<T>::clear()
  303. {
  304.     data.clear();
  305.     first_free = -1;
  306. }
  307.  
  308. template <class T>
  309. int FreeList<T>::range() const
  310. {
  311.     return data.size();
  312. }
  313.  
  314. template <class T>
  315. T& FreeList<T>::operator[](int n)
  316. {
  317.     return data[n].element;
  318. }
  319.  
  320. template <class T>
  321. const T& FreeList<T>::operator[](int n) const
  322. {
  323.     return data[n].element;
  324. }
  325.  
  326. template <class T>
  327. void FreeList<T>::reserve(int n)
  328. {
  329.     data.reserve(n);
  330. }
  331.  
  332. template <class T>
  333. void FreeList<T>::swap(FreeList& other)
  334. {
  335.     const int temp = first_free;
  336.     data.swap(other.data);
  337.     first_free = other.first_free;
  338.     other.first_free = temp;
  339. }
  340.  
  341. #endif
  342.  
  343. // ************************************************************************************
  344. // LGrid.hpp
  345. // ************************************************************************************
  346. #ifndef LGRID_HPP
  347. #define LGRID_HPP
  348.  
  349. #include "SmallList.hpp"
  350.  
  351. struct LGridQuery4
  352. {
  353.     // Stores the resulting elements of the SIMD query.
  354.     SmallList<int> elements[4];
  355. };
  356.  
  357. struct LGridElt
  358. {
  359.     // Stores the index to the next element in the loose cell using an indexed SLL.
  360.     int next;
  361.  
  362.     // Stores the ID of the element. This can be used to associate external
  363.     // data to the element.
  364.     int id;
  365.  
  366.     // Stores the center of the element.
  367.     float mx, my;
  368.  
  369.     // Stores the half-size of the element relative to the upper-left corner
  370.     // of the grid.
  371.     float hx, hy;
  372. };
  373.  
  374. struct LGridLooseCell
  375. {
  376.     // Stores the extents of the grid cell relative to the upper-left corner
  377.     // of the grid which expands and shrinks with the elements inserted and
  378.     // removed.
  379.     float rect[4];
  380.  
  381.     // Stores the index to the first element using an indexed SLL.
  382.     int head;
  383. };
  384.  
  385. struct LGridLoose
  386. {
  387.     // Stores all the cells in the loose grid.
  388.     LGridLooseCell* cells;
  389.  
  390.     // Stores the number of columns, rows, and cells in the loose grid.
  391.     int num_cols, num_rows, num_cells;
  392.  
  393.     // Stores the inverse size of a loose cell.
  394.     float inv_cell_w, inv_cell_h;
  395. };
  396.  
  397. struct LGridTightCell
  398. {
  399.     // Stores the index to the next loose cell in the grid cell.
  400.     int next;
  401.  
  402.     // Stores the position of the loose cell in the grid.
  403.     int lcell;
  404. };
  405.  
  406. struct LGridTight
  407. {
  408.     // Stores all the tight cell nodes in the grid.
  409.     FreeList<LGridTightCell> cells;
  410.  
  411.     // Stores the tight cell heads.
  412.     int* heads;
  413.  
  414.     // Stores the number of columns, rows, and cells in the tight grid.
  415.     int num_cols, num_rows, num_cells;
  416.  
  417.     // Stores the inverse size of a tight cell.
  418.     float inv_cell_w, inv_cell_h;
  419. };
  420.  
  421. struct LGrid
  422. {
  423.     // Stores the tight cell data for the grid.
  424.     LGridTight tight;
  425.  
  426.     // Stores the loose cell data for the grid.
  427.     LGridLoose loose;
  428.  
  429.     // Stores all the elements in the grid.
  430.     FreeList<LGridElt> elts;
  431.  
  432.     // Stores the number of elements in the grid.
  433.     int num_elts;
  434.  
  435.     // Stores the upper-left corner of the grid.
  436.     float x, y;
  437.  
  438.     // Stores the size of the grid.
  439.     float w, h;
  440. };
  441.  
  442. // Creates a loose grid encompassing the specified extents using the specified cell
  443. // size. Elements inserted to the loose grid are only inserted in one cell, but the
  444. // extents of each cell are allowed to expand and shrink. To avoid requiring every
  445. // loose cell to be checked during a search, a second grid of tight cells referencing
  446. // the loose cells is stored.
  447. LGrid* lgrid_create(float lcell_w, float lcell_h, float tcell_w, float tcell_h,
  448.                     float l, float t, float r, float b);
  449.  
  450. // Destroys the grid.
  451. void lgrid_destroy(LGrid* grid);
  452.  
  453. // Returns the grid cell index for the specified position.
  454. int lgrid_lcell_idx(LGrid* grid, float x, float y);
  455.  
  456. // Inserts an element to the grid.
  457. void lgrid_insert(LGrid* grid, int id, float mx, float my, float hx, float hy);
  458.  
  459. // Removes an element from the grid.
  460. void lgrid_remove(LGrid* grid, int id, float mx, float my);
  461.  
  462. // Moves an element in the grid from the former position to the new one.
  463. void lgrid_move(LGrid* grid, int id, float prev_mx, float prev_my, float mx, float my);
  464.  
  465. // Returns all the element IDs that intersect the specified rectangle excluding elements
  466. // with the specified ID to omit.
  467. SmallList<int> lgrid_query(const LGrid* grid, float mx, float my, float hx, float hy, int omit_id);
  468.  
  469. // Returns all the element IDs that intersect the specified 4 rectangles excluding elements
  470. // with the specified IDs to omit.
  471. LGridQuery4 lgrid_query4(const LGrid* grid, const SimdVec4f* mx4, const SimdVec4f* my4,
  472.                          const SimdVec4f* hx4, const SimdVec4f* hy4, const SimdVec4i* omit_id4);
  473.  
  474. // Returns true if the specified rectangle is inside the grid boundaries.
  475. bool lgrid_in_bounds(const LGrid* grid, float mx, float my, float hx, float hy);
  476.  
  477. // Optimizes the grid, shrinking bounding boxes in response to removed elements and
  478. // rearranging the memory of the grid to allow cache-friendly cell traversal.
  479. void lgrid_optimize(LGrid* grid);
  480.  
  481. #endif
  482.  
  483. // ************************************************************************************
  484. // LGrid.cpp
  485. // ************************************************************************************
  486. #include "LGrid.hpp"
  487. #include <cstdlib>
  488. #include <cfloat>
  489. #include <utility>
  490.  
  491. static int ceil_div(float value, float divisor)
  492. {
  493.     // Returns the value divided by the divisor rounded up.
  494.     const float resultf = value / divisor;
  495.     const int result = (int)resultf;
  496.     return result < resultf ? result+1: result;
  497. }
  498.  
  499. static int min_int(int a, int b)
  500. {
  501.     assert(sizeof(int) == 4);
  502.     a -= b;
  503.     a &= a >> 31;
  504.     return a + b;
  505. }
  506.  
  507. static int max_int(int a, int b)
  508. {
  509.     assert(sizeof(int) == 4);
  510.     a -= b;
  511.     a &= (~a) >> 31;
  512.     return a + b;
  513. }
  514.  
  515. static float min_flt(float a, float b)
  516. {
  517.     return std::min(a, b);
  518. }
  519.  
  520. static float max_flt(float a, float b)
  521. {
  522.     return std::max(a, b);
  523. }
  524.  
  525. static int to_cell_idx(float val, float inv_cell_size, int num_cells)
  526. {
  527.     const int cell_pos = (int)(val * inv_cell_size);
  528.     return min_int(max_int(cell_pos, 0), num_cells - 1);
  529. }
  530.  
  531. static SimdVec4i to_tcell_idx4(const LGrid* grid, __m128 rect)
  532. {
  533.     __m128 inv_cell_size_vec = simd_create4f(grid->tight.inv_cell_w, grid->tight.inv_cell_h,
  534.                                              grid->tight.inv_cell_w, grid->tight.inv_cell_h);
  535.     __m128 cell_xyf_vec = simd_mul4f(rect, inv_cell_size_vec);
  536.     __m128i clamp_vec = simd_create4i(grid->tight.num_cols-1, grid->tight.num_rows-1,
  537.                                       grid->tight.num_cols-1, grid->tight.num_rows-1);
  538.     __m128i cell_xy_vec = simd_clamp4i(simd_ftoi4f(cell_xyf_vec), simd_zero4i(), clamp_vec);
  539.     return simd_store4i(cell_xy_vec);
  540. }
  541.  
  542. static void grid_optimize(LGrid* grid)
  543. {
  544.     FreeList<LGridElt> new_elts;
  545.     new_elts.reserve(grid->num_elts);
  546.     for (int c=0; c < grid->loose.num_cells; ++c)
  547.     {
  548.         // Replace links to the old elements list to links in the new
  549.         // cache-friendly element list.
  550.         SmallList<int> new_elt_idxs;
  551.         LGridLooseCell* lcell = &grid->loose.cells[c];
  552.         while (lcell->head != -1)
  553.         {
  554.             const LGridElt* elt = &grid->elts[lcell->head];
  555.             new_elt_idxs.push_back(new_elts.insert(*elt));
  556.             lcell->head = elt->next;
  557.         }
  558.         for (int j=0; j < new_elt_idxs.size(); ++j)
  559.         {
  560.             const int new_elt_idx = new_elt_idxs[j];
  561.             new_elts[new_elt_idx].next = lcell->head;
  562.             lcell->head = new_elt_idx;
  563.         }
  564.     }
  565.     // Swap the new element list with the old one.
  566.     grid->elts.swap(new_elts);
  567. }
  568.  
  569. static void expand_aabb(LGrid* grid, int cell_idx, float mx, float my, float hx, float hy)
  570. {
  571.     LGridLooseCell* lcell = &grid->loose.cells[cell_idx];
  572.     const SimdVec4f prev_rect = {lcell->rect[0], lcell->rect[1], lcell->rect[2], lcell->rect[3]};
  573.     lcell->rect[0] = min_flt(lcell->rect[0], mx - hx);
  574.     lcell->rect[1] = min_flt(lcell->rect[1], my - hx);
  575.     lcell->rect[2] = max_flt(lcell->rect[2], mx + hx);
  576.     lcell->rect[3] = max_flt(lcell->rect[3], my + hy);
  577.  
  578.     // Determine the cells occupied by the loose cell in the tight grid.
  579.     const SimdVec4f elt_rect = {mx-hx, my-hy, mx+hx, my+hy};
  580.     const SimdVec4i trect = to_tcell_idx4(grid, simd_load4f(&elt_rect));
  581.  
  582.     if (prev_rect.data[0] > prev_rect.data[2])
  583.     {
  584.         // If the loose cell was empty, simply insert the loose cell
  585.         // to all the tight cells it occupies. We don't need to check
  586.         // to see if it was already inserted.
  587.         for (int ty = trect.data[1]; ty <= trect.data[3]; ++ty)
  588.         {
  589.             int* tight_row = grid->tight.heads + ty*grid->tight.num_cols;
  590.             for (int tx = trect.data[0]; tx <= trect.data[2]; ++tx)
  591.             {
  592.                 const LGridTightCell new_tcell = {tight_row[tx], cell_idx};
  593.                 tight_row[tx] = grid->tight.cells.insert(new_tcell);
  594.             }
  595.         }
  596.     }
  597.     else
  598.     {
  599.         // Only perform the insertion if the loose cell overlaps new tight cells.
  600.         const SimdVec4i prev_trect = to_tcell_idx4(grid, simd_load4f(&prev_rect));
  601.         if (trect.data[0] != prev_trect.data[0] || trect.data[1] != prev_trect.data[1] ||
  602.             trect.data[2] != prev_trect.data[2] || trect.data[3] != prev_trect.data[3])
  603.         {
  604.             for (int ty = trect.data[1]; ty <= trect.data[3]; ++ty)
  605.             {
  606.                 int* tight_row = grid->tight.heads + ty*grid->tight.num_cols;
  607.                 for (int tx = trect.data[0]; tx <= trect.data[2]; ++tx)
  608.                 {
  609.                     if (tx < prev_trect.data[0] || tx > prev_trect.data[2] ||
  610.                         ty < prev_trect.data[1] || ty > prev_trect.data[3])
  611.                     {
  612.                         const LGridTightCell new_tcell = {tight_row[tx], cell_idx};
  613.                         tight_row[tx] = grid->tight.cells.insert(new_tcell);
  614.                     }
  615.                 }
  616.             }
  617.         }
  618.     }
  619. }
  620.  
  621. static __m128 element_rect(const LGridElt* elt)
  622. {
  623.     return simd_create4f(elt->mx-elt->hx, elt->my-elt->hy,
  624.                          elt->mx+elt->hx, elt->my+elt->hy);
  625. }
  626.  
  627. LGrid* lgrid_create(float lcell_w, float lcell_h, float tcell_w, float tcell_h,
  628.                     float l, float t, float r, float b)
  629. {
  630.     const float w = r - l, h = b - t;
  631.     const int num_lcols = ceil_div(w, lcell_w), num_lrows = ceil_div(h, lcell_h);
  632.     const int num_tcols = ceil_div(w, tcell_w), num_trows = ceil_div(h, tcell_h);
  633.  
  634.     LGrid* grid = new LGrid;
  635.     grid->num_elts = 0;
  636.     grid->x = l;
  637.     grid->y = t;
  638.     grid->h = w;
  639.     grid->w = h;
  640.  
  641.     grid->loose.num_cols = num_lcols;
  642.     grid->loose.num_rows = num_lrows;
  643.     grid->loose.num_cells = grid->loose.num_cols * grid->loose.num_rows;
  644.     grid->loose.inv_cell_w = 1.0f / lcell_w;
  645.     grid->loose.inv_cell_h = 1.0f / lcell_h;
  646.  
  647.     grid->tight.num_cols = num_tcols;
  648.     grid->tight.num_rows = num_trows;
  649.     grid->tight.num_cells = grid->tight.num_cols * grid->tight.num_rows;
  650.     grid->tight.inv_cell_w = 1.0f / tcell_w;
  651.     grid->tight.inv_cell_h = 1.0f / tcell_h;
  652.  
  653.     // Initialize tight cell heads with -1 to indicate empty indexed SLLs.
  654.     grid->tight.heads = new int[grid->tight.num_cells];
  655.     for (int j=0; j < grid->tight.num_cells; ++j)
  656.         grid->tight.heads[j] = -1;
  657.  
  658.     // Initialize all the loose cells.
  659.     grid->loose.cells = new LGridLooseCell[grid->loose.num_cells];
  660.     for (int c=0; c < grid->loose.num_cells; ++c)
  661.     {
  662.         grid->loose.cells[c].head = -1;
  663.         grid->loose.cells[c].rect[0] = FLT_MAX;
  664.         grid->loose.cells[c].rect[1] = FLT_MAX;
  665.         grid->loose.cells[c].rect[2] = -FLT_MAX;
  666.         grid->loose.cells[c].rect[3] = -FLT_MAX;
  667.     }
  668.     return grid;
  669. }
  670.  
  671. void lgrid_destroy(LGrid* grid)
  672. {
  673.     delete[] grid->loose.cells;
  674.     delete[] grid->tight.heads;
  675.     delete grid;
  676. }
  677.  
  678. int lgrid_lcell_idx(LGrid* grid, float x, float y)
  679. {
  680.     const int cell_x = to_cell_idx(x - grid->x, grid->loose.inv_cell_w, grid->loose.num_cols);
  681.     const int cell_y = to_cell_idx(y - grid->y, grid->loose.inv_cell_h, grid->loose.num_rows);
  682.     return cell_y * grid->loose.num_cols + cell_x;
  683. }
  684.  
  685. void lgrid_insert(LGrid* grid, int id, float mx, float my, float hx, float hy)
  686. {
  687.     const int cell_idx = lgrid_lcell_idx(grid, mx, my);
  688.     LGridLooseCell* lcell = &grid->loose.cells[cell_idx];
  689.  
  690.     // Insert the element to the appropriate loose cell and row.
  691.     const LGridElt new_elt = {lcell->head, id, mx - grid->x, my - grid->y, hx, hy};
  692.     lcell->head = grid->elts.insert(new_elt);
  693.     ++grid->num_elts;
  694.  
  695.     // Expand the loose cell's bounding box to fit the new element.
  696.     expand_aabb(grid, cell_idx, mx, my, hx, hy);
  697. }
  698.  
  699. void lgrid_remove(LGrid* grid, int id, float mx, float my)
  700. {
  701.     // Find the element in the loose cell.
  702.     LGridLooseCell* lcell = &grid->loose.cells[lgrid_lcell_idx(grid, mx, my)];
  703.     int* link = &lcell->head;
  704.     while (grid->elts[*link].id != id)
  705.         link = &grid->elts[*link].next;
  706.  
  707.     // Remove the element from the loose cell and row.
  708.     const int elt_idx = *link;
  709.     *link = grid->elts[elt_idx].next;
  710.     grid->elts.erase(elt_idx);
  711.     --grid->num_elts;
  712. }
  713.  
  714. void lgrid_move(LGrid* grid, int id, float prev_mx, float prev_my, float mx, float my)
  715. {
  716.     const int prev_cell_idx = lgrid_lcell_idx(grid, prev_mx, prev_my);
  717.     const int new_cell_idx = lgrid_lcell_idx(grid, mx, my);
  718.     LGridLooseCell* lcell = &grid->loose.cells[prev_cell_idx];
  719.  
  720.     if (prev_cell_idx == new_cell_idx)
  721.     {
  722.         // Find the element in the loose cell.
  723.         int elt_idx = lcell->head;
  724.         while (grid->elts[elt_idx].id != id)
  725.             elt_idx = grid->elts[elt_idx].next;
  726.  
  727.         // Since the element is still inside the same cell, we can simply overwrite
  728.         // its position and expand the loose cell's AABB.
  729.         mx -= grid->x;
  730.         my -= grid->y;
  731.         grid->elts[elt_idx].mx = mx;
  732.         grid->elts[elt_idx].my = my;
  733.         expand_aabb(grid, prev_cell_idx, mx, my, grid->elts[elt_idx].hx, grid->elts[elt_idx].hy);
  734.     }
  735.     else
  736.     {
  737.         // Find the element in the loose cell.
  738.         int* link = &lcell->head;
  739.         while (grid->elts[*link].id != id)
  740.             link = &grid->elts[*link].next;
  741.  
  742.         const int elt_idx = *link;
  743.         const float hx = grid->elts[elt_idx].hx;
  744.         const float hy = grid->elts[elt_idx].hy;
  745.  
  746.         // If the element has moved into a different loose cell, remove
  747.         // remove the element from the previous loose cell and row.
  748.         *link = grid->elts[elt_idx].next;
  749.         grid->elts.erase(elt_idx);
  750.         --grid->num_elts;
  751.  
  752.         // Now insert the element to its new position.
  753.         lgrid_insert(grid, id, mx, my, hx, hy);
  754.     }
  755. }
  756.  
  757. SmallList<int> lgrid_query(const LGrid* grid, float mx, float my, float hx, float hy, int omit_id)
  758. {
  759.     mx -= grid->x;
  760.     my -= grid->y;
  761.  
  762.     // Compute the tight cell extents [min_tx, min_ty, max_tx, max_ty].
  763.     const SimdVec4f qrect = {mx-hx, my-hy, mx+hx, my+hy};
  764.     __m128 qrect_vec = simd_load4f(&qrect);
  765.     const SimdVec4i trect = to_tcell_idx4(grid, qrect_vec);
  766.  
  767.     // Gather the intersecting loose cells in the tight cells that intersect.
  768.     SmallList<int> lcell_idxs;
  769.     for (int ty = trect.data[1]; ty <= trect.data[3]; ++ty)
  770.     {
  771.         const int* tight_row = grid->tight.heads + ty*grid->tight.num_cols;
  772.         for (int tx = trect.data[0]; tx <= trect.data[2]; ++tx)
  773.         {
  774.             // Iterate through the loose cells that intersect the tight cells.
  775.             int tcell_idx = tight_row[tx];
  776.             while (tcell_idx != -1)
  777.             {
  778.                 const LGridTightCell* tcell = &grid->tight.cells[tcell_idx];
  779.                 const LGridLooseCell* lcell = &grid->loose.cells[tcell->lcell];
  780.                 if (lcell_idxs.find_index(tcell->lcell) == -1 && simd_rect_intersect4f(qrect_vec, simd_loadu4f(lcell->rect)))
  781.                     lcell_idxs.push_back(tcell->lcell);
  782.                 tcell_idx = tcell->next;
  783.             }
  784.         }
  785.     }
  786.  
  787.     // For each loose cell, determine what elements intersect.
  788.     SmallList<int> res;
  789.     for (int j=0; j < lcell_idxs.size(); ++j)
  790.     {
  791.         const LGridLooseCell* lcell = &grid->loose.cells[lcell_idxs[j]];
  792.         int elt_idx = lcell->head;
  793.         while (elt_idx != -1)
  794.         {
  795.             // If the element intersects the search rectangle, add it to the
  796.             // resulting elements unless it has an ID that should be omitted.
  797.             const LGridElt* elt = &grid->elts[elt_idx];
  798.             if (elt->id != omit_id && simd_rect_intersect4f(qrect_vec, element_rect(elt)))
  799.                 res.push_back(elt->id);
  800.             elt_idx = elt->next;
  801.         }
  802.     }
  803.     return res;
  804. }
  805.  
  806. LGridQuery4 lgrid_query4(const LGrid* grid, const SimdVec4f* mx4, const SimdVec4f* my4,
  807.                          const SimdVec4f* hx4, const SimdVec4f* hy4, const SimdVec4i* omit_id4)
  808. {
  809.     __m128 hx_vec = simd_load4f(hx4), hy_vec = simd_load4f(hy4);
  810.     __m128 mx_vec = simd_sub4f(simd_load4f(mx4), simd_scalar4f(grid->x));
  811.     __m128 my_vec = simd_sub4f(simd_load4f(my4), simd_scalar4f(grid->y));
  812.     __m128 ql_vec = simd_sub4f(mx_vec, hx_vec), qt_vec = simd_sub4f(my_vec, hy_vec);
  813.     __m128 qr_vec = simd_add4f(mx_vec, hx_vec), qb_vec = simd_add4f(my_vec, hy_vec);
  814.  
  815.     __m128 inv_cell_w_vec = simd_scalar4f(grid->tight.inv_cell_w), inv_cell_h_vec = simd_scalar4f(grid->tight.inv_cell_h);
  816.     __m128i max_x_vec = simd_scalar4i(grid->tight.num_cols-1), max_y_vec = simd_scalar4i(grid->tight.num_rows-1);
  817.     __m128i tmin_x_vec = simd_clamp4i(simd_ftoi4f(simd_mul4f(ql_vec, inv_cell_w_vec)), simd_zero4i(), max_x_vec);
  818.     __m128i tmin_y_vec = simd_clamp4i(simd_ftoi4f(simd_mul4f(qt_vec, inv_cell_h_vec)), simd_zero4i(), max_y_vec);
  819.     __m128i tmax_x_vec = simd_clamp4i(simd_ftoi4f(simd_mul4f(qr_vec, inv_cell_w_vec)), simd_zero4i(), max_x_vec);
  820.     __m128i tmax_y_vec = simd_clamp4i(simd_ftoi4f(simd_mul4f(qb_vec, inv_cell_h_vec)), simd_zero4i(), max_y_vec);
  821.  
  822.     const SimdVec4i tmin_x4 = simd_store4i(tmin_x_vec), tmin_y4 = simd_store4i(tmin_y_vec);
  823.     const SimdVec4i tmax_x4 = simd_store4i(tmax_x_vec), tmax_y4 = simd_store4i(tmax_y_vec);
  824.     const SimdVec4f ql4 = simd_store4f(ql_vec), qt4 = simd_store4f(qt_vec);
  825.     const SimdVec4f qr4 = simd_store4f(qr_vec), qb4 = simd_store4f(qb_vec);
  826.  
  827.     LGridQuery4 res4;
  828.     for (int k=0; k < 4; ++k)
  829.     {
  830.         const int trect[4] = {tmin_x4.data[k], tmin_y4.data[k], tmax_x4.data[k], tmax_y4.data[k]};
  831.         const int omit_id = omit_id4->data[k];
  832.  
  833.         // Gather the intersecting loose cells in the tight cells that intersect.
  834.         SmallList<int> lcell_idxs;
  835.         __m128 qrect_vec = simd_create4f(ql4.data[k], qt4.data[k], qr4.data[k], qb4.data[k]);
  836.         for (int ty = trect[1]; ty <= trect[3]; ++ty)
  837.         {
  838.             const int* tight_row = grid->tight.heads + ty*grid->tight.num_cols;
  839.             for (int tx = trect[0]; tx <= trect[2]; ++tx)
  840.             {
  841.                 // Iterate through the loose cells that intersect the tight cells.
  842.                 int tcell_idx = tight_row[tx];
  843.                 while (tcell_idx != -1)
  844.                 {
  845.                     const LGridTightCell* tcell = &grid->tight.cells[tcell_idx];
  846.                     if (lcell_idxs.find_index(tcell->lcell) && simd_rect_intersect4f(qrect_vec, simd_loadu4f(grid->loose.cells[tcell->lcell].rect)))
  847.                         lcell_idxs.push_back(tcell->lcell);
  848.                     tcell_idx = tcell->next;
  849.                 }
  850.             }
  851.         }
  852.  
  853.         // For each loose cell, determine what elements intersect.
  854.         for (int j=0; j < lcell_idxs.size(); ++j)
  855.         {
  856.             const LGridLooseCell* lcell = &grid->loose.cells[lcell_idxs[j]];
  857.             int elt_idx = lcell->head;
  858.             while (elt_idx != -1)
  859.             {
  860.                 // If the element intersects the search rectangle, add it to the
  861.                 // resulting elements unless it has an ID that should be omitted.
  862.                 const LGridElt* elt = &grid->elts[elt_idx];
  863.                 if (elt->id != omit_id && simd_rect_intersect4f(qrect_vec, element_rect(elt)))
  864.                     res4.elements[k].push_back(elt->id);
  865.                 elt_idx = elt->next;
  866.             }
  867.         }
  868.     }
  869.     return res4;
  870. }
  871.  
  872. bool lgrid_in_bounds(const LGrid* grid, float mx, float my, float hx, float hy)
  873. {
  874.     mx -= grid->x;
  875.     my -= grid->y;
  876.     const float x1 = mx-hx, y1 = my-hy, x2 = mx+hx, y2 = my+hy;
  877.     return x1 >= 0.0f && x2 < grid->w && y1 >= 0.0f && y2 < grid->h;
  878. }
  879.  
  880. void lgrid_optimize(LGrid* grid)
  881. {
  882.     // Clear all the tight cell data.
  883.     for (int j=0; j < grid->tight.num_cells; ++j)
  884.         grid->tight.heads[j] = -1;
  885.     grid->tight.cells.clear();
  886.  
  887.     // Optimize the memory layout of the grid.
  888.     grid_optimize(grid);
  889.  
  890.     #pragma omp parallel for
  891.     for (int c=0; c < grid->loose.num_cells; ++c)
  892.     {
  893.         // Empty the loose cell's bounding box.
  894.         LGridLooseCell* lcell = &grid->loose.cells[c];
  895.         lcell->rect[0] = FLT_MAX;
  896.         lcell->rect[1] = FLT_MAX;
  897.         lcell->rect[2] = -FLT_MAX;
  898.         lcell->rect[3] = -FLT_MAX;
  899.  
  900.         // Expand the bounding box by each element's extents in
  901.         // the loose cell.
  902.         int elt_idx = lcell->head;
  903.         while (elt_idx != -1)
  904.         {
  905.             const LGridElt* elt = &grid->elts[elt_idx];
  906.             lcell->rect[0] = min_flt(lcell->rect[0], elt->mx - elt->hx);
  907.             lcell->rect[1] = min_flt(lcell->rect[1], elt->my - elt->hy);
  908.             lcell->rect[2] = max_flt(lcell->rect[2], elt->mx + elt->hx);
  909.             lcell->rect[3] = max_flt(lcell->rect[3], elt->my + elt->hy);
  910.             elt_idx = elt->next;
  911.         }
  912.     }
  913.  
  914.     for (int c=0; c < grid->loose.num_cells; ++c)
  915.     {
  916.         // Insert the loose cell to all the tight cells in which
  917.         // it now belongs.
  918.         LGridLooseCell* lcell = &grid->loose.cells[c];
  919.         const SimdVec4i trect = to_tcell_idx4(grid, simd_loadu4f(lcell->rect));
  920.         for (int ty = trect.data[1]; ty <= trect.data[3]; ++ty)
  921.         {
  922.             int* tight_row = grid->tight.heads + ty*grid->tight.num_cols;
  923.             for (int tx = trect.data[0]; tx <= trect.data[2]; ++tx)
  924.             {
  925.                 const LGridTightCell new_tcell = {tight_row[tx], c};
  926.                 tight_row[tx] = grid->tight.cells.insert(new_tcell);
  927.             }
  928.         }
  929.     }
  930. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement