grid_impl.h
Go to the documentation of this file.
1 namespace argos {
2 
3  /****************************************/
4  /****************************************/
5 
6  static const Real EPSILON = 1e-6;
7 
8  /****************************************/
9  /****************************************/
10 
11 #define APPLY_ENTITY_OPERATION_TO_CELL(nI,nJ,nK) \
12  { \
13  SCell& sCell = GetCellAt((nI), (nJ), (nK)); \
14  if((sCell.Timestamp == m_unCurTimestamp) && \
15  (! sCell.Entities.empty())) { \
16  for(typename CSet<ENTITY*,SEntityComparator>::iterator it = sCell.Entities.begin(); \
17  it != sCell.Entities.end(); \
18  ++it) { \
19  if(!c_operation(**it)) return; \
20  } \
21  } \
22  }
23 
24 #define APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI,nJ,nK) \
25  { \
26  SCell& sCell = GetCellAt(nI, nJ, nK); \
27  if((sCell.Timestamp == m_unCurTimestamp) && \
28  (! sCell.Entities.empty())) { \
29  for(typename CSet<ENTITY*,SEntityComparator>::iterator it = sCell.Entities.begin(); \
30  it != sCell.Entities.end(); \
31  ++it) { \
32  if(!c_operation(**it)) return; \
33  } \
34  if(b_stop_at_closest_match) return; \
35  } \
36  }
37 
38 #define APPLY_CELL_OPERATION_TO_CELL(nI,nJ,nK) \
39  { \
40  SCell& sCell = GetCellAt((nI), (nJ), (nK)); \
41  if(!c_operation((nI), (nJ), (nK), sCell)) return; \
42  }
43 
44 /****************************************/
45 /****************************************/
46 
47 template<class ENTITY>
48 CGrid<ENTITY>::CGrid(const CVector3& c_area_min_corner,
49  const CVector3& c_area_max_corner,
50  SInt32 n_size_i,
51  SInt32 n_size_j,
52  SInt32 n_size_k) :
53  m_cAreaMinCorner(c_area_min_corner),
54  m_cAreaMaxCorner(c_area_max_corner),
55  m_nSizeI(n_size_i),
56  m_nSizeJ(n_size_j),
57  m_nSizeK(n_size_k),
58  m_cRangeX(m_cAreaMinCorner.GetX(), m_cAreaMaxCorner.GetX()),
59  m_cRangeY(m_cAreaMinCorner.GetY(), m_cAreaMaxCorner.GetY()),
60  m_cRangeZ(m_cAreaMinCorner.GetZ(), m_cAreaMaxCorner.GetZ()),
61  m_unCurTimestamp(0),
62  m_pcUpdateEntityOperation(NULL) {
67  1.0f / m_cCellSize.GetY(),
68  1.0f / m_cCellSize.GetZ());
70 }
71 
72  /****************************************/
73  /****************************************/
74 
75  template<class ENTITY>
77  delete[] m_psCells;
78  }
79 
80  /****************************************/
81  /****************************************/
82 
83  template<class ENTITY>
85  }
86 
87  /****************************************/
88  /****************************************/
89 
90  template<class ENTITY>
92  m_unCurTimestamp = 0;
93  for(SInt32 i = 0; i < m_nSizeI; ++i) {
94  for(SInt32 j = 0; j < m_nSizeJ; ++j) {
95  for(SInt32 k = 0; k < m_nSizeK; ++k) {
96  GetCellAt(i,j,k).Reset();
97  }
98  }
99  }
100  Update();
101  }
102 
103  /****************************************/
104  /****************************************/
105 
106  template<class ENTITY>
108  }
109 
110  /****************************************/
111  /****************************************/
112 
113  template<class ENTITY>
114  void CGrid<ENTITY>::AddEntity(ENTITY& c_entity) {
115  m_cEntities.insert(&c_entity);
116  }
117 
118  /****************************************/
119  /****************************************/
120 
121  template<class ENTITY>
122  void CGrid<ENTITY>::RemoveEntity(ENTITY& c_entity) {
123  m_cEntities.erase(&c_entity);
124  }
125 
126  /****************************************/
127  /****************************************/
128 
129  template<class ENTITY>
131  ++m_unCurTimestamp;
132  ForAllEntities(*m_pcUpdateEntityOperation);
133  }
134 
135  /****************************************/
136  /****************************************/
137 
138  template<class ENTITY>
140  const CVector3& c_position) const {
141  try {
142  SInt32 i, j, k;
143  PositionToCell(i, j, k, c_position);
144  const SCell& sCell = GetCellAt(i, j, k);
145  if(sCell.Timestamp < m_unCurTimestamp) {
146  c_entities.clear();
147  }
148  else {
149  c_entities = sCell.Entities;
150  }
151  }
152  catch(CARGoSException& ex) {
153  THROW_ARGOSEXCEPTION_NESTED("CGrid<ENTITY>::GetEntitiesAt() : Position <" << c_position << "> out of bounds X -> " << m_cRangeX << " Y -> " << m_cRangeY << " Z -> " << m_cRangeZ, ex);
154  }
155  }
156 
157  /****************************************/
158  /****************************************/
159 
160  template<class ENTITY>
162  for(typename CSet<ENTITY*,SEntityComparator>::iterator it = m_cEntities.begin();
163  it != m_cEntities.end() && c_operation(**it);
164  ++it);
165  }
166 
167  /****************************************/
168  /****************************************/
169 
170  template<class ENTITY>
172  Real f_radius,
173  CEntityOperation& c_operation) {
174  /* Calculate cells for center */
175  SInt32 nIC, nJC, nKC, nIR, nJR, nKR;
176  PositionToCellUnsafe(nIC, nJC, nKC, c_center);
177  if(nKC >= 0 && nKC < m_nSizeK) {
178  /* Check circle center */
179  if((nIC >= 0 && nIC < m_nSizeI) && (nJC >= 0 && nJC < m_nSizeJ)) APPLY_ENTITY_OPERATION_TO_CELL(nIC, nJC, nKC);
180  /* Calculate radia of circle */
181  nIR = Floor(f_radius * m_cInvCellSize.GetX() + 0.5f);
182  nJR = Floor(f_radius * m_cInvCellSize.GetY() + 0.5f);
183  /* Go through diameter on j at i = 0 */
184  if(nIC >= 0 && nIC < m_nSizeI) {
185  for(SInt32 j = nJR; j > 0; --j) {
186  if(nJC + j >= 0 && nJC + j < m_nSizeJ) APPLY_ENTITY_OPERATION_TO_CELL(nIC, nJC + j, nKC);
187  if(nJC - j >= 0 && nJC - j < m_nSizeJ) APPLY_ENTITY_OPERATION_TO_CELL(nIC, nJC - j, nKC);
188  }
189  }
190  /* Go through diameter on i at j = 0 */
191  if(nJC >= 0 && nJC < m_nSizeJ) {
192  for(SInt32 i = nIR; i > 0; --i) {
193  if(nIC + i >= 0 && nIC + i < m_nSizeI) APPLY_ENTITY_OPERATION_TO_CELL(nIC + i, nJC, nKC);
194  if(nIC - i >= 0 && nIC - i < m_nSizeI) APPLY_ENTITY_OPERATION_TO_CELL(nIC - i, nJC, nKC);
195  }
196  }
197  /* Go through cells with k = nKC */
198  for(SInt32 j = nJR; j > 0; --j) {
199  nIR = Floor(Sqrt(Max<Real>(0.0f, f_radius * f_radius - j * m_cCellSize.GetY() * j * m_cCellSize.GetY())) * m_cInvCellSize.GetX() + 0.5f);
200  for(SInt32 i = nIR; i > 0; --i) {
201  if(nIC + i >= 0 && nIC + i < m_nSizeI && nJC + j >= 0 && nJC + j < m_nSizeJ) APPLY_ENTITY_OPERATION_TO_CELL(nIC + i, nJC + j, nKC);
202  if(nIC + i >= 0 && nIC + i < m_nSizeI && nJC - j >= 0 && nJC - j < m_nSizeJ) APPLY_ENTITY_OPERATION_TO_CELL(nIC + i, nJC - j, nKC);
203  if(nIC - i >= 0 && nIC - i < m_nSizeI && nJC + j >= 0 && nJC + j < m_nSizeJ) APPLY_ENTITY_OPERATION_TO_CELL(nIC - i, nJC + j, nKC);
204  if(nIC - i >= 0 && nIC - i < m_nSizeI && nJC - j >= 0 && nJC - j < m_nSizeJ) APPLY_ENTITY_OPERATION_TO_CELL(nIC - i, nJC - j, nKC);
205  }
206  }
207  }
208  /* Go through other cells */
209  nKR = Floor(f_radius * m_cInvCellSize.GetZ() + 0.5f);
210  Real fCircleRadius2;
211  for(SInt32 k = nKR; k > 0; --k) {
212  /* Check center of circle at k and -k */
213  if((nIC >= 0 && nIC < m_nSizeI) && (nJC >= 0 && nJC < m_nSizeJ)) {
214  if(nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC, nJC, nKC + k);
215  if(nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC, nJC, nKC - k);
216  }
217  /* Calculate radius of circle at k and -k */
218  fCircleRadius2 = Max<Real>(0.0f, f_radius * f_radius - k * m_cCellSize.GetZ() * k * m_cCellSize.GetZ());
219  /* Calculate radius of circle at j,k */
220  nIR = Floor(Sqrt(fCircleRadius2) * m_cInvCellSize.GetX() + 0.5f);
221  /* Go through diameter on i at j = 0 */
222  if(nJC >= 0 && nJC < m_nSizeJ) {
223  for(SInt32 i = nIR; i > 0; --i) {
224  if(nIC + i >= 0 && nIC + i < m_nSizeI && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC + i, nJC, nKC + k);
225  if(nIC + i >= 0 && nIC + i < m_nSizeI && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC + i, nJC, nKC - k);
226  if(nIC - i >= 0 && nIC - i < m_nSizeI && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC - i, nJC, nKC + k);
227  if(nIC - i >= 0 && nIC - i < m_nSizeI && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC - i, nJC, nKC - k);
228  }
229  }
230  /* Calculate circle radius in cells on j */
231  nJR = Floor(Sqrt(fCircleRadius2) * m_cInvCellSize.GetY() + 0.5f);
232  for(SInt32 j = nJR; j > 0; --j) {
233  /* Go through diameter on j at i = 0 */
234  if(nIC >= 0 && nIC < m_nSizeI) {
235  if(nJC + j >= 0 && nJC + j < m_nSizeJ && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC, nJC + j, nKC + k);
236  if(nJC + j >= 0 && nJC + j < m_nSizeJ && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC, nJC + j, nKC - k);
237  if(nJC - j >= 0 && nJC - j < m_nSizeJ && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC, nJC - j, nKC + k);
238  if(nJC - j >= 0 && nJC - j < m_nSizeJ && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC, nJC - j, nKC - k);
239  }
240  /* Calculate radius of circle at j,k */
241  nIR = Floor(Sqrt(Max<Real>(0.0f, fCircleRadius2 - j * m_cCellSize.GetY() * j * m_cCellSize.GetY())) * m_cInvCellSize.GetX() + 0.5f);
242  for(SInt32 i = nIR; i > 0; --i) {
243  if(nIC + i >= 0 && nIC + i < m_nSizeI && nJC + j >= 0 && nJC + j < m_nSizeJ && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC + i, nJC + j, nKC + k);
244  if(nIC + i >= 0 && nIC + i < m_nSizeI && nJC + j >= 0 && nJC + j < m_nSizeJ && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC + i, nJC + j, nKC - k);
245  if(nIC + i >= 0 && nIC + i < m_nSizeI && nJC - j >= 0 && nJC - j < m_nSizeJ && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC + i, nJC - j, nKC + k);
246  if(nIC + i >= 0 && nIC + i < m_nSizeI && nJC - j >= 0 && nJC - j < m_nSizeJ && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC + i, nJC - j, nKC - k);
247  if(nIC - i >= 0 && nIC - i < m_nSizeI && nJC + j >= 0 && nJC + j < m_nSizeJ && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC - i, nJC + j, nKC + k);
248  if(nIC - i >= 0 && nIC - i < m_nSizeI && nJC + j >= 0 && nJC + j < m_nSizeJ && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC - i, nJC + j, nKC - k);
249  if(nIC - i >= 0 && nIC - i < m_nSizeI && nJC - j >= 0 && nJC - j < m_nSizeJ && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC - i, nJC - j, nKC + k);
250  if(nIC - i >= 0 && nIC - i < m_nSizeI && nJC - j >= 0 && nJC - j < m_nSizeJ && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_ENTITY_OPERATION_TO_CELL(nIC - i, nJC - j, nKC - k);
251  }
252  }
253  }
254  }
255 
256  /****************************************/
257  /****************************************/
258 
259  template<class ENTITY>
261  const CVector3& c_half_size,
262  CEntityOperation& c_operation) {
263  /* Calculate cell range */
264  SInt32 nI1, nJ1, nK1, nI2, nJ2, nK2;
265  PositionToCellUnsafe(nI1, nJ1, nK1, c_center - c_half_size);
266  ClampCoordinates(nI1, nJ1, nK1);
267  PositionToCellUnsafe(nI2, nJ2, nK2, c_center + c_half_size);
268  ClampCoordinates(nI2, nJ2, nK2);
269  /* Go through cells */
270  for(SInt32 k = nK1; k <= nK2; ++k) {
271  for(SInt32 j = nJ1; j <= nJ2; ++j) {
272  for(SInt32 i = nI1; i <= nI2; ++i) {
274  }
275  }
276  }
277  }
278 
279  /****************************************/
280  /****************************************/
281 
282  template<class ENTITY>
284  Real f_radius,
285  CEntityOperation& c_operation) {
286  /* Make sure the Z coordinate is inside the range */
287  if(! m_cRangeZ.WithinMinBoundIncludedMaxBoundIncluded(c_center.GetZ())) return;
288  /* Calculate cells for center */
289  SInt32 nI, nJ, nK;
290  PositionToCellUnsafe(nI, nJ, nK, c_center);
291  /* Check circle center */
292  if((nI >= 0 && nI < m_nSizeI) && (nJ >= 0 && nJ < m_nSizeJ)) APPLY_ENTITY_OPERATION_TO_CELL(nI, nJ, nK);
293  /* Check circle diameter on I */
294  SInt32 nID = Floor(f_radius * m_cInvCellSize.GetX() + 0.5f);
295  for(SInt32 h = nID; h > 0; --h) {
296  if((nI + h >= 0 && nI + h < m_nSizeI) && (nJ >= 0 && nJ < m_nSizeJ)) APPLY_ENTITY_OPERATION_TO_CELL(nI + h, nJ, nK);
297  if((nI - h >= 0 && nI - h < m_nSizeI) && (nJ >= 0 && nJ < m_nSizeJ)) APPLY_ENTITY_OPERATION_TO_CELL(nI - h, nJ, nK);
298  }
299  /* Check circle diameter on J */
300  SInt32 nJD = Floor(f_radius * m_cInvCellSize.GetY() + 0.5f);
301  for(SInt32 h = nJD; h > 0; --h) {
302  if((nI >= 0 && nI < m_nSizeI) && (nJ + h >= 0 && nJ + h < m_nSizeJ)) APPLY_ENTITY_OPERATION_TO_CELL(nI, nJ + h, nK);
303  if((nI >= 0 && nI < m_nSizeI) && (nJ - h >= 0 && nJ - h < m_nSizeJ)) APPLY_ENTITY_OPERATION_TO_CELL(nI, nJ - h, nK);
304  }
305  /* Check rest of the circle */
306  for(SInt32 i = nID; i > 0; --i) {
307  nJD = Floor(Sqrt(f_radius * f_radius - i * m_cCellSize.GetX() * i * m_cCellSize.GetX()) * m_cInvCellSize.GetY() + 0.5f);
308  for(SInt32 j = nJD; j > 0; --j) {
309  if((nI + i >= 0 && nI + i < m_nSizeI) && (nJ + j >= 0 && nJ + j < m_nSizeJ)) APPLY_ENTITY_OPERATION_TO_CELL(nI + i, nJ + j, nK);
310  if((nI + i >= 0 && nI + i < m_nSizeI) && (nJ - j >= 0 && nJ - j < m_nSizeJ)) APPLY_ENTITY_OPERATION_TO_CELL(nI + i, nJ - j, nK);
311  if((nI - i >= 0 && nI - i < m_nSizeI) && (nJ + j >= 0 && nJ + j < m_nSizeJ)) APPLY_ENTITY_OPERATION_TO_CELL(nI - i, nJ + j, nK);
312  if((nI - i >= 0 && nI - i < m_nSizeI) && (nJ - j >= 0 && nJ - j < m_nSizeJ)) APPLY_ENTITY_OPERATION_TO_CELL(nI - i, nJ - j, nK);
313  }
314  }
315  }
316 
317  /****************************************/
318  /****************************************/
319 
320  template<class ENTITY>
322  const CVector2& c_half_size,
323  CEntityOperation& c_operation) {
324  /* Calculate cell range */
325  SInt32 nI1 = Min<SInt32>(m_nSizeI-1, Max<SInt32>(0, Floor((c_center.GetX() - c_half_size.GetX() - m_cAreaMinCorner.GetX()) * m_cInvCellSize.GetX())));
326  SInt32 nJ1 = Min<SInt32>(m_nSizeJ-1, Max<SInt32>(0, Floor((c_center.GetY() - c_half_size.GetY() - m_cAreaMinCorner.GetY()) * m_cInvCellSize.GetY())));
327  SInt32 nI2 = Min<SInt32>(m_nSizeI-1, Max<SInt32>(0, Floor((c_center.GetX() + c_half_size.GetX() - m_cAreaMinCorner.GetX()) * m_cInvCellSize.GetX())));
328  SInt32 nJ2 = Min<SInt32>(m_nSizeJ-1, Max<SInt32>(0, Floor((c_center.GetY() + c_half_size.GetY() - m_cAreaMinCorner.GetY()) * m_cInvCellSize.GetY())));
329  SInt32 nK = Min<SInt32>(m_nSizeK-1, Max<SInt32>(0, Floor((c_center.GetZ() - m_cAreaMinCorner.GetZ()) * m_cInvCellSize.GetZ())));
330  /* Go through cells */
331  for(SInt32 j = nJ1; j <= nJ2; ++j) {
332  for(SInt32 i = nI1; i <= nI2; ++i) {
334  }
335  }
336  }
337 
338  /****************************************/
339  /****************************************/
340 
341  template<class ENTITY>
343  CEntityOperation& c_operation,
344  bool b_stop_at_closest_match) {
345  /* Transform ray start and end position into cell coordinates */
346  SInt32 nI1, nJ1, nK1, nI2, nJ2, nK2;
347  PositionToCellUnsafe(nI1, nJ1, nK1, c_ray.GetStart());
348  ClampCoordinates(nI1, nJ1, nK1);
349  PositionToCellUnsafe(nI2, nJ2, nK2, c_ray.GetEnd());
350  ClampCoordinates(nI2, nJ2, nK2);
351  /* Go through cells one by one, from start to end.
352  Stop as soon as an entity is found.
353  If the loop is completed, it means no entities were found -> no intersection.
354  */
355  /* Calculate deltas for later use */
356  SInt32 nDI(Abs(nI2 - nI1));
357  SInt32 nDJ(Abs(nJ2 - nJ1));
358  SInt32 nDK(Abs(nK2 - nK1));
359  /* Calculate the increment for each direction */
360  SInt32 nSI(nI2 >= nI1 ? 1 : -1);
361  SInt32 nSJ(nJ2 >= nJ1 ? 1 : -1);
362  SInt32 nSK(nK2 >= nK1 ? 1 : -1);
363  /* Set the starting cell */
364  SInt32 nI(nI1), nJ(nJ1), nK(nK1);
365  if(nDI >= nDJ && nDI >= nDK) {
366  /* I is the driving axis */
367  /* Calculate error used to know when to move on other axes */
368  SInt32 nEJ(3 * nDJ - nDI);
369  SInt32 nEK(3 * nDK - nDI);
371  /* Cycle through cells */
372  for(SInt32 nCell = nDI; nCell > 0; --nCell) {
373  /* Advance on driving axis */
374  nI += nSI;
376  /* Advance on other axes, if necessary */
377  if(nEJ > 0 && nEK > 0) {
378  /* Advance on both the other axes */
379  if(nEJ * nDK > nEK * nDJ) {
380  nJ += nSJ;
382  nK += nSK;
384  }
385  else {
386  nK += nSK;
388  nJ += nSJ;
390  }
391  nEJ += 2 * (nDJ - nDI);
392  nEK += 2 * (nDK - nDI);
393  }
394  else if(nEJ > 0) {
395  /* Advance only on J */
396  nJ += nSJ;
398  nEJ += 2 * (nDJ - nDI);
399  nEK += 2 * nDK;
400  }
401  else {
402  nEJ += 2 * nDJ;
403  if(nEK > 0) {
404  /* Advance only on K */
405  nK += nSK;
407  nEK += 2 * (nDK - nDI);
408  }
409  else {
410  nEK += 2 * nDK;
411  }
412  }
413  }
414  }
415  else if(nDJ >= nDI && nDJ >= nDK) {
416  /* J is the driving axis */
417  /* Calculate error used to know when to move on other axes */
418  SInt32 nEI(3 * nDI - nDJ);
419  SInt32 nEK(3 * nDK - nDJ);
421  /* Cycle through cells */
422  for(SInt32 nCell = nDJ; nCell > 0; --nCell) {
423  /* Advance on driving axis */
424  nJ += nSJ;
426  /* Advance on other axes, if necessary */
427  if(nEI > 0 && nEK > 0) {
428  /* Advance on both the other axes */
429  if(nEI * nDK > nEK * nDI) {
430  nI += nSI;
432  nK += nSK;
434  }
435  else {
436  nK += nSK;
438  nI += nSI;
440  }
441  nEI += 2 * (nDI - nDJ);
442  nEK += 2 * (nDK - nDJ);
443  }
444  else if(nEI > 0) {
445  /* Advance only on I */
446  nI += nSI;
448  nEI += 2 * (nDI - nDJ);
449  nEK += 2 * nDK;
450  }
451  else {
452  nEI += 2 * nDI;
453  if(nEK > 0) {
454  /* Advance only on K */
455  nK += nSK;
457  nEK += 2 * (nDK - nDJ);
458  }
459  else {
460  nEK += 2 * nDK;
461  }
462  }
463  }
464  }
465  else {
466  /* K is the driving axis */
467  /* Calculate error used to know when to move on other axes */
468  SInt32 nEI(3 * nDI - nDK);
469  SInt32 nEJ(3 * nDJ - nDK);
471  /* Cycle through cells */
472  for(SInt32 nCell = nDK; nCell > 0; --nCell) {
473  /* Advance on driving axis */
474  nK += nSK;
476  /* Advance on other axes, if necessary */
477  if(nEI > 0 && nEJ > 0) {
478  /* Advance on both the other axes */
479  if(nEI * nDJ > nEJ * nDI) {
480  nI += nSI;
482  nJ += nSJ;
484  }
485  else {
486  nJ += nSJ;
488  nI += nSI;
490  }
491  nEI += 2 * (nDI - nDK);
492  nEJ += 2 * (nDJ - nDK);
493  }
494  else if(nEI > 0) {
495  /* Advance only on I */
496  nI += nSI;
498  nEI += 2 * (nDI - nDK);
499  nEJ += 2 * nDJ;
500  }
501  else {
502  nEI += 2 * nDI;
503  if(nEJ > 0) {
504  /* Advance only on J */
505  nJ += nSJ;
507  nEJ += 2 * (nDJ - nDK);
508  }
509  else {
510  nEJ += 2 * nDJ;
511  }
512  }
513  }
514  }
515  }
516 
517  /****************************************/
518  /****************************************/
519 
520  template<class ENTITY>
522  for(SInt32 k = 0; k < m_nSizeK; ++k) {
523  for(SInt32 j = 0; j < m_nSizeJ; ++j) {
524  for(SInt32 i = 0; i < m_nSizeI; ++i) {
526  }
527  }
528  }
529  }
530 
531  /****************************************/
532  /****************************************/
533 
534  template<class ENTITY>
536  Real f_radius,
537  CCellOperation& c_operation) {
538  /* Calculate cells for center */
539  SInt32 nIC, nJC, nKC, nIR, nJR, nKR;
540  PositionToCellUnsafe(nIC, nJC, nKC, c_center);
541  if(nKC >= 0 && nKC < m_nSizeK) {
542  /* Check circle center */
543  if((nIC >= 0 && nIC < m_nSizeI) && (nJC >= 0 && nJC < m_nSizeJ)) APPLY_CELL_OPERATION_TO_CELL(nIC, nJC, nKC);
544  /* Calculate radia of circle */
545  nIR = Floor(f_radius * m_cInvCellSize.GetX() + 0.5f);
546  nJR = Floor(f_radius * m_cInvCellSize.GetY() + 0.5f);
547  /* Go through diameter on j at i = 0 */
548  if(nIC >= 0 && nIC < m_nSizeI) {
549  for(SInt32 j = nJR; j > 0; --j) {
550  if(nJC + j >= 0 && nJC + j < m_nSizeJ) APPLY_CELL_OPERATION_TO_CELL(nIC, nJC + j, nKC);
551  if(nJC - j >= 0 && nJC - j < m_nSizeJ) APPLY_CELL_OPERATION_TO_CELL(nIC, nJC - j, nKC);
552  }
553  }
554  /* Go through diameter on i at j = 0 */
555  if(nJC >= 0 && nJC < m_nSizeJ) {
556  for(SInt32 i = nIR; i > 0; --i) {
557  if(nIC + i >= 0 && nIC + i < m_nSizeI) APPLY_CELL_OPERATION_TO_CELL(nIC + i, nJC, nKC);
558  if(nIC - i >= 0 && nIC - i < m_nSizeI) APPLY_CELL_OPERATION_TO_CELL(nIC - i, nJC, nKC);
559  }
560  }
561  /* Go through cells with k = nKC */
562  for(SInt32 j = nJR; j > 0; --j) {
563  nIR = Floor(Sqrt(Max<Real>(0.0f, f_radius * f_radius - j * m_cCellSize.GetY() * j * m_cCellSize.GetY())) * m_cInvCellSize.GetX() + 0.5f);
564  for(SInt32 i = nIR; i > 0; --i) {
565  if(nIC + i >= 0 && nIC + i < m_nSizeI && nJC + j >= 0 && nJC + j < m_nSizeJ) APPLY_CELL_OPERATION_TO_CELL(nIC + i, nJC + j, nKC);
566  if(nIC + i >= 0 && nIC + i < m_nSizeI && nJC - j >= 0 && nJC - j < m_nSizeJ) APPLY_CELL_OPERATION_TO_CELL(nIC + i, nJC - j, nKC);
567  if(nIC - i >= 0 && nIC - i < m_nSizeI && nJC + j >= 0 && nJC + j < m_nSizeJ) APPLY_CELL_OPERATION_TO_CELL(nIC - i, nJC + j, nKC);
568  if(nIC - i >= 0 && nIC - i < m_nSizeI && nJC - j >= 0 && nJC - j < m_nSizeJ) APPLY_CELL_OPERATION_TO_CELL(nIC - i, nJC - j, nKC);
569  }
570  }
571  }
572  /* Go through other cells */
573  nKR = Floor(f_radius * m_cInvCellSize.GetZ() + 0.5f);
574  Real fCircleRadius2;
575  for(SInt32 k = nKR; k > 0; --k) {
576  /* Check center of circle at k and -k */
577  if((nIC >= 0 && nIC < m_nSizeI) && (nJC >= 0 && nJC < m_nSizeJ)) {
578  if(nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC, nJC, nKC + k);
579  if(nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC, nJC, nKC - k);
580  }
581  /* Calculate radius of circle at k and -k */
582  fCircleRadius2 = Max<Real>(0.0f, f_radius * f_radius - k * m_cCellSize.GetZ() * k * m_cCellSize.GetZ());
583  /* Calculate radius of circle at j,k */
584  nIR = Floor(Sqrt(fCircleRadius2) * m_cInvCellSize.GetX() + 0.5f);
585  /* Go through diameter on i at j = 0 */
586  if(nJC >= 0 && nJC < m_nSizeJ) {
587  for(SInt32 i = nIR; i > 0; --i) {
588  if(nIC + i >= 0 && nIC + i < m_nSizeI && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC + i, nJC, nKC + k);
589  if(nIC + i >= 0 && nIC + i < m_nSizeI && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC + i, nJC, nKC - k);
590  if(nIC - i >= 0 && nIC - i < m_nSizeI && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC - i, nJC, nKC + k);
591  if(nIC - i >= 0 && nIC - i < m_nSizeI && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC - i, nJC, nKC - k);
592  }
593  }
594  /* Calculate circle radius in cells on j */
595  nJR = Floor(Sqrt(fCircleRadius2) * m_cInvCellSize.GetY() + 0.5f);
596  for(SInt32 j = nJR; j > 0; --j) {
597  /* Go through diameter on j at i = 0 */
598  if(nIC >= 0 && nIC < m_nSizeI) {
599  if(nJC + j >= 0 && nJC + j < m_nSizeJ && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC, nJC + j, nKC + k);
600  if(nJC + j >= 0 && nJC + j < m_nSizeJ && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC, nJC + j, nKC - k);
601  if(nJC - j >= 0 && nJC - j < m_nSizeJ && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC, nJC - j, nKC + k);
602  if(nJC - j >= 0 && nJC - j < m_nSizeJ && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC, nJC - j, nKC - k);
603  }
604  /* Calculate radius of circle at j,k */
605  nIR = Floor(Sqrt(Max<Real>(0.0f, fCircleRadius2 - j * m_cCellSize.GetY() * j * m_cCellSize.GetY())) * m_cInvCellSize.GetX() + 0.5f);
606  for(SInt32 i = nIR; i > 0; --i) {
607  if(nIC + i >= 0 && nIC + i < m_nSizeI && nJC + j >= 0 && nJC + j < m_nSizeJ && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC + i, nJC + j, nKC + k);
608  if(nIC + i >= 0 && nIC + i < m_nSizeI && nJC + j >= 0 && nJC + j < m_nSizeJ && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC + i, nJC + j, nKC - k);
609  if(nIC + i >= 0 && nIC + i < m_nSizeI && nJC - j >= 0 && nJC - j < m_nSizeJ && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC + i, nJC - j, nKC + k);
610  if(nIC + i >= 0 && nIC + i < m_nSizeI && nJC - j >= 0 && nJC - j < m_nSizeJ && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC + i, nJC - j, nKC - k);
611  if(nIC - i >= 0 && nIC - i < m_nSizeI && nJC + j >= 0 && nJC + j < m_nSizeJ && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC - i, nJC + j, nKC + k);
612  if(nIC - i >= 0 && nIC - i < m_nSizeI && nJC + j >= 0 && nJC + j < m_nSizeJ && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC - i, nJC + j, nKC - k);
613  if(nIC - i >= 0 && nIC - i < m_nSizeI && nJC - j >= 0 && nJC - j < m_nSizeJ && nKC + k >= 0 && nKC + k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC - i, nJC - j, nKC + k);
614  if(nIC - i >= 0 && nIC - i < m_nSizeI && nJC - j >= 0 && nJC - j < m_nSizeJ && nKC - k >= 0 && nKC - k < m_nSizeK) APPLY_CELL_OPERATION_TO_CELL(nIC - i, nJC - j, nKC - k);
615  }
616  }
617  }
618  }
619 
620  /****************************************/
621  /****************************************/
622 
623  template<class ENTITY>
625  const CVector3& c_half_size,
626  CCellOperation& c_operation) {
627  /* Calculate cell range */
628  SInt32 nI1, nJ1, nK1, nI2, nJ2, nK2;
629  PositionToCellUnsafe(nI1, nJ1, nK1, c_center - c_half_size);
630  ClampCoordinates(nI1, nJ1, nK1);
631  PositionToCellUnsafe(nI2, nJ2, nK2, c_center + c_half_size);
632  ClampCoordinates(nI2, nJ2, nK2);
633  /* Go through cells */
634  for(SInt32 k = nK1; k <= nK2; ++k) {
635  for(SInt32 j = nJ1; j <= nJ2; ++j) {
636  for(SInt32 i = nI1; i <= nI2; ++i) {
638  }
639  }
640  }
641  }
642 
643  /****************************************/
644  /****************************************/
645 
646  template<class ENTITY>
648  Real f_radius,
649  CCellOperation& c_operation) {
650  /* Make sure the Z coordinate is inside the range */
651  if(! m_cRangeZ.WithinMinBoundIncludedMaxBoundIncluded(c_center.GetZ())) return;
652  /* Calculate cells for center */
653  SInt32 nI, nJ, nK;
654  PositionToCellUnsafe(nI, nJ, nK, c_center);
655  /* Check circle center */
656  if((nI >= 0 && nI < m_nSizeI) && (nJ >= 0 && nJ < m_nSizeJ)) APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
657  /* Check circle diameter on I */
658  SInt32 nID = Floor(f_radius * m_cInvCellSize.GetX() + 0.5f);
659  for(SInt32 h = nID; h > 0; --h) {
660  if((nI + h >= 0 && nI + h < m_nSizeI) && (nJ >= 0 && nJ < m_nSizeJ)) APPLY_CELL_OPERATION_TO_CELL(nI + h, nJ, nK);
661  if((nI - h >= 0 && nI - h < m_nSizeI) && (nJ >= 0 && nJ < m_nSizeJ)) APPLY_CELL_OPERATION_TO_CELL(nI - h, nJ, nK);
662  }
663  /* Check circle diameter on J */
664  SInt32 nJD = Floor(f_radius * m_cInvCellSize.GetY() + 0.5f);
665  for(SInt32 h = nJD; h > 0; --h) {
666  if((nI >= 0 && nI < m_nSizeI) && (nJ + h >= 0 && nJ + h < m_nSizeJ)) APPLY_CELL_OPERATION_TO_CELL(nI, nJ + h, nK);
667  if((nI >= 0 && nI < m_nSizeI) && (nJ - h >= 0 && nJ - h < m_nSizeJ)) APPLY_CELL_OPERATION_TO_CELL(nI, nJ - h, nK);
668  }
669  /* Check rest of the circle */
670  for(SInt32 i = nID; i > 0; --i) {
671  nJD = Floor(Sqrt(f_radius * f_radius - i * m_cCellSize.GetX() * i * m_cCellSize.GetX()) * m_cInvCellSize.GetY() + 0.5f);
672  for(SInt32 j = nJD; j > 0; --j) {
673  if((nI + i >= 0 && nI + i < m_nSizeI) && (nJ + j >= 0 && nJ + j < m_nSizeJ)) APPLY_CELL_OPERATION_TO_CELL(nI + i, nJ + j, nK);
674  if((nI + i >= 0 && nI + i < m_nSizeI) && (nJ - j >= 0 && nJ - j < m_nSizeJ)) APPLY_CELL_OPERATION_TO_CELL(nI + i, nJ - j, nK);
675  if((nI - i >= 0 && nI - i < m_nSizeI) && (nJ + j >= 0 && nJ + j < m_nSizeJ)) APPLY_CELL_OPERATION_TO_CELL(nI - i, nJ + j, nK);
676  if((nI - i >= 0 && nI - i < m_nSizeI) && (nJ - j >= 0 && nJ - j < m_nSizeJ)) APPLY_CELL_OPERATION_TO_CELL(nI - i, nJ - j, nK);
677  }
678  }
679  }
680 
681  /****************************************/
682  /****************************************/
683 
684  template<class ENTITY>
686  const CVector2& c_half_size,
687  CCellOperation& c_operation) {
688  /* Calculate cell range */
689  SInt32 nI1 = Min<SInt32>(m_nSizeI-1, Max<SInt32>(0, Floor((c_center.GetX() - c_half_size.GetX() - m_cAreaMinCorner.GetX()) * m_cInvCellSize.GetX())));
690  SInt32 nJ1 = Min<SInt32>(m_nSizeJ-1, Max<SInt32>(0, Floor((c_center.GetY() - c_half_size.GetY() - m_cAreaMinCorner.GetY()) * m_cInvCellSize.GetY())));
691  SInt32 nI2 = Min<SInt32>(m_nSizeI-1, Max<SInt32>(0, Floor((c_center.GetX() + c_half_size.GetX() - m_cAreaMinCorner.GetX()) * m_cInvCellSize.GetX())));
692  SInt32 nJ2 = Min<SInt32>(m_nSizeJ-1, Max<SInt32>(0, Floor((c_center.GetY() + c_half_size.GetY() - m_cAreaMinCorner.GetY()) * m_cInvCellSize.GetY())));
693  SInt32 nK = Min<SInt32>(m_nSizeK-1, Max<SInt32>(0, Floor((c_center.GetZ() - m_cAreaMinCorner.GetZ()) * m_cInvCellSize.GetZ())));
694  /* Go through cells */
695  for(SInt32 j = nJ1; j <= nJ2; ++j) {
696  for(SInt32 i = nI1; i <= nI2; ++i) {
698  }
699  }
700  }
701 
702  /****************************************/
703  /****************************************/
704 
705  template<class ENTITY>
707  CCellOperation& c_operation) {
708  /* Transform ray start and end position into cell coordinates */
709  SInt32 nI1, nJ1, nK1, nI2, nJ2, nK2;
710  PositionToCellUnsafe(nI1, nJ1, nK1, c_ray.GetStart());
711  ClampCoordinates(nI1, nJ1, nK1);
712  PositionToCellUnsafe(nI2, nJ2, nK2, c_ray.GetEnd());
713  ClampCoordinates(nI2, nJ2, nK2);
714  /* Go through cells one by one, from start to end.
715  Stop as soon as an entity is found.
716  If the loop is completed, it means no entities were found -> no intersection.
717  */
718  /* Calculate deltas for later use */
719  SInt32 nDI(Abs(nI2 - nI1));
720  SInt32 nDJ(Abs(nJ2 - nJ1));
721  SInt32 nDK(Abs(nK2 - nK1));
722  /* Calculate the increment for each direction */
723  SInt32 nSI(nI2 >= nI1 ? 1 : -1);
724  SInt32 nSJ(nJ2 >= nJ1 ? 1 : -1);
725  SInt32 nSK(nK2 >= nK1 ? 1 : -1);
726  /* Set the starting cell */
727  SInt32 nI(nI1), nJ(nJ1), nK(nK1);
728  if(nDI >= nDJ && nDI >= nDK) {
729  /* I is the driving axis */
730  /* Calculate error used to know when to move on other axes */
731  SInt32 nEJ(3 * nDJ - nDI);
732  SInt32 nEK(3 * nDK - nDI);
733  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
734  /* Cycle through cells */
735  for(SInt32 nCell = nDI; nCell > 0; --nCell) {
736  /* Advance on driving axis */
737  nI += nSI;
738  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
739  /* Advance on other axes, if necessary */
740  if(nEJ > 0 && nEK > 0) {
741  /* Advance on both the other axes */
742  if(nEJ * nDK > nEK * nDJ) {
743  nJ += nSJ;
744  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
745  nK += nSK;
746  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
747  }
748  else {
749  nK += nSK;
750  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
751  nJ += nSJ;
752  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
753  }
754  nEJ += 2 * (nDJ - nDI);
755  nEK += 2 * (nDK - nDI);
756  }
757  else if(nEJ > 0) {
758  /* Advance only on J */
759  nJ += nSJ;
760  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
761  nEJ += 2 * (nDJ - nDI);
762  nEK += 2 * nDK;
763  }
764  else {
765  nEJ += 2 * nDJ;
766  if(nEK > 0) {
767  /* Advance only on K */
768  nK += nSK;
769  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
770  nEK += 2 * (nDK - nDI);
771  }
772  else {
773  nEK += 2 * nDK;
774  }
775  }
776  }
777  }
778  else if(nDJ >= nDI && nDJ >= nDK) {
779  /* J is the driving axis */
780  /* Calculate error used to know when to move on other axes */
781  SInt32 nEI(3 * nDI - nDJ);
782  SInt32 nEK(3 * nDK - nDJ);
783  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
784  /* Cycle through cells */
785  for(SInt32 nCell = nDJ; nCell > 0; --nCell) {
786  /* Advance on driving axis */
787  nJ += nSJ;
788  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
789  /* Advance on other axes, if necessary */
790  if(nEI > 0 && nEK > 0) {
791  /* Advance on both the other axes */
792  if(nEI * nDK > nEK * nDI) {
793  nI += nSI;
794  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
795  nK += nSK;
796  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
797  }
798  else {
799  nK += nSK;
800  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
801  nI += nSI;
802  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
803  }
804  nEI += 2 * (nDI - nDJ);
805  nEK += 2 * (nDK - nDJ);
806  }
807  else if(nEI > 0) {
808  /* Advance only on I */
809  nI += nSI;
810  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
811  nEI += 2 * (nDI - nDJ);
812  nEK += 2 * nDK;
813  }
814  else {
815  nEI += 2 * nDI;
816  if(nEK > 0) {
817  /* Advance only on K */
818  nK += nSK;
819  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
820  nEK += 2 * (nDK - nDJ);
821  }
822  else {
823  nEK += 2 * nDK;
824  }
825  }
826  }
827  }
828  else {
829  /* K is the driving axis */
830  /* Calculate error used to know when to move on other axes */
831  SInt32 nEI(3 * nDI - nDK);
832  SInt32 nEJ(3 * nDJ - nDK);
833  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
834  /* Cycle through cells */
835  for(SInt32 nCell = nDK; nCell > 0; --nCell) {
836  /* Advance on driving axis */
837  nK += nSK;
838  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
839  /* Advance on other axes, if necessary */
840  if(nEI > 0 && nEJ > 0) {
841  /* Advance on both the other axes */
842  if(nEI * nDJ > nEJ * nDI) {
843  nI += nSI;
844  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
845  nJ += nSJ;
846  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
847  }
848  else {
849  nJ += nSJ;
850  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
851  nI += nSI;
852  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
853  }
854  nEI += 2 * (nDI - nDK);
855  nEJ += 2 * (nDJ - nDK);
856  }
857  else if(nEI > 0) {
858  /* Advance only on I */
859  nI += nSI;
860  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
861  nEI += 2 * (nDI - nDK);
862  nEJ += 2 * nDJ;
863  }
864  else {
865  nEI += 2 * nDI;
866  if(nEJ > 0) {
867  /* Advance only on J */
868  nJ += nSJ;
869  APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK);
870  nEJ += 2 * (nDJ - nDK);
871  }
872  else {
873  nEJ += 2 * nDJ;
874  }
875  }
876  }
877  }
878  }
879 
880  /****************************************/
881  /****************************************/
882 
883  template<class ENTITY>
885  SInt32 n_j,
886  SInt32 n_k,
887  ENTITY& c_entity) {
888  if((n_i >= 0) && (n_i < m_nSizeI) &&
889  (n_j >= 0) && (n_j < m_nSizeJ) &&
890  (n_k >= 0) && (n_k < m_nSizeK)) {
891  SCell& sCell = GetCellAt(n_i, n_j, n_k);
892  if(sCell.Timestamp < m_unCurTimestamp) {
893  sCell.Entities.clear();
894  sCell.Timestamp = m_unCurTimestamp;
895  }
896  sCell.Entities.insert(&c_entity);
897  }
898  else {
899  THROW_ARGOSEXCEPTION("CGrid<ENTITY>::UpdateCell() : index (" << n_i << "," << n_j << "," << n_k << ") out of bounds (" << m_nSizeI-1 << "," << m_nSizeJ-1 << "," << m_nSizeK-1 << ")");
900  }
901  }
902 
903  /****************************************/
904  /****************************************/
905 
906  template<class ENTITY>
908  m_pcUpdateEntityOperation = pc_operation;
909  }
910 
911  /****************************************/
912  /****************************************/
913 
914  template<class ENTITY>
916  SInt32& n_j,
917  SInt32& n_k,
918  const CVector3& c_position) const {
919  if(m_cRangeX.WithinMinBoundIncludedMaxBoundIncluded(c_position.GetX()) &&
920  m_cRangeY.WithinMinBoundIncludedMaxBoundIncluded(c_position.GetY()) &&
921  m_cRangeZ.WithinMinBoundIncludedMaxBoundIncluded(c_position.GetZ())) {
922  n_i = Floor((c_position.GetX() - m_cAreaMinCorner.GetX()) * m_cInvCellSize.GetX());
923  n_j = Floor((c_position.GetY() - m_cAreaMinCorner.GetY()) * m_cInvCellSize.GetY());
924  n_k = Floor((c_position.GetZ() - m_cAreaMinCorner.GetZ()) * m_cInvCellSize.GetZ());
925  }
926  else {
927  THROW_ARGOSEXCEPTION("CGrid<ENTITY>::PositionToCell() : Position <" << c_position << "> out of bounds X -> " << m_cRangeX << " Y -> " << m_cRangeY << " Z -> " << m_cRangeZ);
928  }
929  }
930 
931  /****************************************/
932  /****************************************/
933 
934  template<class ENTITY>
936  SInt32& n_j,
937  SInt32& n_k,
938  const CVector3& c_position) const {
939  n_i = Floor((c_position.GetX() - m_cAreaMinCorner.GetX()) * m_cInvCellSize.GetX());
940  n_j = Floor((c_position.GetY() - m_cAreaMinCorner.GetY()) * m_cInvCellSize.GetY());
941  n_k = Floor((c_position.GetZ() - m_cAreaMinCorner.GetZ()) * m_cInvCellSize.GetZ());
942  }
943 
944  /****************************************/
945  /****************************************/
946 
947  template<class ENTITY>
949  SInt32& n_j,
950  SInt32& n_k) const {
951  if(n_i < 0) n_i = 0;
952  else if(n_i >= m_nSizeI) n_i = m_nSizeI - 1;
953  if(n_j < 0) n_j = 0;
954  else if(n_j >= m_nSizeJ) n_j = m_nSizeJ - 1;
955  if(n_k < 0) n_k = 0;
956  else if(n_k >= m_nSizeK) n_k = m_nSizeK - 1;
957  }
958 
959  /****************************************/
960  /****************************************/
961 
962  template<class ENTITY>
964  if(c_pos.GetX() < m_cRangeX.GetMin()) c_pos.SetX(m_cRangeX.GetMin() + EPSILON);
965  else if(c_pos.GetX() > m_cRangeX.GetMax()) c_pos.SetX(m_cRangeX.GetMax() - EPSILON);
966  if(c_pos.GetY() < m_cRangeY.GetMin()) c_pos.SetY(m_cRangeY.GetMin() + EPSILON);
967  else if(c_pos.GetY() > m_cRangeY.GetMax()) c_pos.SetY(m_cRangeY.GetMax() - EPSILON);
968  if(c_pos.GetZ() < m_cRangeZ.GetMin()) c_pos.SetZ(m_cRangeZ.GetMin() + EPSILON);
969  else if(c_pos.GetZ() > m_cRangeZ.GetMax()) c_pos.SetZ(m_cRangeZ.GetMax() - EPSILON);
970  }
971 
972  /****************************************/
973  /****************************************/
974 
975  template<class ENTITY>
977  SInt32 n_j,
978  SInt32 n_k) {
979  return m_psCells[m_nSizeI * m_nSizeJ * n_k +
980  m_nSizeI * n_j +
981  n_i];
982  }
983 
984  /****************************************/
985  /****************************************/
986 
987  template<class ENTITY>
989  SInt32 n_j,
990  SInt32 n_k) const {
991  return m_psCells[m_nSizeI * m_nSizeJ * n_k +
992  m_nSizeI * n_j +
993  n_i];
994  }
995 
996  /****************************************/
997  /****************************************/
998 
999 }
CRange< Real > m_cRangeZ
Definition: grid.h:156
signed int SInt32
32-bit signed integer.
Definition: datatypes.h:93
A 3D vector class.
Definition: vector3.h:29
virtual void ForAllCells(CCellOperation &c_operation)
Definition: grid_impl.h:521
CSet< ENTITY *, SEntityComparator > Entities
Definition: grid.h:19
#define Sqrt
Definition: general.h:64
virtual void ForEntitiesInCircleRange(const CVector3 &c_center, Real f_radius, CEntityOperation &c_operation)
Executes an operation on all entities within the specified circle range.
Definition: grid_impl.h:283
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
CVector3 & GetEnd()
Definition: ray3.h:45
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector2.h:78
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
Definition: general.h:25
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
virtual void ForEntitiesInRectangleRange(const CVector3 &c_center, const CVector2 &c_half_size, CEntityOperation &c_operation)
Executes an operation on all entities within the specified rectangle range.
Definition: grid_impl.h:321
void SetUpdateEntityOperation(CEntityOperation *pc_operation)
Definition: grid_impl.h:907
virtual void Reset()
Resets the resource.
Definition: grid_impl.h:91
SCell * m_psCells
Definition: grid.h:159
void UpdateCell(SInt32 n_i, SInt32 n_j, SInt32 n_k, ENTITY &c_entity)
Definition: grid_impl.h:884
void PositionToCellUnsafe(SInt32 &n_i, SInt32 &n_j, SInt32 &n_k, const CVector3 &c_position) const
Definition: grid_impl.h:935
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector2.h:94
void clear()
Erases the contents of the list.
Definition: set.h:351
Defines a very simple double-linked list that stores unique elements.
Definition: set.h:101
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
CRange< Real > m_cRangeY
Definition: grid.h:155
virtual void ForEntitiesInSphereRange(const CVector3 &c_center, Real f_radius, CEntityOperation &c_operation)
Executes an operation on all entities within the specified sphere range.
Definition: grid_impl.h:171
#define APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK)
Definition: grid_impl.h:24
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
Definition: vector3.h:143
A 2D vector class.
Definition: vector2.h:25
virtual void GetEntitiesAt(CSet< ENTITY *, SEntityComparator > &c_entities, const CVector3 &c_position) const
Puts the entities located at the given point in the passed buffer.
Definition: grid_impl.h:139
void SetX(const Real f_x)
Sets the x coordinate of this vector.
Definition: vector3.h:101
CRange< Real > m_cRangeX
Definition: grid.h:154
SInt32 m_nSizeK
Definition: grid.h:153
CVector3 m_cInvCellSize
Definition: grid.h:158
virtual void ForEntitiesAlongRay(const CRay3 &c_ray, CEntityOperation &c_operation, bool b_stop_at_closest_match=false)
Executes an operation on all entities that intersect the given ray.
Definition: grid_impl.h:342
virtual void ForEntitiesInBoxRange(const CVector3 &c_center, const CVector3 &c_half_size, CEntityOperation &c_operation)
Executes an operation on all entities within the specified box range.
Definition: grid_impl.h:260
CVector3 m_cCellSize
Definition: grid.h:157
virtual void AddEntity(ENTITY &c_entity)
Adds an entity to this index.
Definition: grid_impl.h:114
void PositionToCell(SInt32 &n_i, SInt32 &n_j, SInt32 &n_k, const CVector3 &c_position) const
Definition: grid_impl.h:915
The exception that wraps all errors in ARGoS.
virtual void ForCellsInCircleRange(const CVector3 &c_center, Real f_radius, CCellOperation &c_operation)
Definition: grid_impl.h:647
CVector3 & GetStart()
Definition: ray3.h:37
The CSet iterator.
Definition: set.h:39
virtual void ForCellsInRectangleRange(const CVector3 &c_center, const CVector2 &c_half_size, CCellOperation &c_operation)
Definition: grid_impl.h:685
void SetY(const Real f_y)
Sets the y coordinate of this vector.
Definition: vector3.h:117
virtual void Update()
Updates this positional index.
Definition: grid_impl.h:130
virtual void RemoveEntity(ENTITY &c_entity)
Removes an entity from this index.
Definition: grid_impl.h:122
size_t Timestamp
Definition: grid.h:20
virtual void ForAllEntities(CEntityOperation &c_operation)
Executes an operation on all the indexed entities.
Definition: grid_impl.h:161
virtual void Destroy()
Undoes whatever was done by Init().
Definition: grid_impl.h:107
SInt32 m_nSizeJ
Definition: grid.h:152
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
Definition: grid_impl.h:84
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
void ClampCoordinates(SInt32 &n_i, SInt32 &n_j, SInt32 &n_k) const
Definition: grid_impl.h:948
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:125
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
Definition: vector3.h:133
#define APPLY_ENTITY_OPERATION_TO_CELL(nI, nJ, nK)
Definition: grid_impl.h:11
T GetSpan() const
Definition: range.h:64
virtual ~CGrid()
Definition: grid_impl.h:76
SInt32 m_nSizeI
Definition: grid.h:151
virtual void ForCellsAlongRay(const CRay3 &c_ray, CCellOperation &c_operation)
Definition: grid_impl.h:706
SCell & GetCellAt(SInt32 n_i, SInt32 n_j, SInt32 n_k)
Definition: grid_impl.h:976
The operation to perform on each entity found in range.
virtual void ForCellsInSphereRange(const CVector3 &c_center, Real f_radius, CCellOperation &c_operation)
Definition: grid_impl.h:535
#define APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK)
Definition: grid_impl.h:38
SInt32 Floor(Real f_value)
Rounds the passed floating-point value to the closest lower integer.
Definition: general.h:140
virtual void ForCellsInBoxRange(const CVector3 &c_center, const CVector3 &c_half_size, CCellOperation &c_operation)
Definition: grid_impl.h:624
CGrid(const CVector3 &c_area_min_corner, const CVector3 &c_area_max_corner, SInt32 n_size_i, SInt32 n_size_j, SInt32 n_size_k)
Definition: grid_impl.h:48