16 #ifndef SURGSIM_DATASTRUCTURES_GRID_INL_H
17 #define SURGSIM_DATASTRUCTURES_GRID_INL_H
19 #include <boost/functional/hash.hpp>
23 namespace DataStructures
45 template <
typename T,
size_t B,
size_t N>
49 static_assert(B > 1 && B < 10,
"B (the base) needs to be within [2..9]");
58 size_t toDecimal()
const
61 size_t BexponentDigit = 1;
62 for (
size_t digit = 0; digit < N; ++digit)
64 value += (*this)[digit] * BexponentDigit;
78 if ((*
this)[digit] == B)
96 template <
typename T,
size_t N>
99 return boost::hash_range(nd.data(), nd.data() + N);
102 template <
typename T,
size_t N>
103 Grid<T, N>::Grid(
const Eigen::Matrix<double, N, 1>& cellSize,
const Eigen::AlignedBox<double, N>& bounds)
108 static_assert(N >= 1,
"A grid must have a positive non null dimension");
111 template <
typename T,
size_t N>
116 template <
typename T,
size_t N>
123 m_activeCells.clear();
126 m_neighborsDirtyFlag =
false;
129 template <
typename T,
size_t N>
130 template <
class Derived>
134 if (!m_aabb.contains(position))
143 NDId cellId = ((position - m_aabb.min()).cwiseQuotient(m_size)).
template cast<int>();
146 m_activeCells[cellId].elements.push_back(element);
149 m_cellIds[element] = cellId;
152 m_neighborsDirtyFlag =
true;
155 template <
typename T,
size_t N>
158 std::array<NDId, powerOf3<N>::value> cellsIds;
163 cell.second.neighbors.clear();
169 getNeighborsCellIds(cell.first, &cellsIds);
171 for (
size_t index = 0; index < powerOf3<N>::value; ++index)
174 if (isNdGreaterOrEqual(cellsIds[index], cell.first))
176 auto neighborCell = m_activeCells.find(cellsIds[index]);
177 if (neighborCell != m_activeCells.end())
179 cell.second.neighbors.insert(cell.second.neighbors.end(),
180 neighborCell->second.elements.cbegin(),
181 neighborCell->second.elements.cend());
184 if (cellsIds[index] != cell.first)
186 neighborCell->second.neighbors.insert(neighborCell->second.neighbors.end(),
187 cell.second.elements.cbegin(),
188 cell.second.elements.cend());
194 m_neighborsDirtyFlag =
false;
197 template <
typename T,
size_t N>
200 static std::vector<T> empty;
202 if (m_neighborsDirtyFlag)
207 auto const foundCell = m_cellIds.find(element);
208 if (foundCell != m_cellIds.cend())
210 return m_activeCells[foundCell->second].neighbors;
216 template <
typename T,
size_t N>
217 template <
class Derived>
220 static const std::vector<T> empty;
223 if (m_aabb.contains(position))
225 if (m_neighborsDirtyFlag)
230 NDId cellId = ((position - m_aabb.min()).cwiseQuotient(m_size)).
template cast<int>();
232 auto foundCell = m_activeCells.find(cellId);
233 if (foundCell == m_activeCells.end())
236 std::array<NDId, powerOf3<N>::value> cellsIds;
237 getNeighborsCellIds(cellId, &cellsIds);
238 std::vector<T> neighbors;
239 for (
const auto& neighborId : cellsIds)
241 auto neighborCell = m_activeCells.find(neighborId);
242 if (neighborCell != m_activeCells.end())
244 neighbors.insert(neighbors.end(),
245 neighborCell->second.elements.cbegin(),
246 neighborCell->second.elements.cend());
249 m_activeCells[cellId].neighbors = std::move(neighbors);
251 return m_activeCells[cellId].neighbors;
256 template <
typename T,
size_t N>
270 cellId -= NDId::Ones();
272 Number<int, 3, N> currentNumberNDigitBase3;
273 for (
size_t i = 0; i < powerOf3<N>::value; ++i)
275 (*cellsIds)[i] = cellId + currentNumberNDigitBase3;
276 currentNumberNDigitBase3.next();
280 template <
typename T,
size_t N>
283 for (
size_t i = 0; i < N; ++i)
301 #endif // SURGSIM_DATASTRUCTURES_GRID_INL_H