dali-physics/third-party/chipmunk2d/src/cpSpaceHash.c

   1 /* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
   2  *
   3  * Permission is hereby granted, free of charge, to any person obtaining a copy
   4  * of this software and associated documentation files (the "Software"), to deal
   5  * in the Software without restriction, including without limitation the rights
   6  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   7  * copies of the Software, and to permit persons to whom the Software is
   8  * furnished to do so, subject to the following conditions:
   9  *
  10  * The above copyright notice and this permission notice shall be included in
  11  * all copies or substantial portions of the Software.
  12  *
  13  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  14  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  15  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  16  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  17  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  18  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  19  * SOFTWARE.
  20  */
  21
  22 #include "chipmunk/chipmunk_private.h"
  23 #include "prime.h"
  24
  25 typedef struct cpSpaceHashBin cpSpaceHashBin;
  26 typedef struct cpHandle cpHandle;
  27
  28 struct cpSpaceHash {
  29         cpSpatialIndex spatialIndex;
  30
  31         int numcells;
  32         cpFloat celldim;
  33
  34         cpSpaceHashBin **table;
  35         cpHashSet *handleSet;
  36
  37         cpSpaceHashBin *pooledBins;
  38         cpArray *pooledHandles;
  39         cpArray *allocatedBuffers;
  40
  41         cpTimestamp stamp;
  42 };
  43
  44
  45 //MARK: Handle Functions
  46
  47 struct cpHandle {
  48         void *obj;
  49         int retain;
  50         cpTimestamp stamp;
  51 };
  52
  53 static cpHandle*
  54 cpHandleInit(cpHandle *hand, void *obj)
  55 {
  56         hand->obj = obj;
  57         hand->retain = 0;
  58         hand->stamp = 0;
  59
  60         return hand;
  61 }
  62
  63 static inline void cpHandleRetain(cpHandle *hand){hand->retain++;}
  64
  65 static inline void
  66 cpHandleRelease(cpHandle *hand, cpArray *pooledHandles)
  67 {
  68         hand->retain--;
  69         if(hand->retain == 0) cpArrayPush(pooledHandles, hand);
  70 }
  71
  72 static int handleSetEql(void *obj, cpHandle *hand){return (obj == hand->obj);}
  73
  74 static void *
  75 handleSetTrans(void *obj, cpSpaceHash *hash)
  76 {
  77         if(hash->pooledHandles->num == 0){
  78                 // handle pool is exhausted, make more
  79                 int count = CP_BUFFER_BYTES/sizeof(cpHandle);
  80                 cpAssertHard(count, "Internal Error: Buffer size is too small.");
  81
  82                 cpHandle *buffer = (cpHandle *)cpcalloc(1, CP_BUFFER_BYTES);
  83                 cpArrayPush(hash->allocatedBuffers, buffer);
  84
  85                 for(int i=0; i<count; i++) cpArrayPush(hash->pooledHandles, buffer + i);
  86         }
  87
  88         cpHandle *hand = cpHandleInit((cpHandle *)cpArrayPop(hash->pooledHandles), obj);
  89         cpHandleRetain(hand);
  90
  91         return hand;
  92 }
  93
  94 //MARK: Bin Functions
  95
  96 struct cpSpaceHashBin {
  97         cpHandle *handle;
  98         cpSpaceHashBin *next;
  99 };
 100
 101 static inline void
 102 recycleBin(cpSpaceHash *hash, cpSpaceHashBin *bin)
 103 {
 104         bin->next = hash->pooledBins;
 105         hash->pooledBins = bin;
 106 }
 107
 108 static inline void
 109 clearTableCell(cpSpaceHash *hash, int idx)
 110 {
 111         cpSpaceHashBin *bin = hash->table[idx];
 112         while(bin){
 113                 cpSpaceHashBin *next = bin->next;
 114
 115                 cpHandleRelease(bin->handle, hash->pooledHandles);
 116                 recycleBin(hash, bin);
 117
 118                 bin = next;
 119         }
 120
 121         hash->table[idx] = NULL;
 122 }
 123
 124 static void
 125 clearTable(cpSpaceHash *hash)
 126 {
 127         for(int i=0; i<hash->numcells; i++) clearTableCell(hash, i);
 128 }
 129
 130 // Get a recycled or new bin.
 131 static inline cpSpaceHashBin *
 132 getEmptyBin(cpSpaceHash *hash)
 133 {
 134         cpSpaceHashBin *bin = hash->pooledBins;
 135
 136         if(bin){
 137                 hash->pooledBins = bin->next;
 138                 return bin;
 139         } else {
 140                 // Pool is exhausted, make more
 141                 int count = CP_BUFFER_BYTES/sizeof(cpSpaceHashBin);
 142                 cpAssertHard(count, "Internal Error: Buffer size is too small.");
 143
 144                 cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpcalloc(1, CP_BUFFER_BYTES);
 145                 cpArrayPush(hash->allocatedBuffers, buffer);
 146
 147                 // push all but the first one, return the first instead
 148                 for(int i=1; i<count; i++) recycleBin(hash, buffer + i);
 149                 return buffer;
 150         }
 151 }
 152
 153 //MARK: Memory Management Functions
 154
 155 cpSpaceHash *
 156 cpSpaceHashAlloc(void)
 157 {
 158         return (cpSpaceHash *)cpcalloc(1, sizeof(cpSpaceHash));
 159 }
 160
 161 // Frees the old table, and allocate a new one.
 162 static void
 163 cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells)
 164 {
 165         cpfree(hash->table);
 166
 167         hash->numcells = numcells;
 168         hash->table = (cpSpaceHashBin **)cpcalloc(numcells, sizeof(cpSpaceHashBin *));
 169 }
 170
 171 static inline cpSpatialIndexClass *Klass();
 172
 173 cpSpatialIndex *
 174 cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
 175 {
 176         cpSpatialIndexInit((cpSpatialIndex *)hash, Klass(), bbfunc, staticIndex);
 177
 178         cpSpaceHashAllocTable(hash, next_prime(numcells));
 179         hash->celldim = celldim;
 180
 181         hash->handleSet = cpHashSetNew(0, (cpHashSetEqlFunc)handleSetEql);
 182
 183         hash->pooledHandles = cpArrayNew(0);
 184
 185         hash->pooledBins = NULL;
 186         hash->allocatedBuffers = cpArrayNew(0);
 187
 188         hash->stamp = 1;
 189
 190         return (cpSpatialIndex *)hash;
 191 }
 192
 193 cpSpatialIndex *
 194 cpSpaceHashNew(cpFloat celldim, int cells, cpSpatialIndexBBFunc bbfunc, cpSpatialIndex *staticIndex)
 195 {
 196         return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc, staticIndex);
 197 }
 198
 199 static void
 200 cpSpaceHashDestroy(cpSpaceHash *hash)
 201 {
 202         if(hash->table) clearTable(hash);
 203         cpfree(hash->table);
 204
 205         cpHashSetFree(hash->handleSet);
 206
 207         cpArrayFreeEach(hash->allocatedBuffers, cpfree);
 208         cpArrayFree(hash->allocatedBuffers);
 209         cpArrayFree(hash->pooledHandles);
 210 }
 211
 212 //MARK: Helper Functions
 213
 214 static inline cpBool
 215 containsHandle(cpSpaceHashBin *bin, cpHandle *hand)
 216 {
 217         while(bin){
 218                 if(bin->handle == hand) return cpTrue;
 219                 bin = bin->next;
 220         }
 221
 222         return cpFalse;
 223 }
 224
 225 // The hash function itself.
 226 static inline cpHashValue
 227 hash_func(cpHashValue x, cpHashValue y, cpHashValue n)
 228 {
 229         return (x*1640531513ul ^ y*2654435789ul) % n;
 230 }
 231
 232 // Much faster than (int)floor(f)
 233 // Profiling showed floor() to be a sizable performance hog
 234 static inline int
 235 floor_int(cpFloat f)
 236 {
 237         int i = (int)f;
 238         return (f < 0.0f && f != i ? i - 1 : i);
 239 }
 240
 241 static inline void
 242 hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb)
 243 {
 244         // Find the dimensions in cell coordinates.
 245         cpFloat dim = hash->celldim;
 246         int l = floor_int(bb.l/dim); // Fix by ShiftZ
 247         int r = floor_int(bb.r/dim);
 248         int b = floor_int(bb.b/dim);
 249         int t = floor_int(bb.t/dim);
 250
 251         int n = hash->numcells;
 252         for(int i=l; i<=r; i++){
 253                 for(int j=b; j<=t; j++){
 254                         cpHashValue idx = hash_func(i,j,n);
 255                         cpSpaceHashBin *bin = hash->table[idx];
 256
 257                         // Don't add an object twice to the same cell.
 258                         if(containsHandle(bin, hand)) continue;
 259
 260                         cpHandleRetain(hand);
 261                         // Insert a new bin for the handle in this cell.
 262                         cpSpaceHashBin *newBin = getEmptyBin(hash);
 263                         newBin->handle = hand;
 264                         newBin->next = bin;
 265                         hash->table[idx] = newBin;
 266                 }
 267         }
 268 }
 269
 270 //MARK: Basic Operations
 271
 272 static void
 273 cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid)
 274 {
 275         cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, hashid, obj, (cpHashSetTransFunc)handleSetTrans, hash);
 276         hashHandle(hash, hand, hash->spatialIndex.bbfunc(obj));
 277 }
 278
 279 static void
 280 cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid)
 281 {
 282         cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj);
 283
 284         if(hand){
 285                 hand->obj = NULL;
 286                 cpHandleRelease(hand, hash->pooledHandles);
 287
 288                 cpSpaceHashInsert(hash, obj, hashid);
 289         }
 290 }
 291
 292 static void
 293 rehash_helper(cpHandle *hand, cpSpaceHash *hash)
 294 {
 295         hashHandle(hash, hand, hash->spatialIndex.bbfunc(hand->obj));
 296 }
 297
 298 static void
 299 cpSpaceHashRehash(cpSpaceHash *hash)
 300 {
 301         clearTable(hash);
 302         cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)rehash_helper, hash);
 303 }
 304
 305 static void
 306 cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid)
 307 {
 308         cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, hashid, obj);
 309
 310         if(hand){
 311                 hand->obj = NULL;
 312                 cpHandleRelease(hand, hash->pooledHandles);
 313         }
 314 }
 315
 316 typedef struct eachContext {
 317         cpSpatialIndexIteratorFunc func;
 318         void *data;
 319 } eachContext;
 320
 321 static void eachHelper(cpHandle *hand, eachContext *context){context->func(hand->obj, context->data);}
 322
 323 static void
 324 cpSpaceHashEach(cpSpaceHash *hash, cpSpatialIndexIteratorFunc func, void *data)
 325 {
 326         eachContext context = {func, data};
 327         cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)eachHelper, &context);
 328 }
 329
 330 static void
 331 remove_orphaned_handles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr)
 332 {
 333         cpSpaceHashBin *bin = *bin_ptr;
 334         while(bin){
 335                 cpHandle *hand = bin->handle;
 336                 cpSpaceHashBin *next = bin->next;
 337
 338                 if(!hand->obj){
 339                         // orphaned handle, unlink and recycle the bin
 340                         (*bin_ptr) = bin->next;
 341                         recycleBin(hash, bin);
 342
 343                         cpHandleRelease(hand, hash->pooledHandles);
 344                 } else {
 345                         bin_ptr = &bin->next;
 346                 }
 347
 348                 bin = next;
 349         }
 350 }
 351
 352 //MARK: Query Functions
 353
 354 static inline void
 355 query_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexQueryFunc func, void *data)
 356 {
 357         restart:
 358         for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){
 359                 cpHandle *hand = bin->handle;
 360                 void *other = hand->obj;
 361
 362                 if(hand->stamp == hash->stamp || obj == other){
 363                         continue;
 364                 } else if(other){
 365                         func(obj, other, 0, data);
 366                         hand->stamp = hash->stamp;
 367                 } else {
 368                         // The object for this handle has been removed
 369                         // cleanup this cell and restart the query
 370                         remove_orphaned_handles(hash, bin_ptr);
 371                         goto restart; // GCC not smart enough/able to tail call an inlined function.
 372                 }
 373         }
 374 }
 375
 376 static void
 377 cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpatialIndexQueryFunc func, void *data)
 378 {
 379         // Get the dimensions in cell coordinates.
 380         cpFloat dim = hash->celldim;
 381         int l = floor_int(bb.l/dim);  // Fix by ShiftZ
 382         int r = floor_int(bb.r/dim);
 383         int b = floor_int(bb.b/dim);
 384         int t = floor_int(bb.t/dim);
 385
 386         int n = hash->numcells;
 387         cpSpaceHashBin **table = hash->table;
 388
 389         // Iterate over the cells and query them.
 390         for(int i=l; i<=r; i++){
 391                 for(int j=b; j<=t; j++){
 392                         query_helper(hash, &table[hash_func(i,j,n)], obj, func, data);
 393                 }
 394         }
 395
 396         hash->stamp++;
 397 }
 398
 399 // Similar to struct eachPair above.
 400 typedef struct queryRehashContext {
 401         cpSpaceHash *hash;
 402         cpSpatialIndexQueryFunc func;
 403         void *data;
 404 } queryRehashContext;
 405
 406 // Hashset iterator func used with cpSpaceHashQueryRehash().
 407 static void
 408 queryRehash_helper(cpHandle *hand, queryRehashContext *context)
 409 {
 410         cpSpaceHash *hash = context->hash;
 411         cpSpatialIndexQueryFunc func = context->func;
 412         void *data = context->data;
 413
 414         cpFloat dim = hash->celldim;
 415         int n = hash->numcells;
 416
 417         void *obj = hand->obj;
 418         cpBB bb = hash->spatialIndex.bbfunc(obj);
 419
 420         int l = floor_int(bb.l/dim);
 421         int r = floor_int(bb.r/dim);
 422         int b = floor_int(bb.b/dim);
 423         int t = floor_int(bb.t/dim);
 424
 425         cpSpaceHashBin **table = hash->table;
 426
 427         for(int i=l; i<=r; i++){
 428                 for(int j=b; j<=t; j++){
 429                         cpHashValue idx = hash_func(i,j,n);
 430                         cpSpaceHashBin *bin = table[idx];
 431
 432                         if(containsHandle(bin, hand)) continue;
 433
 434                         cpHandleRetain(hand); // this MUST be done first in case the object is removed in func()
 435                         query_helper(hash, &bin, obj, func, data);
 436
 437                         cpSpaceHashBin *newBin = getEmptyBin(hash);
 438                         newBin->handle = hand;
 439                         newBin->next = bin;
 440                         table[idx] = newBin;
 441                 }
 442         }
 443
 444         // Increment the stamp for each object hashed.
 445         hash->stamp++;
 446 }
 447
 448 static void
 449 cpSpaceHashReindexQuery(cpSpaceHash *hash, cpSpatialIndexQueryFunc func, void *data)
 450 {
 451         clearTable(hash);
 452
 453         queryRehashContext context = {hash, func, data};
 454         cpHashSetEach(hash->handleSet, (cpHashSetIteratorFunc)queryRehash_helper, &context);
 455
 456         cpSpatialIndexCollideStatic((cpSpatialIndex *)hash, hash->spatialIndex.staticIndex, func, data);
 457 }
 458
 459 static inline cpFloat
 460 segmentQuery_helper(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpatialIndexSegmentQueryFunc func, void *data)
 461 {
 462         cpFloat t = 1.0f;
 463
 464         restart:
 465         for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin->next){
 466                 cpHandle *hand = bin->handle;
 467                 void *other = hand->obj;
 468
 469                 // Skip over certain conditions
 470                 if(hand->stamp == hash->stamp){
 471                         continue;
 472                 } else if(other){
 473                         t = cpfmin(t, func(obj, other, data));
 474                         hand->stamp = hash->stamp;
 475                 } else {
 476                         // The object for this handle has been removed
 477                         // cleanup this cell and restart the query
 478                         remove_orphaned_handles(hash, bin_ptr);
 479                         goto restart; // GCC not smart enough/able to tail call an inlined function.
 480                 }
 481         }
 482
 483         return t;
 484 }
 485
 486 // modified from http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html
 487 static void
 488 cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data)
 489 {
 490         a = cpvmult(a, 1.0f/hash->celldim);
 491         b = cpvmult(b, 1.0f/hash->celldim);
 492
 493         int cell_x = floor_int(a.x), cell_y = floor_int(a.y);
 494
 495         cpFloat t = 0;
 496
 497         int x_inc, y_inc;
 498         cpFloat temp_v, temp_h;
 499
 500         if (b.x > a.x){
 501                 x_inc = 1;
 502                 temp_h = (cpffloor(a.x + 1.0f) - a.x);
 503         } else {
 504                 x_inc = -1;
 505                 temp_h = (a.x - cpffloor(a.x));
 506         }
 507
 508         if (b.y > a.y){
 509                 y_inc = 1;
 510                 temp_v = (cpffloor(a.y + 1.0f) - a.y);
 511         } else {
 512                 y_inc = -1;
 513                 temp_v = (a.y - cpffloor(a.y));
 514         }
 515
 516         // Division by zero is *very* slow on ARM
 517         cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y);
 518         cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY);
 519
 520         // fix NANs in horizontal directions
 521         cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx);
 522         cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy);
 523
 524         int n = hash->numcells;
 525         cpSpaceHashBin **table = hash->table;
 526
 527         while(t < t_exit){
 528                 cpHashValue idx = hash_func(cell_x, cell_y, n);
 529                 t_exit = cpfmin(t_exit, segmentQuery_helper(hash, &table[idx], obj, func, data));
 530
 531                 if (next_v < next_h){
 532                         cell_y += y_inc;
 533                         t = next_v;
 534                         next_v += dt_dy;
 535                 } else {
 536                         cell_x += x_inc;
 537                         t = next_h;
 538                         next_h += dt_dx;
 539                 }
 540         }
 541
 542         hash->stamp++;
 543 }
 544
 545 //MARK: Misc
 546
 547 void
 548 cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells)
 549 {
 550         if(hash->spatialIndex.klass != Klass()){
 551                 cpAssertWarn(cpFalse, "Ignoring cpSpaceHashResize() call to non-cpSpaceHash spatial index.");
 552                 return;
 553         }
 554
 555         clearTable(hash);
 556
 557         hash->celldim = celldim;
 558         cpSpaceHashAllocTable(hash, next_prime(numcells));
 559 }
 560
 561 static int
 562 cpSpaceHashCount(cpSpaceHash *hash)
 563 {
 564         return cpHashSetCount(hash->handleSet);
 565 }
 566
 567 static int
 568 cpSpaceHashContains(cpSpaceHash *hash, void *obj, cpHashValue hashid)
 569 {
 570         return cpHashSetFind(hash->handleSet, hashid, obj) != NULL;
 571 }
 572
 573 static cpSpatialIndexClass klass = {
 574         (cpSpatialIndexDestroyImpl)cpSpaceHashDestroy,
 575
 576         (cpSpatialIndexCountImpl)cpSpaceHashCount,
 577         (cpSpatialIndexEachImpl)cpSpaceHashEach,
 578         (cpSpatialIndexContainsImpl)cpSpaceHashContains,
 579
 580         (cpSpatialIndexInsertImpl)cpSpaceHashInsert,
 581         (cpSpatialIndexRemoveImpl)cpSpaceHashRemove,
 582
 583         (cpSpatialIndexReindexImpl)cpSpaceHashRehash,
 584         (cpSpatialIndexReindexObjectImpl)cpSpaceHashRehashObject,
 585         (cpSpatialIndexReindexQueryImpl)cpSpaceHashReindexQuery,
 586
 587         (cpSpatialIndexQueryImpl)cpSpaceHashQuery,
 588         (cpSpatialIndexSegmentQueryImpl)cpSpaceHashSegmentQuery,
 589 };
 590
 591 static inline cpSpatialIndexClass *Klass(){return &klass;}
 592
 593 //MARK: Debug Drawing
 594
 595 //#define CP_BBTREE_DEBUG_DRAW
 596 #ifdef CP_BBTREE_DEBUG_DRAW
 597 #include "OpenGL/gl.h"
 598 #include "OpenGL/glu.h"
 599 #include <GLUT/glut.h>
 600
 601 void
 602 cpSpaceHashRenderDebug(cpSpatialIndex *index)
 603 {
 604         if(index->klass != &klass){
 605                 cpAssertWarn(cpFalse, "Ignoring cpSpaceHashRenderDebug() call to non-spatial hash spatial index.");
 606                 return;
 607         }
 608
 609         cpSpaceHash *hash = (cpSpaceHash *)index;
 610         cpBB bb = cpBBNew(-320, -240, 320, 240);
 611
 612         cpFloat dim = hash->celldim;
 613         int n = hash->numcells;
 614
 615         int l = (int)floor(bb.l/dim);
 616         int r = (int)floor(bb.r/dim);
 617         int b = (int)floor(bb.b/dim);
 618         int t = (int)floor(bb.t/dim);
 619
 620         for(int i=l; i<=r; i++){
 621                 for(int j=b; j<=t; j++){
 622                         int cell_count = 0;
 623
 624                         int index = hash_func(i,j,n);
 625                         for(cpSpaceHashBin *bin = hash->table[index]; bin; bin = bin->next)
 626                                 cell_count++;
 627
 628                         GLfloat v = 1.0f - (GLfloat)cell_count/10.0f;
 629                         glColor3f(v,v,v);
 630                         glRectf(i*dim, j*dim, (i + 1)*dim, (j + 1)*dim);
 631                 }
 632         }
 633 }
 634 #endif