Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/cpSpaceHash.d @ 4:7ebbd4d05553

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
initial commit
author Extrawurst
date Thu, 02 Dec 2010 02:11:26 +0100
parents
children df4ebc8add66
comparison
equal deleted inserted replaced
3:81145a61c2fe 4:7ebbd4d05553
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 }