ARGoS 3
A parallel, multi-engine simulator for swarm robotics
grid_impl.h
Go to the documentation of this file.
1namespace 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
47template<class ENTITY>
48CGrid<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) {
63 m_cCellSize.Set(m_cRangeX.GetSpan() / m_nSizeI,
64 m_cRangeY.GetSpan() / m_nSizeJ,
65 m_cRangeZ.GetSpan() / m_nSizeK);
66 m_cInvCellSize.Set(1.0f / m_cCellSize.GetX(),
67 1.0f / m_cCellSize.GetY(),
68 1.0f / m_cCellSize.GetZ());
69 m_psCells = new SCell[m_nSizeI * m_nSizeJ * m_nSizeK];
70}
71
72 /****************************************/
73 /****************************************/
74
75 template<class ENTITY>
76 CGrid<ENTITY>::~CGrid() {
77 delete[] m_psCells;
78 }
79
80 /****************************************/
81 /****************************************/
82
83 template<class ENTITY>
86
87 /****************************************/
88 /****************************************/
89
90 template<class ENTITY>
91 void CGrid<ENTITY>::Reset() {
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>
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>
130 void CGrid<ENTITY>::Update() {
131 ++m_unCurTimestamp;
132 ForAllEntities(*m_pcUpdateEntityOperation);
133 }
134
135 /****************************************/
136 /****************************************/
137
138 template<class ENTITY>
139 void CGrid<ENTITY>::GetEntitiesAt(CSet<ENTITY*,SEntityComparator>& c_entities,
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>
161 void CGrid<ENTITY>::ForAllEntities(CEntityOperation& c_operation) {
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>
171 void CGrid<ENTITY>::ForEntitiesInSphereRange(const CVector3& c_center,
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>
260 void CGrid<ENTITY>::ForEntitiesInBoxRange(const CVector3& c_center,
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) {
273 APPLY_ENTITY_OPERATION_TO_CELL(i, j, k);
274 }
275 }
276 }
277 }
278
279 /****************************************/
280 /****************************************/
281
282 template<class ENTITY>
283 void CGrid<ENTITY>::ForEntitiesInCircleRange(const CVector3& c_center,
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>
321 void CGrid<ENTITY>::ForEntitiesInRectangleRange(const CVector3& c_center,
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) {
333 APPLY_ENTITY_OPERATION_TO_CELL(i, j, nK);
334 }
335 }
336 }
337
338 /****************************************/
339 /****************************************/
340
341 template<class ENTITY>
342 void CGrid<ENTITY>::ForEntitiesAlongRay(const CRay3& c_ray,
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);
370 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
371 /* Cycle through cells */
372 for(SInt32 nCell = nDI; nCell > 0; --nCell) {
373 /* Advance on driving axis */
374 nI += nSI;
375 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
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;
381 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
382 nK += nSK;
383 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
384 }
385 else {
386 nK += nSK;
387 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
388 nJ += nSJ;
389 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
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;
397 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
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;
406 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
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);
420 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
421 /* Cycle through cells */
422 for(SInt32 nCell = nDJ; nCell > 0; --nCell) {
423 /* Advance on driving axis */
424 nJ += nSJ;
425 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
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;
431 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
432 nK += nSK;
433 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
434 }
435 else {
436 nK += nSK;
437 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
438 nI += nSI;
439 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
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;
447 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
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;
456 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
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);
470 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
471 /* Cycle through cells */
472 for(SInt32 nCell = nDK; nCell > 0; --nCell) {
473 /* Advance on driving axis */
474 nK += nSK;
475 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
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;
481 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
482 nJ += nSJ;
483 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
484 }
485 else {
486 nJ += nSJ;
487 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
488 nI += nSI;
489 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
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;
497 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
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;
506 APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK);
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>
521 void CGrid<ENTITY>::ForAllCells(CCellOperation& c_operation) {
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) {
525 APPLY_CELL_OPERATION_TO_CELL(i, j, k);
526 }
527 }
528 }
529 }
530
531 /****************************************/
532 /****************************************/
533
534 template<class ENTITY>
535 void CGrid<ENTITY>::ForCellsInSphereRange(const CVector3& c_center,
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>
624 void CGrid<ENTITY>::ForCellsInBoxRange(const CVector3& c_center,
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) {
637 APPLY_CELL_OPERATION_TO_CELL(i, j, k);
638 }
639 }
640 }
641 }
642
643 /****************************************/
644 /****************************************/
645
646 template<class ENTITY>
647 void CGrid<ENTITY>::ForCellsInCircleRange(const CVector3& c_center,
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>
685 void CGrid<ENTITY>::ForCellsInRectangleRange(const CVector3& c_center,
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) {
697 APPLY_CELL_OPERATION_TO_CELL(i, j, nK);
698 }
699 }
700 }
701
702 /****************************************/
703 /****************************************/
704
705 template<class ENTITY>
706 void CGrid<ENTITY>::ForCellsAlongRay(const CRay3& c_ray,
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>
884 void CGrid<ENTITY>::UpdateCell(SInt32 n_i,
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>
907 void CGrid<ENTITY>::SetUpdateEntityOperation(CEntityOperation* pc_operation) {
908 m_pcUpdateEntityOperation = pc_operation;
909 }
910
911 /****************************************/
912 /****************************************/
913
914 template<class ENTITY>
915 void CGrid<ENTITY>::PositionToCell(SInt32& n_i,
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>
935 void CGrid<ENTITY>::PositionToCellUnsafe(SInt32& n_i,
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>
948 void CGrid<ENTITY>::ClampCoordinates(SInt32& n_i,
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>
963 void CGrid<ENTITY>::ClampCoordinates(CVector3& c_pos) const {
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>
976 typename CGrid<ENTITY>::SCell& CGrid<ENTITY>::GetCellAt(SInt32 n_i,
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>
988 const typename CGrid<ENTITY>::SCell& CGrid<ENTITY>::GetCellAt(SInt32 n_i,
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}
Sqrt
#define Sqrt
Definition general.h:64
SInt32
signed int SInt32
32-bit signed integer.
Definition datatypes.h:93
Real
float Real
Collects all ARGoS code.
Definition datatypes.h:39
THROW_ARGOSEXCEPTION_NESTED
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception.
Definition argos_exception.h:115
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition argos_exception.h:111
APPLY_ENTITY_OPERATION_TO_CELL
#define APPLY_ENTITY_OPERATION_TO_CELL(nI, nJ, nK)
Definition grid_impl.h:11
APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY
#define APPLY_ENTITY_OPERATION_TO_CELL_ALONG_RAY(nI, nJ, nK)
Definition grid_impl.h:24
APPLY_CELL_OPERATION_TO_CELL
#define APPLY_CELL_OPERATION_TO_CELL(nI, nJ, nK)
Definition grid_impl.h:38
argos
The namespace containing all the ARGoS related code.
Definition ci_actuator.h:12
argos::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition argos_configuration.h:27
argos::Floor
SInt32 Floor(Real f_value)
Rounds the passed floating-point value to the closest lower integer.
Definition general.h:140
argos::Max
T Max(const T &t_v1, const T &t_v2)
Returns the bigger of the two passed arguments.
Definition general.h:95
argos::Abs
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
Definition general.h:25
argos::Min
T Min(const T &t_v1, const T &t_v2)
Returns the smaller of the two passed arguments.
Definition general.h:77
argos::CGrid::Reset
virtual void Reset()
Resets the resource.
Definition grid_impl.h:91
argos::CGrid::GetCellAt
SCell & GetCellAt(SInt32 n_i, SInt32 n_j, SInt32 n_k)
Definition grid_impl.h:976
argos::CGrid::Update
virtual void Update()
Updates this positional index.
Definition grid_impl.h:130
argos::CGrid::ForCellsInBoxRange
virtual void ForCellsInBoxRange(const CVector3 &c_center, const CVector3 &c_half_size, CCellOperation &c_operation)
Definition grid_impl.h:624
argos::CGrid::UpdateCell
void UpdateCell(SInt32 n_i, SInt32 n_j, SInt32 n_k, ENTITY &c_entity)
Definition grid_impl.h:884
argos::CGrid::ClampCoordinates
void ClampCoordinates(SInt32 &n_i, SInt32 &n_j, SInt32 &n_k) const
Definition grid_impl.h:948
argos::CGrid::ForEntitiesInRectangleRange
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
argos::CGrid::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
Definition grid_impl.h:84
argos::CGrid::Destroy
virtual void Destroy()
Undoes whatever was done by Init().
Definition grid_impl.h:107
argos::CGrid::ForEntitiesInSphereRange
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
argos::CGrid::ForEntitiesInCircleRange
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
argos::CGrid::ForCellsAlongRay
virtual void ForCellsAlongRay(const CRay3 &c_ray, CCellOperation &c_operation)
Definition grid_impl.h:706
argos::CGrid::ForCellsInCircleRange
virtual void ForCellsInCircleRange(const CVector3 &c_center, Real f_radius, CCellOperation &c_operation)
Definition grid_impl.h:647
argos::CGrid::AddEntity
virtual void AddEntity(ENTITY &c_entity)
Adds an entity to this index.
Definition grid_impl.h:114
argos::CGrid::~CGrid
virtual ~CGrid()
Definition grid_impl.h:76
argos::CGrid::PositionToCellUnsafe
void PositionToCellUnsafe(SInt32 &n_i, SInt32 &n_j, SInt32 &n_k, const CVector3 &c_position) const
Definition grid_impl.h:935
argos::CGrid::ForEntitiesAlongRay
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
argos::CGrid::PositionToCell
void PositionToCell(SInt32 &n_i, SInt32 &n_j, SInt32 &n_k, const CVector3 &c_position) const
Definition grid_impl.h:915
argos::CGrid::ForEntitiesInBoxRange
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
argos::CGrid::SetUpdateEntityOperation
void SetUpdateEntityOperation(CEntityOperation *pc_operation)
Definition grid_impl.h:907
argos::CGrid::RemoveEntity
virtual void RemoveEntity(ENTITY &c_entity)
Removes an entity from this index.
Definition grid_impl.h:122
argos::CGrid::ForAllEntities
virtual void ForAllEntities(CEntityOperation &c_operation)
Executes an operation on all the indexed entities.
Definition grid_impl.h:161
argos::CGrid::ForCellsInSphereRange
virtual void ForCellsInSphereRange(const CVector3 &c_center, Real f_radius, CCellOperation &c_operation)
Definition grid_impl.h:535
argos::CGrid::ForCellsInRectangleRange
virtual void ForCellsInRectangleRange(const CVector3 &c_center, const CVector2 &c_half_size, CCellOperation &c_operation)
Definition grid_impl.h:685
argos::CGrid::ForAllCells
virtual void ForAllCells(CCellOperation &c_operation)
Definition grid_impl.h:521
argos::CGrid::GetEntitiesAt
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
argos::CGrid::CGrid
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
argos::CGrid::SCell::Timestamp
size_t Timestamp
Definition grid.h:20
argos::CGrid::SCell::Entities
CSet< ENTITY *, SEntityComparator > Entities
Definition grid.h:19
argos::CPositionalIndex::COperation
The operation to perform on each entity found in range.
Definition positional_index.h:60
argos::CSetIterator
The CSet iterator.
Definition set.h:39
argos::CSet
Defines a very simple double-linked list that stores unique elements.
Definition set.h:101
argos::CSet::clear
void clear()
Erases the contents of the list.
Definition set.h:351
argos::CRay3::GetStart
CVector3 & GetStart()
Definition ray3.h:37
argos::CRay3::GetEnd
CVector3 & GetEnd()
Definition ray3.h:45
argos::CVector2
A 2D vector class.
Definition vector2.h:27
argos::CVector2::GetY
Real GetY() const
Returns the y coordinate of this vector.
Definition vector2.h:110
argos::CVector2::GetX
Real GetX() const
Returns the x coordinate of this vector.
Definition vector2.h:94
argos::CVector3
A 3D vector class.
Definition vector3.h:31
argos::CVector3::SetY
void SetY(const Real f_y)
Sets the y coordinate of this vector.
Definition vector3.h:129
argos::CVector3::GetX
Real GetX() const
Returns the x coordinate of this vector.
Definition vector3.h:105
argos::CVector3::SetX
void SetX(const Real f_x)
Sets the x coordinate of this vector.
Definition vector3.h:113
argos::CVector3::SetZ
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
Definition vector3.h:145
argos::CVector3::GetY
Real GetY() const
Returns the y coordinate of this vector.
Definition vector3.h:121
argos::CVector3::GetZ
Real GetZ() const
Returns the z coordinate of this vector.
Definition vector3.h:137