dali-physics/third-party/chipmunk2d/src/cpSpace.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 <stdio.h>
  23 #include <string.h>
  24
  25 #include "chipmunk/chipmunk_private.h"
  26
  27 //MARK: Contact Set Helpers
  28
  29 // Equal function for arbiterSet.
  30 static cpBool
  31 arbiterSetEql(cpShape **shapes, cpArbiter *arb)
  32 {
  33         cpShape *a = shapes[0];
  34         cpShape *b = shapes[1];
  35
  36         return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b));
  37 }
  38
  39 //MARK: Collision Handler Set HelperFunctions
  40
  41 // Equals function for collisionHandlers.
  42 static cpBool
  43 handlerSetEql(cpCollisionHandler *check, cpCollisionHandler *pair)
  44 {
  45         return (
  46                 (check->typeA == pair->typeA && check->typeB == pair->typeB) ||
  47                 (check->typeB == pair->typeA && check->typeA == pair->typeB)
  48         );
  49 }
  50
  51 // Transformation function for collisionHandlers.
  52 static void *
  53 handlerSetTrans(cpCollisionHandler *handler, void *unused)
  54 {
  55         cpCollisionHandler *copy = (cpCollisionHandler *)cpcalloc(1, sizeof(cpCollisionHandler));
  56         memcpy(copy, handler, sizeof(cpCollisionHandler));
  57
  58         return copy;
  59 }
  60
  61 //MARK: Misc Helper Funcs
  62
  63 // Default collision functions.
  64
  65 static cpBool
  66 DefaultBegin(cpArbiter *arb, cpSpace *space, void *data){
  67         cpBool retA = cpArbiterCallWildcardBeginA(arb, space);
  68         cpBool retB = cpArbiterCallWildcardBeginB(arb, space);
  69         return retA && retB;
  70 }
  71
  72 static cpBool
  73 DefaultPreSolve(cpArbiter *arb, cpSpace *space, void *data){
  74         cpBool retA = cpArbiterCallWildcardPreSolveA(arb, space);
  75         cpBool retB = cpArbiterCallWildcardPreSolveB(arb, space);
  76         return retA && retB;
  77 }
  78
  79 static void
  80 DefaultPostSolve(cpArbiter *arb, cpSpace *space, void *data){
  81         cpArbiterCallWildcardPostSolveA(arb, space);
  82         cpArbiterCallWildcardPostSolveB(arb, space);
  83 }
  84
  85 static void
  86 DefaultSeparate(cpArbiter *arb, cpSpace *space, void *data){
  87         cpArbiterCallWildcardSeparateA(arb, space);
  88         cpArbiterCallWildcardSeparateB(arb, space);
  89 }
  90
  91 // Use the wildcard identifier since  the default handler should never match any type pair.
  92 static cpCollisionHandler cpCollisionHandlerDefault = {
  93         CP_WILDCARD_COLLISION_TYPE, CP_WILDCARD_COLLISION_TYPE,
  94         DefaultBegin, DefaultPreSolve, DefaultPostSolve, DefaultSeparate, NULL
  95 };
  96
  97 static cpBool AlwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return cpTrue;}
  98 static void DoNothing(cpArbiter *arb, cpSpace *space, void *data){}
  99
 100 cpCollisionHandler cpCollisionHandlerDoNothing = {
 101         CP_WILDCARD_COLLISION_TYPE, CP_WILDCARD_COLLISION_TYPE,
 102         AlwaysCollide, AlwaysCollide, DoNothing, DoNothing, NULL
 103 };
 104
 105 // function to get the estimated velocity of a shape for the cpBBTree.
 106 static cpVect ShapeVelocityFunc(cpShape *shape){return shape->body->v;}
 107
 108 // Used for disposing of collision handlers.
 109 static void FreeWrap(void *ptr, void *unused){cpfree(ptr);}
 110
 111 //MARK: Memory Management Functions
 112
 113 cpSpace *
 114 cpSpaceAlloc(void)
 115 {
 116         return (cpSpace *)cpcalloc(1, sizeof(cpSpace));
 117 }
 118
 119 cpSpace*
 120 cpSpaceInit(cpSpace *space)
 121 {
 122 #ifndef NDEBUG
 123         static cpBool done = cpFalse;
 124         if(!done){
 125                 printf("Initializing cpSpace - Chipmunk v%s (Debug Enabled)\n", cpVersionString);
 126                 printf("Compile with -DNDEBUG defined to disable debug mode and runtime assertion checks\n");
 127                 done = cpTrue;
 128         }
 129 #endif
 130
 131         space->iterations = 10;
 132
 133         space->gravity = cpvzero;
 134         space->damping = 1.0f;
 135
 136         space->collisionSlop = 0.1f;
 137         space->collisionBias = cpfpow(1.0f - 0.1f, 60.0f);
 138         space->collisionPersistence = 3;
 139
 140         space->locked = 0;
 141         space->stamp = 0;
 142
 143         space->shapeIDCounter = 0;
 144         space->staticShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, NULL);
 145         space->dynamicShapes = cpBBTreeNew((cpSpatialIndexBBFunc)cpShapeGetBB, space->staticShapes);
 146         cpBBTreeSetVelocityFunc(space->dynamicShapes, (cpBBTreeVelocityFunc)ShapeVelocityFunc);
 147
 148         space->allocatedBuffers = cpArrayNew(0);
 149
 150         space->dynamicBodies = cpArrayNew(0);
 151         space->staticBodies = cpArrayNew(0);
 152         space->sleepingComponents = cpArrayNew(0);
 153         space->rousedBodies = cpArrayNew(0);
 154
 155         space->sleepTimeThreshold = INFINITY;
 156         space->idleSpeedThreshold = 0.0f;
 157
 158         space->arbiters = cpArrayNew(0);
 159         space->pooledArbiters = cpArrayNew(0);
 160
 161         space->contactBuffersHead = NULL;
 162         space->cachedArbiters = cpHashSetNew(0, (cpHashSetEqlFunc)arbiterSetEql);
 163
 164         space->constraints = cpArrayNew(0);
 165
 166         space->usesWildcards = cpFalse;
 167         memcpy(&space->defaultHandler, &cpCollisionHandlerDoNothing, sizeof(cpCollisionHandler));
 168         space->collisionHandlers = cpHashSetNew(0, (cpHashSetEqlFunc)handlerSetEql);
 169
 170         space->postStepCallbacks = cpArrayNew(0);
 171         space->skipPostStep = cpFalse;
 172
 173         cpBody *staticBody = cpBodyInit(&space->_staticBody, 0.0f, 0.0f);
 174         cpBodySetType(staticBody, CP_BODY_TYPE_STATIC);
 175         cpSpaceSetStaticBody(space, staticBody);
 176
 177         return space;
 178 }
 179
 180 cpSpace*
 181 cpSpaceNew(void)
 182 {
 183         return cpSpaceInit(cpSpaceAlloc());
 184 }
 185
 186 static void cpBodyActivateWrap(cpBody *body, void *unused){cpBodyActivate(body);}
 187
 188 void
 189 cpSpaceDestroy(cpSpace *space)
 190 {
 191         cpSpaceEachBody(space, (cpSpaceBodyIteratorFunc)cpBodyActivateWrap, NULL);
 192
 193         cpSpatialIndexFree(space->staticShapes);
 194         cpSpatialIndexFree(space->dynamicShapes);
 195
 196         cpArrayFree(space->dynamicBodies);
 197         cpArrayFree(space->staticBodies);
 198         cpArrayFree(space->sleepingComponents);
 199         cpArrayFree(space->rousedBodies);
 200
 201         cpArrayFree(space->constraints);
 202
 203         cpHashSetFree(space->cachedArbiters);
 204
 205         cpArrayFree(space->arbiters);
 206         cpArrayFree(space->pooledArbiters);
 207
 208         if(space->allocatedBuffers){
 209                 cpArrayFreeEach(space->allocatedBuffers, cpfree);
 210                 cpArrayFree(space->allocatedBuffers);
 211         }
 212
 213         if(space->postStepCallbacks){
 214                 cpArrayFreeEach(space->postStepCallbacks, cpfree);
 215                 cpArrayFree(space->postStepCallbacks);
 216         }
 217
 218         if(space->collisionHandlers) cpHashSetEach(space->collisionHandlers, FreeWrap, NULL);
 219         cpHashSetFree(space->collisionHandlers);
 220 }
 221
 222 void
 223 cpSpaceFree(cpSpace *space)
 224 {
 225         if(space){
 226                 cpSpaceDestroy(space);
 227                 cpfree(space);
 228         }
 229 }
 230
 231
 232 //MARK: Basic properties:
 233
 234 int
 235 cpSpaceGetIterations(const cpSpace *space)
 236 {
 237         return space->iterations;
 238 }
 239
 240 void
 241 cpSpaceSetIterations(cpSpace *space, int iterations)
 242 {
 243         cpAssertHard(iterations > 0, "Iterations must be positive and non-zero.");
 244         space->iterations = iterations;
 245 }
 246
 247 cpVect
 248 cpSpaceGetGravity(const cpSpace *space)
 249 {
 250         return space->gravity;
 251 }
 252
 253 void
 254 cpSpaceSetGravity(cpSpace *space, cpVect gravity)
 255 {
 256         space->gravity = gravity;
 257
 258         // Wake up all of the bodies since the gravity changed.
 259         cpArray *components = space->sleepingComponents;
 260         for(int i=0; i<components->num; i++){
 261                 cpBodyActivate((cpBody *)components->arr[i]);
 262         }
 263 }
 264
 265 cpFloat
 266 cpSpaceGetDamping(const cpSpace *space)
 267 {
 268         return space->damping;
 269 }
 270
 271 void
 272 cpSpaceSetDamping(cpSpace *space, cpFloat damping)
 273 {
 274         cpAssertHard(damping >= 0.0, "Damping must be positive.");
 275         space->damping = damping;
 276 }
 277
 278 cpFloat
 279 cpSpaceGetIdleSpeedThreshold(const cpSpace *space)
 280 {
 281         return space->idleSpeedThreshold;
 282 }
 283
 284 void
 285 cpSpaceSetIdleSpeedThreshold(cpSpace *space, cpFloat idleSpeedThreshold)
 286 {
 287         space->idleSpeedThreshold = idleSpeedThreshold;
 288 }
 289
 290 cpFloat
 291 cpSpaceGetSleepTimeThreshold(const cpSpace *space)
 292 {
 293         return space->sleepTimeThreshold;
 294 }
 295
 296 void
 297 cpSpaceSetSleepTimeThreshold(cpSpace *space, cpFloat sleepTimeThreshold)
 298 {
 299         space->sleepTimeThreshold = sleepTimeThreshold;
 300 }
 301
 302 cpFloat
 303 cpSpaceGetCollisionSlop(const cpSpace *space)
 304 {
 305         return space->collisionSlop;
 306 }
 307
 308 void
 309 cpSpaceSetCollisionSlop(cpSpace *space, cpFloat collisionSlop)
 310 {
 311         space->collisionSlop = collisionSlop;
 312 }
 313
 314 cpFloat
 315 cpSpaceGetCollisionBias(const cpSpace *space)
 316 {
 317         return space->collisionBias;
 318 }
 319
 320 void
 321 cpSpaceSetCollisionBias(cpSpace *space, cpFloat collisionBias)
 322 {
 323         space->collisionBias = collisionBias;
 324 }
 325
 326 cpTimestamp
 327 cpSpaceGetCollisionPersistence(const cpSpace *space)
 328 {
 329         return space->collisionPersistence;
 330 }
 331
 332 void
 333 cpSpaceSetCollisionPersistence(cpSpace *space, cpTimestamp collisionPersistence)
 334 {
 335         space->collisionPersistence = collisionPersistence;
 336 }
 337
 338 cpDataPointer
 339 cpSpaceGetUserData(const cpSpace *space)
 340 {
 341         return space->userData;
 342 }
 343
 344 void
 345 cpSpaceSetUserData(cpSpace *space, cpDataPointer userData)
 346 {
 347         space->userData = userData;
 348 }
 349
 350 cpBody *
 351 cpSpaceGetStaticBody(const cpSpace *space)
 352 {
 353         return space->staticBody;
 354 }
 355
 356 cpFloat
 357 cpSpaceGetCurrentTimeStep(const cpSpace *space)
 358 {
 359         return space->curr_dt;
 360 }
 361
 362 void
 363 cpSpaceSetStaticBody(cpSpace *space, cpBody *body)
 364 {
 365         if(space->staticBody != NULL){
 366                 cpAssertHard(space->staticBody->shapeList == NULL, "Internal Error: Changing the designated static body while the old one still had shapes attached.");
 367                 space->staticBody->space = NULL;
 368         }
 369
 370         space->staticBody = body;
 371         body->space = space;
 372 }
 373
 374 cpBool
 375 cpSpaceIsLocked(cpSpace *space)
 376 {
 377         return (space->locked > 0);
 378 }
 379
 380 //MARK: Collision Handler Function Management
 381
 382 static void
 383 cpSpaceUseWildcardDefaultHandler(cpSpace *space)
 384 {
 385         // Spaces default to using the slightly faster "do nothing" default handler until wildcards are potentially needed.
 386         if(!space->usesWildcards){
 387                 space->usesWildcards = cpTrue;
 388                 memcpy(&space->defaultHandler, &cpCollisionHandlerDefault, sizeof(cpCollisionHandler));
 389         }
 390 }
 391
 392 cpCollisionHandler *cpSpaceAddDefaultCollisionHandler(cpSpace *space)
 393 {
 394         cpSpaceUseWildcardDefaultHandler(space);
 395         return &space->defaultHandler;
 396 }
 397
 398 cpCollisionHandler *cpSpaceAddCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b)
 399 {
 400         cpHashValue hash = CP_HASH_PAIR(a, b);
 401         cpCollisionHandler handler = {a, b, DefaultBegin, DefaultPreSolve, DefaultPostSolve, DefaultSeparate, NULL};
 402         return (cpCollisionHandler*)cpHashSetInsert(space->collisionHandlers, hash, &handler, (cpHashSetTransFunc)handlerSetTrans, NULL);
 403 }
 404
 405 cpCollisionHandler *
 406 cpSpaceAddWildcardHandler(cpSpace *space, cpCollisionType type)
 407 {
 408         cpSpaceUseWildcardDefaultHandler(space);
 409
 410         cpHashValue hash = CP_HASH_PAIR(type, CP_WILDCARD_COLLISION_TYPE);
 411         cpCollisionHandler handler = {type, CP_WILDCARD_COLLISION_TYPE, AlwaysCollide, AlwaysCollide, DoNothing, DoNothing, NULL};
 412         return (cpCollisionHandler*)cpHashSetInsert(space->collisionHandlers, hash, &handler, (cpHashSetTransFunc)handlerSetTrans, NULL);
 413 }
 414
 415
 416 //MARK: Body, Shape, and Joint Management
 417 cpShape *
 418 cpSpaceAddShape(cpSpace *space, cpShape *shape)
 419 {
 420         cpBody *body = shape->body;
 421
 422         cpAssertHard(shape->space != space, "You have already added this shape to this space. You must not add it a second time.");
 423         cpAssertHard(!shape->space, "You have already added this shape to another space. You cannot add it to a second.");
 424 //      cpAssertHard(body->space == space, "The shape's body must be added to the space before the shape.");
 425         cpAssertSpaceUnlocked(space);
 426
 427         cpBool isStatic = (cpBodyGetType(body) == CP_BODY_TYPE_STATIC);
 428         if(!isStatic) cpBodyActivate(body);
 429         cpBodyAddShape(body, shape);
 430
 431         shape->hashid = space->shapeIDCounter++;
 432         cpShapeUpdate(shape, body->transform);
 433         cpSpatialIndexInsert(isStatic ? space->staticShapes : space->dynamicShapes, shape, shape->hashid);
 434         shape->space = space;
 435
 436         return shape;
 437 }
 438
 439 cpBody *
 440 cpSpaceAddBody(cpSpace *space, cpBody *body)
 441 {
 442         cpAssertHard(body->space != space, "You have already added this body to this space. You must not add it a second time.");
 443         cpAssertHard(!body->space, "You have already added this body to another space. You cannot add it to a second.");
 444         cpAssertSpaceUnlocked(space);
 445
 446         cpArrayPush(cpSpaceArrayForBodyType(space, cpBodyGetType(body)), body);
 447         body->space = space;
 448
 449         return body;
 450 }
 451
 452 cpConstraint *
 453 cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
 454 {
 455         cpAssertHard(constraint->space != space, "You have already added this constraint to this space. You must not add it a second time.");
 456         cpAssertHard(!constraint->space, "You have already added this constraint to another space. You cannot add it to a second.");
 457         cpAssertSpaceUnlocked(space);
 458
 459         cpBody *a = constraint->a, *b = constraint->b;
 460         cpAssertHard(a != NULL && b != NULL, "Constraint is attached to a NULL body.");
 461 //      cpAssertHard(a->space == space && b->space == space, "The constraint's bodies must be added to the space before the constraint.");
 462
 463         cpBodyActivate(a);
 464         cpBodyActivate(b);
 465         cpArrayPush(space->constraints, constraint);
 466
 467         // Push onto the heads of the bodies' constraint lists
 468         constraint->next_a = a->constraintList; a->constraintList = constraint;
 469         constraint->next_b = b->constraintList; b->constraintList = constraint;
 470         constraint->space = space;
 471
 472         return constraint;
 473 }
 474
 475 struct arbiterFilterContext {
 476         cpSpace *space;
 477         cpBody *body;
 478         cpShape *shape;
 479 };
 480
 481 static cpBool
 482 cachedArbitersFilter(cpArbiter *arb, struct arbiterFilterContext *context)
 483 {
 484         cpShape *shape = context->shape;
 485         cpBody *body = context->body;
 486
 487
 488         // Match on the filter shape, or if it's NULL the filter body
 489         if(
 490                 (body == arb->body_a && (shape == arb->a || shape == NULL)) ||
 491                 (body == arb->body_b && (shape == arb->b || shape == NULL))
 492         ){
 493                 // Call separate when removing shapes.
 494                 if(shape && arb->state != CP_ARBITER_STATE_CACHED){
 495                         // Invalidate the arbiter since one of the shapes was removed.
 496                         arb->state = CP_ARBITER_STATE_INVALIDATED;
 497
 498                         cpCollisionHandler *handler = arb->handler;
 499                         handler->separateFunc(arb, context->space, handler->userData);
 500                 }
 501
 502                 cpArbiterUnthread(arb);
 503                 cpArrayDeleteObj(context->space->arbiters, arb);
 504                 cpArrayPush(context->space->pooledArbiters, arb);
 505
 506                 return cpFalse;
 507         }
 508
 509         return cpTrue;
 510 }
 511
 512 void
 513 cpSpaceFilterArbiters(cpSpace *space, cpBody *body, cpShape *filter)
 514 {
 515         cpSpaceLock(space); {
 516                 struct arbiterFilterContext context = {space, body, filter};
 517                 cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cachedArbitersFilter, &context);
 518         } cpSpaceUnlock(space, cpTrue);
 519 }
 520
 521 void
 522 cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
 523 {
 524         cpBody *body = shape->body;
 525         cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a shape that was not added to the space. (Removed twice maybe?)");
 526         cpAssertSpaceUnlocked(space);
 527
 528         cpBool isStatic = (cpBodyGetType(body) == CP_BODY_TYPE_STATIC);
 529         if(isStatic){
 530                 cpBodyActivateStatic(body, shape);
 531         } else {
 532                 cpBodyActivate(body);
 533         }
 534
 535         cpBodyRemoveShape(body, shape);
 536         cpSpaceFilterArbiters(space, body, shape);
 537         cpSpatialIndexRemove(isStatic ? space->staticShapes : space->dynamicShapes, shape, shape->hashid);
 538         shape->space = NULL;
 539         shape->hashid = 0;
 540 }
 541
 542 void
 543 cpSpaceRemoveBody(cpSpace *space, cpBody *body)
 544 {
 545         cpAssertHard(body != cpSpaceGetStaticBody(space), "Cannot remove the designated static body for the space.");
 546         cpAssertHard(cpSpaceContainsBody(space, body), "Cannot remove a body that was not added to the space. (Removed twice maybe?)");
 547 //      cpAssertHard(body->shapeList == NULL, "Cannot remove a body from the space before removing the bodies attached to it.");
 548 //      cpAssertHard(body->constraintList == NULL, "Cannot remove a body from the space before removing the constraints attached to it.");
 549         cpAssertSpaceUnlocked(space);
 550
 551         cpBodyActivate(body);
 552 //      cpSpaceFilterArbiters(space, body, NULL);
 553         cpArrayDeleteObj(cpSpaceArrayForBodyType(space, cpBodyGetType(body)), body);
 554         body->space = NULL;
 555 }
 556
 557 void
 558 cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
 559 {
 560         cpAssertHard(cpSpaceContainsConstraint(space, constraint), "Cannot remove a constraint that was not added to the space. (Removed twice maybe?)");
 561         cpAssertSpaceUnlocked(space);
 562
 563         cpBodyActivate(constraint->a);
 564         cpBodyActivate(constraint->b);
 565         cpArrayDeleteObj(space->constraints, constraint);
 566
 567         cpBodyRemoveConstraint(constraint->a, constraint);
 568         cpBodyRemoveConstraint(constraint->b, constraint);
 569         constraint->space = NULL;
 570 }
 571
 572 cpBool cpSpaceContainsShape(cpSpace *space, cpShape *shape)
 573 {
 574         return (shape->space == space);
 575 }
 576
 577 cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body)
 578 {
 579         return (body->space == space);
 580 }
 581
 582 cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint)
 583 {
 584         return (constraint->space == space);
 585 }
 586
 587 //MARK: Iteration
 588
 589 void
 590 cpSpaceEachBody(cpSpace *space, cpSpaceBodyIteratorFunc func, void *data)
 591 {
 592         cpSpaceLock(space); {
 593                 cpArray *bodies = space->dynamicBodies;
 594                 for(int i=0; i<bodies->num; i++){
 595                         func((cpBody *)bodies->arr[i], data);
 596                 }
 597
 598                 cpArray *otherBodies = space->staticBodies;
 599                 for(int i=0; i<otherBodies->num; i++){
 600                         func((cpBody *)otherBodies->arr[i], data);
 601                 }
 602
 603                 cpArray *components = space->sleepingComponents;
 604                 for(int i=0; i<components->num; i++){
 605                         cpBody *root = (cpBody *)components->arr[i];
 606
 607                         cpBody *body = root;
 608                         while(body){
 609                                 cpBody *next = body->sleeping.next;
 610                                 func(body, data);
 611                                 body = next;
 612                         }
 613                 }
 614         } cpSpaceUnlock(space, cpTrue);
 615 }
 616
 617 typedef struct spaceShapeContext {
 618         cpSpaceShapeIteratorFunc func;
 619         void *data;
 620 } spaceShapeContext;
 621
 622 static void
 623 spaceEachShapeIterator(cpShape *shape, spaceShapeContext *context)
 624 {
 625         context->func(shape, context->data);
 626 }
 627
 628 void
 629 cpSpaceEachShape(cpSpace *space, cpSpaceShapeIteratorFunc func, void *data)
 630 {
 631         cpSpaceLock(space); {
 632                 spaceShapeContext context = {func, data};
 633                 cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context);
 634                 cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)spaceEachShapeIterator, &context);
 635         } cpSpaceUnlock(space, cpTrue);
 636 }
 637
 638 void
 639 cpSpaceEachConstraint(cpSpace *space, cpSpaceConstraintIteratorFunc func, void *data)
 640 {
 641         cpSpaceLock(space); {
 642                 cpArray *constraints = space->constraints;
 643
 644                 for(int i=0; i<constraints->num; i++){
 645                         func((cpConstraint *)constraints->arr[i], data);
 646                 }
 647         } cpSpaceUnlock(space, cpTrue);
 648 }
 649
 650 //MARK: Spatial Index Management
 651
 652 void
 653 cpSpaceReindexStatic(cpSpace *space)
 654 {
 655         cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete.");
 656
 657         cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)&cpShapeUpdateFunc, NULL);
 658         cpSpatialIndexReindex(space->staticShapes);
 659 }
 660
 661 void
 662 cpSpaceReindexShape(cpSpace *space, cpShape *shape)
 663 {
 664         cpAssertHard(!space->locked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete.");
 665
 666         cpShapeCacheBB(shape);
 667
 668         // attempt to rehash the shape in both hashes
 669         cpSpatialIndexReindexObject(space->dynamicShapes, shape, shape->hashid);
 670         cpSpatialIndexReindexObject(space->staticShapes, shape, shape->hashid);
 671 }
 672
 673 void
 674 cpSpaceReindexShapesForBody(cpSpace *space, cpBody *body)
 675 {
 676         CP_BODY_FOREACH_SHAPE(body, shape) cpSpaceReindexShape(space, shape);
 677 }
 678
 679
 680 static void
 681 copyShapes(cpShape *shape, cpSpatialIndex *index)
 682 {
 683         cpSpatialIndexInsert(index, shape, shape->hashid);
 684 }
 685
 686 void
 687 cpSpaceUseSpatialHash(cpSpace *space, cpFloat dim, int count)
 688 {
 689         cpSpatialIndex *staticShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, NULL);
 690         cpSpatialIndex *dynamicShapes = cpSpaceHashNew(dim, count, (cpSpatialIndexBBFunc)cpShapeGetBB, staticShapes);
 691
 692         cpSpatialIndexEach(space->staticShapes, (cpSpatialIndexIteratorFunc)copyShapes, staticShapes);
 693         cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)copyShapes, dynamicShapes);
 694
 695         cpSpatialIndexFree(space->staticShapes);
 696         cpSpatialIndexFree(space->dynamicShapes);
 697
 698         space->staticShapes = staticShapes;
 699         space->dynamicShapes = dynamicShapes;
 700 }