|
4
|
1
|
|
|
2 // written in the D programming language
|
|
|
3
|
|
|
4 module chipmunkd.cpSpaceHash;
|
|
|
5
|
|
|
6 import chipmunkd.chipmunk;
|
|
|
7 import chipmunkd.prime;
|
|
|
8
|
|
|
9 // The spatial hash is Chipmunk's default (and currently only) spatial index type.
|
|
|
10 // Based on a chained hash table.
|
|
|
11
|
|
|
12 // Used internally to track objects added to the hash
|
|
|
13 struct cpHandle{
|
|
|
14 // Pointer to the object
|
|
|
15 void *obj;
|
|
|
16 // Retain count
|
|
|
17 int retain;
|
|
|
18 // Query stamp. Used to make sure two objects
|
|
|
19 // aren't identified twice in the same query.
|
|
|
20 cpTimestamp stamp;
|
|
|
21 }
|
|
|
22
|
|
|
23 // Linked list element for in the chains.
|
|
|
24 struct cpSpaceHashBin{
|
|
|
25 cpHandle *handle;
|
|
|
26 cpSpaceHashBin *next;
|
|
|
27 }
|
|
|
28
|
|
|
29 // BBox callback. Called whenever the hash needs a bounding box from an object.
|
|
|
30 alias cpBB function(void *obj)cpSpaceHashBBFunc;
|
|
|
31
|
|
|
32 struct cpSpaceHash{
|
|
|
33 // Number of cells in the table.
|
|
|
34 int numcells;
|
|
|
35 // Dimentions of the cells.
|
|
|
36 cpFloat celldim;
|
|
|
37
|
|
|
38 // BBox callback.
|
|
|
39 cpSpaceHashBBFunc bbfunc;
|
|
|
40
|
|
|
41 // Hashset of the handles and the recycled ones.
|
|
|
42 cpHashSet *handleSet;
|
|
|
43 cpArray *pooledHandles;
|
|
|
44
|
|
|
45 // The table and the recycled bins.
|
|
|
46 cpSpaceHashBin **table;
|
|
|
47 cpSpaceHashBin *pooledBins;
|
|
|
48
|
|
|
49 // list of buffers to free on destruction.
|
|
|
50 cpArray *allocatedBuffers;
|
|
|
51
|
|
|
52 // Incremented on each query. See cpHandle.stamp.
|
|
|
53 cpTimestamp stamp;
|
|
|
54 }
|
|
|
55
|
|
|
56 ////Basic allocation/destruction functions.
|
|
|
57 //cpSpaceHash *cpSpaceHashAlloc(void);
|
|
|
58 //cpSpaceHash *cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc);
|
|
|
59 //cpSpaceHash *cpSpaceHashNew(cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc);
|
|
|
60 //
|
|
|
61 //void cpSpaceHashDestroy(cpSpaceHash *hash);
|
|
|
62 //void cpSpaceHashFree(cpSpaceHash *hash);
|
|
|
63 //
|
|
|
64 //// Resize the hashtable. (Does not rehash! You must call cpSpaceHashRehash() if needed.)
|
|
|
65 //void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells);
|
|
|
66 //
|
|
|
67 //// Add an object to the hash.
|
|
|
68 //void cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue id, cpBB _deprecated_ignored);
|
|
|
69 //// Remove an object from the hash.
|
|
|
70 //void cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue id);
|
|
|
71 //
|
|
|
72 //// Iterator function
|
|
|
73 alias void function(void *obj, void *data)cpSpaceHashIterator;
|
|
|
74 //// Iterate over the objects in the hash.
|
|
|
75 //void cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data);
|
|
|
76 //
|
|
|
77 //// Rehash the contents of the hash.
|
|
|
78 //void cpSpaceHashRehash(cpSpaceHash *hash);
|
|
|
79 //// Rehash only a specific object.
|
|
|
80 //void cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue id);
|
|
|
81 //
|
|
|
82 //// Query callback.
|
|
|
83 alias void function(void *obj1, void *obj2, void *data)cpSpaceHashQueryFunc;
|
|
|
84 //// Point query the hash. A reference to the query point is passed as obj1 to the query callback.
|
|
|
85 //void cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func, void *data);
|
|
|
86 //// Query the hash for a given BBox.
|
|
|
87 //void cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data);
|
|
|
88 //// Run a query for the object, then insert it. (Optimized case)
|
|
|
89 //void cpSpaceHashQueryInsert(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data);
|
|
|
90 //// Rehashes while querying for each object. (Optimized case)
|
|
|
91 //void cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data);
|
|
|
92 //
|
|
|
93 //// Segment Query callback.
|
|
|
94 //// Return value is uesd for early exits of the query.
|
|
|
95 //// If while traversing the grid, the raytrace function detects that an entire grid cell is beyond the hit point, it will stop the trace.
|
|
|
96 alias cpFloat function(void *obj1, void *obj2, void *data)cpSpaceHashSegmentQueryFunc;
|
|
|
97 //void cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpaceHashSegmentQueryFunc func, void *data);
|
|
|
98 //
|
|
|
99
|
|
|
100 static cpHandle*
|
|
|
101 cpHandleInit(cpHandle *hand, void *obj)
|
|
|
102 {
|
|
|
103 hand.obj = obj;
|
|
|
104 hand.retain = 0;
|
|
|
105 hand.stamp = 0;
|
|
|
106
|
|
|
107 return hand;
|
|
|
108 }
|
|
|
109
|
|
|
110 static void cpHandleRetain(cpHandle *hand){hand.retain++;}
|
|
|
111
|
|
|
112 static void
|
|
|
113 cpHandleRelease(cpHandle *hand, cpArray *pooledHandles)
|
|
|
114 {
|
|
|
115 hand.retain--;
|
|
|
116 if(hand.retain == 0) cpArrayPush(pooledHandles, hand);
|
|
|
117 }
|
|
|
118
|
|
|
119 cpSpaceHash*
|
|
|
120 cpSpaceHashAlloc()
|
|
|
121 {
|
|
|
122 return cast(cpSpaceHash *)cpcalloc(1, cpSpaceHash.sizeof);
|
|
|
123 }
|
|
|
124
|
|
|
125 // Frees the old table, and allocate a new one.
|
|
|
126 static void
|
|
|
127 cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells)
|
|
|
128 {
|
|
|
129 cpfree(hash.table);
|
|
|
130
|
|
|
131 hash.numcells = numcells;
|
|
|
132 hash.table = cast(cpSpaceHashBin **)cpcalloc(numcells, (cpSpaceHashBin *).sizeof);
|
|
|
133 }
|
|
|
134
|
|
|
135 // Equality function for the handleset.
|
|
|
136 static int handleSetEql(void *obj, cpHandle* hand){return (obj == hand.obj);}
|
|
|
137
|
|
|
138 // Transformation function for the handleset.
|
|
|
139 static void *
|
|
|
140 handleSetTrans(void *obj, cpSpaceHash *hash)
|
|
|
141 {
|
|
|
142 if(hash.pooledHandles.num == 0){
|
|
|
143 // handle pool is exhausted, make more
|
|
|
144 int count = CP_BUFFER_BYTES/cpHandle.sizeof;
|
|
|
145 assert(count, "Buffer size is too small.");
|
|
|
146
|
|
|
147 cpHandle *buffer = cast(cpHandle *)cpmalloc(CP_BUFFER_BYTES);
|
|
|
148 cpArrayPush(hash.allocatedBuffers, buffer);
|
|
|
149
|
|
|
150 for(int i=0; i<count; i++) cpArrayPush(hash.pooledHandles, buffer + i);
|
|
|
151 }
|
|
|
152
|
|
|
153 cpHandle *hand = cpHandleInit(cast(cpHandle *) cpArrayPop(hash.pooledHandles), obj);
|
|
|
154 cpHandleRetain(hand);
|
|
|
155
|
|
|
156 return hand;
|
|
|
157 }
|
|
|
158
|
|
|
159 cpSpaceHash*
|
|
|
160 cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpaceHashBBFunc bbfunc)
|
|
|
161 {
|
|
|
162 cpSpaceHashAllocTable(hash, next_prime(numcells));
|
|
|
163 hash.celldim = celldim;
|
|
|
164 hash.bbfunc = bbfunc;
|
|
|
165
|
|
|
166 hash.handleSet = cpHashSetNew(0, cast(cpHashSetEqlFunc)&handleSetEql, cast(cpHashSetTransFunc)&handleSetTrans);
|
|
|
167 hash.pooledHandles = cpArrayNew(0);
|
|
|
168
|
|
|
169 hash.pooledBins = null;
|
|
|
170 hash.allocatedBuffers = cpArrayNew(0);
|
|
|
171
|
|
|
172 hash.stamp = 1;
|
|
|
173
|
|
|
174 return hash;
|
|
|
175 }
|
|
|
176
|
|
|
177 cpSpaceHash*
|
|
|
178 cpSpaceHashNew(cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc)
|
|
|
179 {
|
|
|
180 return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc);
|
|
|
181 }
|
|
|
182
|
|
|
183 static void
|
|
|
184 recycleBin(cpSpaceHash *hash, cpSpaceHashBin *bin)
|
|
|
185 {
|
|
|
186 bin.next = hash.pooledBins;
|
|
|
187 hash.pooledBins = bin;
|
|
|
188 }
|
|
|
189
|
|
|
190 static void
|
|
|
191 clearHashCell(cpSpaceHash *hash, int idx)
|
|
|
192 {
|
|
|
193 cpSpaceHashBin *bin = hash.table[idx];
|
|
|
194 while(bin){
|
|
|
195 cpSpaceHashBin *next = bin.next;
|
|
|
196
|
|
|
197 cpHandleRelease(bin.handle, hash.pooledHandles);
|
|
|
198 recycleBin(hash, bin);
|
|
|
199
|
|
|
200 bin = next;
|
|
|
201 }
|
|
|
202
|
|
|
203 hash.table[idx] = null;
|
|
|
204 }
|
|
|
205
|
|
|
206 // Clear all cells in the hashtable.
|
|
|
207 static void
|
|
|
208 clearHash(cpSpaceHash *hash)
|
|
|
209 {
|
|
|
210 for(int i=0; i<hash.numcells; i++)
|
|
|
211 clearHashCell(hash, i);
|
|
|
212 }
|
|
|
213
|
|
|
214 static void freeWrap(void *ptr, void *unused){cpfree(ptr);}
|
|
|
215
|
|
|
216 void
|
|
|
217 cpSpaceHashDestroy(cpSpaceHash *hash)
|
|
|
218 {
|
|
|
219 clearHash(hash);
|
|
|
220
|
|
|
221 cpHashSetFree(hash.handleSet);
|
|
|
222
|
|
|
223 cpArrayEach(hash.allocatedBuffers, &freeWrap, null);
|
|
|
224 cpArrayFree(hash.allocatedBuffers);
|
|
|
225 cpArrayFree(hash.pooledHandles);
|
|
|
226
|
|
|
227 cpfree(hash.table);
|
|
|
228 }
|
|
|
229
|
|
|
230 void
|
|
|
231 cpSpaceHashFree(cpSpaceHash *hash)
|
|
|
232 {
|
|
|
233 if(hash){
|
|
|
234 cpSpaceHashDestroy(hash);
|
|
|
235 cpfree(hash);
|
|
|
236 }
|
|
|
237 }
|
|
|
238
|
|
|
239 void
|
|
|
240 cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells)
|
|
|
241 {
|
|
|
242 // Clear the hash to release the old handle locks.
|
|
|
243 clearHash(hash);
|
|
|
244
|
|
|
245 hash.celldim = celldim;
|
|
|
246 cpSpaceHashAllocTable(hash, next_prime(numcells));
|
|
|
247 }
|
|
|
248
|
|
|
249 // Return true if the chain contains the handle.
|
|
|
250 static cpBool
|
|
|
251 containsHandle(cpSpaceHashBin *bin, cpHandle *hand)
|
|
|
252 {
|
|
|
253 while(bin){
|
|
|
254 if(bin.handle == hand) return cpTrue;
|
|
|
255 bin = bin.next;
|
|
|
256 }
|
|
|
257
|
|
|
258 return cpFalse;
|
|
|
259 }
|
|
|
260
|
|
|
261 // Get a recycled or new bin.
|
|
|
262 static cpSpaceHashBin *
|
|
|
263 getEmptyBin(cpSpaceHash *hash)
|
|
|
264 {
|
|
|
265 cpSpaceHashBin *bin = hash.pooledBins;
|
|
|
266
|
|
|
267 if(bin){
|
|
|
268 hash.pooledBins = bin.next;
|
|
|
269 return bin;
|
|
|
270 } else {
|
|
|
271 // Pool is exhausted, make more
|
|
|
272 int count = CP_BUFFER_BYTES/cpSpaceHashBin.sizeof;
|
|
|
273 assert(count, "Buffer size is too small.");
|
|
|
274
|
|
|
275 cpSpaceHashBin *buffer = cast(cpSpaceHashBin *)cpmalloc(CP_BUFFER_BYTES);
|
|
|
276 cpArrayPush(hash.allocatedBuffers, buffer);
|
|
|
277
|
|
|
278 // push all but the first one, return the first instead
|
|
|
279 for(int i=1; i<count; i++) recycleBin(hash, buffer + i);
|
|
|
280 return buffer;
|
|
|
281 }
|
|
|
282 }
|
|
|
283
|
|
|
284 // The hash function itself.
|
|
|
285 static cpHashValue
|
|
|
286 hash_func(cpHashValue x, cpHashValue y, cpHashValue n)
|
|
|
287 {
|
|
17
|
288 return cast(cpHashValue)(( (x*1640531513uL) ^ (y*2654435789uL) ) % n);
|
|
4
|
289 }
|
|
|
290
|
|
|
291 // Much faster than (int)floor(f)
|
|
|
292 // Profiling showed floor() to be a sizable performance hog
|
|
|
293 static int
|
|
|
294 floor_int(cpFloat f)
|
|
|
295 {
|
|
|
296 int i = cast(int)f;
|
|
|
297 return (f < 0.0f && f != i ? i - 1 : i);
|
|
|
298 }
|
|
|
299
|
|
|
300 static void
|
|
|
301 hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb)
|
|
|
302 {
|
|
|
303 // Find the dimensions in cell coordinates.
|
|
|
304 cpFloat dim = hash.celldim;
|
|
|
305 int l = floor_int(bb.l/dim); // Fix by ShiftZ
|
|
|
306 int r = floor_int(bb.r/dim);
|
|
|
307 int b = floor_int(bb.b/dim);
|
|
|
308 int t = floor_int(bb.t/dim);
|
|
|
309
|
|
|
310 int n = hash.numcells;
|
|
|
311 for(int i=l; i<=r; i++){
|
|
|
312 for(int j=b; j<=t; j++){
|
|
|
313 int idx = hash_func(i,j,n);
|
|
|
314 cpSpaceHashBin *bin = hash.table[idx];
|
|
|
315
|
|
|
316 // Don't add an object twice to the same cell.
|
|
|
317 if(containsHandle(bin, hand)) continue;
|
|
|
318
|
|
|
319 cpHandleRetain(hand);
|
|
|
320 // Insert a new bin for the handle in this cell.
|
|
|
321 cpSpaceHashBin *newBin = getEmptyBin(hash);
|
|
|
322 newBin.handle = hand;
|
|
|
323 newBin.next = bin;
|
|
|
324 hash.table[idx] = newBin;
|
|
|
325 }
|
|
|
326 }
|
|
|
327 }
|
|
|
328
|
|
|
329 void
|
|
|
330 cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid, cpBB _deprecated_unused)
|
|
|
331 {
|
|
|
332 cpHandle *hand = cast(cpHandle *)cpHashSetInsert(hash.handleSet, hashid, obj, hash);
|
|
|
333 hashHandle(hash, hand, hash.bbfunc(obj));
|
|
|
334 }
|
|
|
335
|
|
|
336 void
|
|
|
337 cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid)
|
|
|
338 {
|
|
|
339 cpHandle *hand = cast(cpHandle *)cpHashSetRemove(hash.handleSet, hashid, obj);
|
|
|
340
|
|
|
341 if(hand){
|
|
|
342 hand.obj = null;
|
|
|
343 cpHandleRelease(hand, hash.pooledHandles);
|
|
|
344
|
|
|
345 cpSpaceHashInsert(hash, obj, hashid, cpBBNew(0.0f, 0.0f, 0.0f, 0.0f));
|
|
|
346 }
|
|
|
347 }
|
|
|
348
|
|
|
349 static void handleRehashHelper(cpHandle *hand, cpSpaceHash *hash){hashHandle(hash, hand, hash.bbfunc(hand.obj));}
|
|
|
350
|
|
|
351 void
|
|
|
352 cpSpaceHashRehash(cpSpaceHash *hash)
|
|
|
353 {
|
|
|
354 clearHash(hash);
|
|
|
355 cpHashSetEach(hash.handleSet, cast(cpHashSetIterFunc)&handleRehashHelper, hash);
|
|
|
356 }
|
|
|
357
|
|
|
358 void
|
|
|
359 cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid)
|
|
|
360 {
|
|
|
361 cpHandle *hand = cast(cpHandle *)cpHashSetRemove(hash.handleSet, hashid, obj);
|
|
|
362
|
|
|
363 if(hand){
|
|
|
364 hand.obj = null;
|
|
|
365 cpHandleRelease(hand, hash.pooledHandles);
|
|
|
366 }
|
|
|
367 }
|
|
|
368
|
|
|
369 struct eachPair {
|
|
|
370 cpSpaceHashIterator func;
|
|
|
371 void *data;
|
|
|
372 }
|
|
|
373
|
|
|
374 static void eachHelper(cpHandle *hand, eachPair *pair){pair.func(hand.obj, pair.data);}
|
|
|
375
|
|
|
376 // Iterate over the objects in the spatial hash.
|
|
|
377 void
|
|
|
378 cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data)
|
|
|
379 {
|
|
|
380 eachPair pair = {func, data};
|
|
|
381 cpHashSetEach(hash.handleSet, cast(cpHashSetIterFunc)&eachHelper, &pair);
|
|
|
382 }
|
|
|
383
|
|
|
384 static void
|
|
|
385 removeOrphanedHandles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr)
|
|
|
386 {
|
|
|
387 cpSpaceHashBin *bin = *bin_ptr;
|
|
|
388 while(bin){
|
|
|
389 cpHandle *hand = bin.handle;
|
|
|
390 cpSpaceHashBin *next = bin.next;
|
|
|
391
|
|
|
392 if(!hand.obj){
|
|
|
393 // orphaned handle, unlink and recycle the bin
|
|
|
394 (*bin_ptr) = bin.next;
|
|
|
395 recycleBin(hash, bin);
|
|
|
396
|
|
|
397 cpHandleRelease(hand, hash.pooledHandles);
|
|
|
398 } else {
|
|
|
399 bin_ptr = &bin.next;
|
|
|
400 }
|
|
|
401
|
|
|
402 bin = next;
|
|
|
403 }
|
|
|
404 }
|
|
|
405
|
|
|
406 // Calls the callback function for the objects in a given chain.
|
|
|
407 static void
|
|
|
408 query(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpaceHashQueryFunc func, void *data)
|
|
|
409 {
|
|
|
410 restart:
|
|
|
411 for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin.next){
|
|
|
412 cpHandle *hand = bin.handle;
|
|
|
413 void *other = hand.obj;
|
|
|
414
|
|
|
415 if(hand.stamp == hash.stamp || obj == other){
|
|
|
416 continue;
|
|
|
417 } else if(other){
|
|
|
418 func(obj, other, data);
|
|
|
419 hand.stamp = hash.stamp;
|
|
|
420 } else {
|
|
|
421 // The object for this handle has been removed
|
|
|
422 // cleanup this cell and restart the query
|
|
|
423 removeOrphanedHandles(hash, bin_ptr);
|
|
|
424 goto restart; // GCC not smart enough/able to tail call an inlined function.
|
|
|
425 }
|
|
|
426 }
|
|
|
427 }
|
|
|
428
|
|
|
429 void
|
|
|
430 cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func, void *data)
|
|
|
431 {
|
|
|
432 cpFloat dim = hash.celldim;
|
|
|
433 int idx = hash_func(floor_int(point.x/dim), floor_int(point.y/dim), hash.numcells); // Fix by ShiftZ
|
|
|
434
|
|
|
435 query(hash, &hash.table[idx], &point, func, data);
|
|
|
436 hash.stamp++;
|
|
|
437 }
|
|
|
438
|
|
|
439 void
|
|
|
440 cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data)
|
|
|
441 {
|
|
|
442 // Get the dimensions in cell coordinates.
|
|
|
443 cpFloat dim = hash.celldim;
|
|
|
444 int l = floor_int(bb.l/dim); // Fix by ShiftZ
|
|
|
445 int r = floor_int(bb.r/dim);
|
|
|
446 int b = floor_int(bb.b/dim);
|
|
|
447 int t = floor_int(bb.t/dim);
|
|
|
448
|
|
|
449 int n = hash.numcells;
|
|
|
450 cpSpaceHashBin **table = hash.table;
|
|
|
451
|
|
|
452 // Iterate over the cells and query them.
|
|
|
453 for(int i=l; i<=r; i++){
|
|
|
454 for(int j=b; j<=t; j++){
|
|
|
455 query(hash, &table[hash_func(i,j,n)], obj, func, data);
|
|
|
456 }
|
|
|
457 }
|
|
|
458
|
|
|
459 hash.stamp++;
|
|
|
460 }
|
|
|
461
|
|
|
462 // Similar to struct eachPair above.
|
|
|
463 struct queryRehashPair {
|
|
|
464 cpSpaceHash *hash;
|
|
|
465 cpSpaceHashQueryFunc func;
|
|
|
466 void *data;
|
|
|
467 }
|
|
|
468
|
|
|
469 // Hashset iterator func used with cpSpaceHashQueryRehash().
|
|
|
470 static void
|
|
|
471 handleQueryRehashHelper(void *elt, void *data)
|
|
|
472 {
|
|
|
473 cpHandle *hand = cast(cpHandle *)elt;
|
|
|
474
|
|
|
475 // Unpack the user callback data.
|
|
|
476 queryRehashPair *pair = cast(queryRehashPair *)data;
|
|
|
477 cpSpaceHash *hash = pair.hash;
|
|
|
478 cpSpaceHashQueryFunc func = pair.func;
|
|
|
479
|
|
|
480 cpFloat dim = hash.celldim;
|
|
|
481 int n = hash.numcells;
|
|
|
482
|
|
|
483 void *obj = hand.obj;
|
|
|
484 cpBB bb = hash.bbfunc(obj);
|
|
|
485
|
|
|
486 int l = floor_int(bb.l/dim);
|
|
|
487 int r = floor_int(bb.r/dim);
|
|
|
488 int b = floor_int(bb.b/dim);
|
|
|
489 int t = floor_int(bb.t/dim);
|
|
|
490
|
|
|
491 cpSpaceHashBin **table = hash.table;
|
|
|
492
|
|
|
493 for(int i=l; i<=r; i++){
|
|
|
494 for(int j=b; j<=t; j++){
|
|
|
495 int idx = hash_func(i,j,n);
|
|
|
496 cpSpaceHashBin *bin = table[idx];
|
|
|
497
|
|
|
498 if(containsHandle(bin, hand)) continue;
|
|
|
499
|
|
|
500 cpHandleRetain(hand); // this MUST be done first in case the object is removed in func()
|
|
|
501 query(hash, &bin, obj, func, pair.data);
|
|
|
502
|
|
|
503 cpSpaceHashBin *newBin = getEmptyBin(hash);
|
|
|
504 newBin.handle = hand;
|
|
|
505 newBin.next = bin;
|
|
|
506 table[idx] = newBin;
|
|
|
507 }
|
|
|
508 }
|
|
|
509
|
|
|
510 // Increment the stamp for each object hashed.
|
|
|
511 hash.stamp++;
|
|
|
512 }
|
|
|
513
|
|
|
514 void
|
|
|
515 cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data)
|
|
|
516 {
|
|
|
517 clearHash(hash);
|
|
|
518
|
|
|
519 queryRehashPair pair = {hash, func, data};
|
|
|
520 cpHashSetEach(hash.handleSet, &handleQueryRehashHelper, &pair);
|
|
|
521 }
|
|
|
522
|
|
|
523 static cpFloat
|
|
|
524 segmentQuery(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpaceHashSegmentQueryFunc func, void *data)
|
|
|
525 {
|
|
|
526 cpFloat t = 1.0f;
|
|
|
527
|
|
|
528 restart:
|
|
|
529 for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin.next){
|
|
|
530 cpHandle *hand = bin.handle;
|
|
|
531 void *other = hand.obj;
|
|
|
532
|
|
|
533 // Skip over certain conditions
|
|
|
534 if(hand.stamp == hash.stamp){
|
|
|
535 continue;
|
|
|
536 } else if(other){
|
|
|
537 t = cpfmin(t, func(obj, other, data));
|
|
|
538 hand.stamp = hash.stamp;
|
|
|
539 } else {
|
|
|
540 // The object for this handle has been removed
|
|
|
541 // cleanup this cell and restart the query
|
|
|
542 removeOrphanedHandles(hash, bin_ptr);
|
|
|
543 goto restart; // GCC not smart enough/able to tail call an inlined function.
|
|
|
544 }
|
|
|
545 }
|
|
|
546
|
|
|
547 return t;
|
|
|
548 }
|
|
|
549
|
|
|
550 // modified from http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html
|
|
|
551 void cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpaceHashSegmentQueryFunc func, void *data)
|
|
|
552 {
|
|
|
553 a = cpvmult(a, 1.0f/hash.celldim);
|
|
|
554 b = cpvmult(b, 1.0f/hash.celldim);
|
|
|
555
|
|
|
556 int cell_x = floor_int(a.x), cell_y = floor_int(a.y);
|
|
|
557
|
|
|
558 cpFloat t = 0;
|
|
|
559
|
|
|
560 int x_inc, y_inc;
|
|
|
561 cpFloat temp_v, temp_h;
|
|
|
562
|
|
|
563 if (b.x > a.x){
|
|
|
564 x_inc = 1;
|
|
|
565 temp_h = (cpffloor(a.x + 1.0f) - a.x);
|
|
|
566 } else {
|
|
|
567 x_inc = -1;
|
|
|
568 temp_h = (a.x - cpffloor(a.x));
|
|
|
569 }
|
|
|
570
|
|
|
571 if (b.y > a.y){
|
|
|
572 y_inc = 1;
|
|
|
573 temp_v = (cpffloor(a.y + 1.0f) - a.y);
|
|
|
574 } else {
|
|
|
575 y_inc = -1;
|
|
|
576 temp_v = (a.y - cpffloor(a.y));
|
|
|
577 }
|
|
|
578
|
|
|
579 // Division by zero is *very* slow on ARM
|
|
|
580 cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y);
|
|
|
581 cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY);
|
|
|
582
|
|
|
583 // fix NANs in horizontal directions
|
|
|
584 cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx);
|
|
|
585 cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy);
|
|
|
586
|
|
|
587 cpSpaceHashBin **table = hash.table;
|
|
|
588
|
|
|
589 int n = hash.numcells;
|
|
|
590 while(t < t_exit){
|
|
|
591 int idx = hash_func(cell_x, cell_y, n);
|
|
|
592 t_exit = cpfmin(t_exit, segmentQuery(hash, &table[idx], obj, func, data));
|
|
|
593
|
|
|
594 if (next_v < next_h){
|
|
|
595 cell_y += y_inc;
|
|
|
596 t = next_v;
|
|
|
597 next_v += dt_dy;
|
|
|
598 } else {
|
|
|
599 cell_x += x_inc;
|
|
|
600 t = next_h;
|
|
|
601 next_h += dt_dx;
|
|
|
602 }
|
|
|
603 }
|
|
|
604
|
|
|
605 hash.stamp++;
|
|
|
606 }
|
