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