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
arbiterSetEql(cpShape ** shapes,cpArbiter * arb)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
handlerSetEql(cpCollisionHandler * check,cpCollisionHandler * pair)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 *
handlerSetTrans(cpCollisionHandler * handler,void * unused)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
DefaultBegin(cpArbiter * arb,cpSpace * space,void * data)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
DefaultPreSolve(cpArbiter * arb,cpSpace * space,void * data)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
DefaultPostSolve(cpArbiter * arb,cpSpace * space,void * data)80 DefaultPostSolve(cpArbiter *arb, cpSpace *space, void *data){
81 cpArbiterCallWildcardPostSolveA(arb, space);
82 cpArbiterCallWildcardPostSolveB(arb, space);
83 }
84
85 static void
DefaultSeparate(cpArbiter * arb,cpSpace * space,void * data)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
AlwaysCollide(cpArbiter * arb,cpSpace * space,void * data)97 static cpBool AlwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return cpTrue;}
DoNothing(cpArbiter * arb,cpSpace * space,void * data)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.
ShapeVelocityFunc(cpShape * shape)106 static cpVect ShapeVelocityFunc(cpShape *shape){return shape->body->v;}
107
108 // Used for disposing of collision handlers.
FreeWrap(void * ptr,void * unused)109 static void FreeWrap(void *ptr, void *unused){cpfree(ptr);}
110
111 //MARK: Memory Management Functions
112
113 cpSpace *
cpSpaceAlloc(void)114 cpSpaceAlloc(void)
115 {
116 return (cpSpace *)cpcalloc(1, sizeof(cpSpace));
117 }
118
119 cpSpace*
cpSpaceInit(cpSpace * space)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*
cpSpaceNew(void)181 cpSpaceNew(void)
182 {
183 return cpSpaceInit(cpSpaceAlloc());
184 }
185
cpBodyActivateWrap(cpBody * body,void * unused)186 static void cpBodyActivateWrap(cpBody *body, void *unused){cpBodyActivate(body);}
187
188 void
cpSpaceDestroy(cpSpace * space)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
cpSpaceFree(cpSpace * space)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
cpSpaceGetIterations(const cpSpace * space)235 cpSpaceGetIterations(const cpSpace *space)
236 {
237 return space->iterations;
238 }
239
240 void
cpSpaceSetIterations(cpSpace * space,int iterations)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
cpSpaceGetGravity(const cpSpace * space)248 cpSpaceGetGravity(const cpSpace *space)
249 {
250 return space->gravity;
251 }
252
253 void
cpSpaceSetGravity(cpSpace * space,cpVect gravity)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
cpSpaceGetDamping(const cpSpace * space)266 cpSpaceGetDamping(const cpSpace *space)
267 {
268 return space->damping;
269 }
270
271 void
cpSpaceSetDamping(cpSpace * space,cpFloat damping)272 cpSpaceSetDamping(cpSpace *space, cpFloat damping)
273 {
274 cpAssertHard(damping >= 0.0, "Damping must be positive.");
275 space->damping = damping;
276 }
277
278 cpFloat
cpSpaceGetIdleSpeedThreshold(const cpSpace * space)279 cpSpaceGetIdleSpeedThreshold(const cpSpace *space)
280 {
281 return space->idleSpeedThreshold;
282 }
283
284 void
cpSpaceSetIdleSpeedThreshold(cpSpace * space,cpFloat idleSpeedThreshold)285 cpSpaceSetIdleSpeedThreshold(cpSpace *space, cpFloat idleSpeedThreshold)
286 {
287 space->idleSpeedThreshold = idleSpeedThreshold;
288 }
289
290 cpFloat
cpSpaceGetSleepTimeThreshold(const cpSpace * space)291 cpSpaceGetSleepTimeThreshold(const cpSpace *space)
292 {
293 return space->sleepTimeThreshold;
294 }
295
296 void
cpSpaceSetSleepTimeThreshold(cpSpace * space,cpFloat sleepTimeThreshold)297 cpSpaceSetSleepTimeThreshold(cpSpace *space, cpFloat sleepTimeThreshold)
298 {
299 space->sleepTimeThreshold = sleepTimeThreshold;
300 }
301
302 cpFloat
cpSpaceGetCollisionSlop(const cpSpace * space)303 cpSpaceGetCollisionSlop(const cpSpace *space)
304 {
305 return space->collisionSlop;
306 }
307
308 void
cpSpaceSetCollisionSlop(cpSpace * space,cpFloat collisionSlop)309 cpSpaceSetCollisionSlop(cpSpace *space, cpFloat collisionSlop)
310 {
311 space->collisionSlop = collisionSlop;
312 }
313
314 cpFloat
cpSpaceGetCollisionBias(const cpSpace * space)315 cpSpaceGetCollisionBias(const cpSpace *space)
316 {
317 return space->collisionBias;
318 }
319
320 void
cpSpaceSetCollisionBias(cpSpace * space,cpFloat collisionBias)321 cpSpaceSetCollisionBias(cpSpace *space, cpFloat collisionBias)
322 {
323 space->collisionBias = collisionBias;
324 }
325
326 cpTimestamp
cpSpaceGetCollisionPersistence(const cpSpace * space)327 cpSpaceGetCollisionPersistence(const cpSpace *space)
328 {
329 return space->collisionPersistence;
330 }
331
332 void
cpSpaceSetCollisionPersistence(cpSpace * space,cpTimestamp collisionPersistence)333 cpSpaceSetCollisionPersistence(cpSpace *space, cpTimestamp collisionPersistence)
334 {
335 space->collisionPersistence = collisionPersistence;
336 }
337
338 cpDataPointer
cpSpaceGetUserData(const cpSpace * space)339 cpSpaceGetUserData(const cpSpace *space)
340 {
341 return space->userData;
342 }
343
344 void
cpSpaceSetUserData(cpSpace * space,cpDataPointer userData)345 cpSpaceSetUserData(cpSpace *space, cpDataPointer userData)
346 {
347 space->userData = userData;
348 }
349
350 cpBody *
cpSpaceGetStaticBody(const cpSpace * space)351 cpSpaceGetStaticBody(const cpSpace *space)
352 {
353 return space->staticBody;
354 }
355
356 cpFloat
cpSpaceGetCurrentTimeStep(const cpSpace * space)357 cpSpaceGetCurrentTimeStep(const cpSpace *space)
358 {
359 return space->curr_dt;
360 }
361
362 void
cpSpaceSetStaticBody(cpSpace * space,cpBody * body)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
cpSpaceIsLocked(cpSpace * space)375 cpSpaceIsLocked(cpSpace *space)
376 {
377 return (space->locked > 0);
378 }
379
380 //MARK: Collision Handler Function Management
381
382 static void
cpSpaceUseWildcardDefaultHandler(cpSpace * space)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
cpSpaceAddDefaultCollisionHandler(cpSpace * space)392 cpCollisionHandler *cpSpaceAddDefaultCollisionHandler(cpSpace *space)
393 {
394 cpSpaceUseWildcardDefaultHandler(space);
395 return &space->defaultHandler;
396 }
397
cpSpaceAddCollisionHandler(cpSpace * space,cpCollisionType a,cpCollisionType b)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 *
cpSpaceAddWildcardHandler(cpSpace * space,cpCollisionType type)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 *
cpSpaceAddShape(cpSpace * space,cpShape * shape)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 *
cpSpaceAddBody(cpSpace * space,cpBody * body)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 *
cpSpaceAddConstraint(cpSpace * space,cpConstraint * constraint)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
cachedArbitersFilter(cpArbiter * arb,struct arbiterFilterContext * context)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
cpSpaceFilterArbiters(cpSpace * space,cpBody * body,cpShape * filter)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
cpSpaceRemoveShape(cpSpace * space,cpShape * shape)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
cpSpaceRemoveBody(cpSpace * space,cpBody * body)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
cpSpaceRemoveConstraint(cpSpace * space,cpConstraint * constraint)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
cpSpaceContainsShape(cpSpace * space,cpShape * shape)572 cpBool cpSpaceContainsShape(cpSpace *space, cpShape *shape)
573 {
574 return (shape->space == space);
575 }
576
cpSpaceContainsBody(cpSpace * space,cpBody * body)577 cpBool cpSpaceContainsBody(cpSpace *space, cpBody *body)
578 {
579 return (body->space == space);
580 }
581
cpSpaceContainsConstraint(cpSpace * space,cpConstraint * constraint)582 cpBool cpSpaceContainsConstraint(cpSpace *space, cpConstraint *constraint)
583 {
584 return (constraint->space == space);
585 }
586
587 //MARK: Iteration
588
589 void
cpSpaceEachBody(cpSpace * space,cpSpaceBodyIteratorFunc func,void * data)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
spaceEachShapeIterator(cpShape * shape,spaceShapeContext * context)623 spaceEachShapeIterator(cpShape *shape, spaceShapeContext *context)
624 {
625 context->func(shape, context->data);
626 }
627
628 void
cpSpaceEachShape(cpSpace * space,cpSpaceShapeIteratorFunc func,void * data)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
cpSpaceEachConstraint(cpSpace * space,cpSpaceConstraintIteratorFunc func,void * data)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
cpSpaceReindexStatic(cpSpace * space)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
cpSpaceReindexShape(cpSpace * space,cpShape * shape)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
cpSpaceReindexShapesForBody(cpSpace * space,cpBody * body)674 cpSpaceReindexShapesForBody(cpSpace *space, cpBody *body)
675 {
676 CP_BODY_FOREACH_SHAPE(body, shape) cpSpaceReindexShape(space, shape);
677 }
678
679
680 static void
copyShapes(cpShape * shape,cpSpatialIndex * index)681 copyShapes(cpShape *shape, cpSpatialIndex *index)
682 {
683 cpSpatialIndexInsert(index, shape, shape->hashid);
684 }
685
686 void
cpSpaceUseSpatialHash(cpSpace * space,cpFloat dim,int count)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 }
701