dali-physics/third-party/chipmunk2d/src/cpSpaceStep.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
  24 //MARK: Post Step Callback Functions
  25
  26 cpPostStepCallback *
  27 cpSpaceGetPostStepCallback(cpSpace *space, void *key)
  28 {
  29         cpArray *arr = space->postStepCallbacks;
  30         for(int i=0; i<arr->num; i++){
  31                 cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i];
  32                 if(callback && callback->key == key) return callback;
  33         }
  34
  35         return NULL;
  36 }
  37
  38 static void PostStepDoNothing(cpSpace *space, void *obj, void *data){}
  39
  40 cpBool
  41 cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, void *data)
  42 {
  43         cpAssertWarn(space->locked,
  44                 "Adding a post-step callback when the space is not locked is unnecessary. "
  45                 "Post-step callbacks will not called until the end of the next call to cpSpaceStep() or the next query.");
  46
  47         if(!cpSpaceGetPostStepCallback(space, key)){
  48                 cpPostStepCallback *callback = (cpPostStepCallback *)cpcalloc(1, sizeof(cpPostStepCallback));
  49                 callback->func = (func ? func : PostStepDoNothing);
  50                 callback->key = key;
  51                 callback->data = data;
  52
  53                 cpArrayPush(space->postStepCallbacks, callback);
  54                 return cpTrue;
  55         } else {
  56                 return cpFalse;
  57         }
  58 }
  59
  60 //MARK: Locking Functions
  61
  62 void
  63 cpSpaceLock(cpSpace *space)
  64 {
  65         space->locked++;
  66 }
  67
  68 void
  69 cpSpaceUnlock(cpSpace *space, cpBool runPostStep)
  70 {
  71         space->locked--;
  72         cpAssertHard(space->locked >= 0, "Internal Error: Space lock underflow.");
  73
  74         if(space->locked == 0){
  75                 cpArray *waking = space->rousedBodies;
  76
  77                 for(int i=0, count=waking->num; i<count; i++){
  78                         cpSpaceActivateBody(space, (cpBody *)waking->arr[i]);
  79                         waking->arr[i] = NULL;
  80                 }
  81
  82                 waking->num = 0;
  83
  84                 if(space->locked == 0 && runPostStep && !space->skipPostStep){
  85                         space->skipPostStep = cpTrue;
  86
  87                         cpArray *arr = space->postStepCallbacks;
  88                         for(int i=0; i<arr->num; i++){
  89                                 cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i];
  90                                 cpPostStepFunc func = callback->func;
  91
  92                                 // Mark the func as NULL in case calling it calls cpSpaceRunPostStepCallbacks() again.
  93                                 // TODO: need more tests around this case I think.
  94                                 callback->func = NULL;
  95                                 if(func) func(space, callback->key, callback->data);
  96
  97                                 arr->arr[i] = NULL;
  98                                 cpfree(callback);
  99                         }
 100
 101                         arr->num = 0;
 102                         space->skipPostStep = cpFalse;
 103                 }
 104         }
 105 }
 106
 107 //MARK: Contact Buffer Functions
 108
 109 struct cpContactBufferHeader {
 110         cpTimestamp stamp;
 111         cpContactBufferHeader *next;
 112         unsigned int numContacts;
 113 };
 114
 115 #define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(struct cpContact))
 116 typedef struct cpContactBuffer {
 117         cpContactBufferHeader header;
 118         struct cpContact contacts[CP_CONTACTS_BUFFER_SIZE];
 119 } cpContactBuffer;
 120
 121 static cpContactBufferHeader *
 122 cpSpaceAllocContactBuffer(cpSpace *space)
 123 {
 124         cpContactBuffer *buffer = (cpContactBuffer *)cpcalloc(1, sizeof(cpContactBuffer));
 125         cpArrayPush(space->allocatedBuffers, buffer);
 126         return (cpContactBufferHeader *)buffer;
 127 }
 128
 129 static cpContactBufferHeader *
 130 cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpContactBufferHeader *splice)
 131 {
 132         header->stamp = stamp;
 133         header->next = (splice ? splice->next : header);
 134         header->numContacts = 0;
 135
 136         return header;
 137 }
 138
 139 void
 140 cpSpacePushFreshContactBuffer(cpSpace *space)
 141 {
 142         cpTimestamp stamp = space->stamp;
 143
 144         cpContactBufferHeader *head = space->contactBuffersHead;
 145
 146         if(!head){
 147                 // No buffers have been allocated, make one
 148                 space->contactBuffersHead = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, NULL);
 149         } else if(stamp - head->next->stamp > space->collisionPersistence){
 150                 // The tail buffer is available, rotate the ring
 151         cpContactBufferHeader *tail = head->next;
 152                 space->contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail);
 153         } else {
 154                 // Allocate a new buffer and push it into the ring
 155                 cpContactBufferHeader *buffer = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, head);
 156                 space->contactBuffersHead = head->next = buffer;
 157         }
 158 }
 159
 160
 161 struct cpContact *
 162 cpContactBufferGetArray(cpSpace *space)
 163 {
 164         if(space->contactBuffersHead->numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){
 165                 // contact buffer could overflow on the next collision, push a fresh one.
 166                 cpSpacePushFreshContactBuffer(space);
 167         }
 168
 169         cpContactBufferHeader *head = space->contactBuffersHead;
 170         return ((cpContactBuffer *)head)->contacts + head->numContacts;
 171 }
 172
 173 void
 174 cpSpacePushContacts(cpSpace *space, int count)
 175 {
 176         cpAssertHard(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal Error: Contact buffer overflow!");
 177         space->contactBuffersHead->numContacts += count;
 178 }
 179
 180 static void
 181 cpSpacePopContacts(cpSpace *space, int count){
 182         space->contactBuffersHead->numContacts -= count;
 183 }
 184
 185 //MARK: Collision Detection Functions
 186
 187 static void *
 188 cpSpaceArbiterSetTrans(cpShape **shapes, cpSpace *space)
 189 {
 190         if(space->pooledArbiters->num == 0){
 191                 // arbiter pool is exhausted, make more
 192                 int count = CP_BUFFER_BYTES/sizeof(cpArbiter);
 193                 cpAssertHard(count, "Internal Error: Buffer size too small.");
 194
 195                 cpArbiter *buffer = (cpArbiter *)cpcalloc(1, CP_BUFFER_BYTES);
 196                 cpArrayPush(space->allocatedBuffers, buffer);
 197
 198                 for(int i=0; i<count; i++) cpArrayPush(space->pooledArbiters, buffer + i);
 199         }
 200
 201         return cpArbiterInit((cpArbiter *)cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
 202 }
 203
 204 static inline cpBool
 205 QueryRejectConstraint(cpBody *a, cpBody *b)
 206 {
 207         CP_BODY_FOREACH_CONSTRAINT(a, constraint){
 208                 if(
 209                         !constraint->collideBodies && (
 210                                 (constraint->a == a && constraint->b == b) ||
 211                                 (constraint->a == b && constraint->b == a)
 212                         )
 213                 ) return cpTrue;
 214         }
 215
 216         return cpFalse;
 217 }
 218
 219 static inline cpBool
 220 QueryReject(cpShape *a, cpShape *b)
 221 {
 222         return (
 223                 // BBoxes must overlap
 224                 !cpBBIntersects(a->bb, b->bb)
 225                 // Don't collide shapes attached to the same body.
 226                 || a->body == b->body
 227                 // Don't collide shapes that are filtered.
 228                 || cpShapeFilterReject(a->filter, b->filter)
 229                 // Don't collide bodies if they have a constraint with collideBodies == cpFalse.
 230                 || QueryRejectConstraint(a->body, b->body)
 231         );
 232 }
 233
 234 // Callback from the spatial hash.
 235 cpCollisionID
 236 cpSpaceCollideShapes(cpShape *a, cpShape *b, cpCollisionID id, cpSpace *space)
 237 {
 238         // Reject any of the simple cases
 239         if(QueryReject(a,b)) return id;
 240
 241         // Narrow-phase collision detection.
 242         struct cpCollisionInfo info = cpCollide(a, b, id, cpContactBufferGetArray(space));
 243
 244         if(info.count == 0) return info.id; // Shapes are not colliding.
 245         cpSpacePushContacts(space, info.count);
 246
 247         // Get an arbiter from space->arbiterSet for the two shapes.
 248         // This is where the persistant contact magic comes from.
 249         const cpShape *shape_pair[] = {info.a, info.b};
 250         cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)info.a, (cpHashValue)info.b);
 251         cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, (cpHashSetTransFunc)cpSpaceArbiterSetTrans, space);
 252         cpArbiterUpdate(arb, &info, space);
 253
 254         cpCollisionHandler *handler = arb->handler;
 255
 256         // Call the begin function first if it's the first step
 257         if(arb->state == CP_ARBITER_STATE_FIRST_COLLISION && !handler->beginFunc(arb, space, handler->userData)){
 258                 cpArbiterIgnore(arb); // permanently ignore the collision until separation
 259         }
 260
 261         if(
 262                 // Ignore the arbiter if it has been flagged
 263                 (arb->state != CP_ARBITER_STATE_IGNORE) &&
 264                 // Call preSolve
 265                 handler->preSolveFunc(arb, space, handler->userData) &&
 266                 // Check (again) in case the pre-solve() callback called cpArbiterIgnored().
 267                 arb->state != CP_ARBITER_STATE_IGNORE &&
 268                 // Process, but don't add collisions for sensors.
 269                 !(a->sensor || b->sensor) &&
 270                 // Don't process collisions between two infinite mass bodies.
 271                 // This includes collisions between two kinematic bodies, or a kinematic body and a static body.
 272                 !(a->body->m == INFINITY && b->body->m == INFINITY)
 273         ){
 274                 cpArrayPush(space->arbiters, arb);
 275         } else {
 276                 cpSpacePopContacts(space, info.count);
 277
 278                 arb->contacts = NULL;
 279                 arb->count = 0;
 280
 281                 // Normally arbiters are set as used after calling the post-solve callback.
 282                 // However, post-solve() callbacks are not called for sensors or arbiters rejected from pre-solve.
 283                 if(arb->state != CP_ARBITER_STATE_IGNORE) arb->state = CP_ARBITER_STATE_NORMAL;
 284         }
 285
 286         // Time stamp the arbiter so we know it was used recently.
 287         arb->stamp = space->stamp;
 288         return info.id;
 289 }
 290
 291 // Hashset filter func to throw away old arbiters.
 292 cpBool
 293 cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space)
 294 {
 295         cpTimestamp ticks = space->stamp - arb->stamp;
 296
 297         cpBody *a = arb->body_a, *b = arb->body_b;
 298
 299         // TODO: should make an arbiter state for this so it doesn't require filtering arbiters for dangling body pointers on body removal.
 300         // Preserve arbiters on sensors and rejected arbiters for sleeping objects.
 301         // This prevents errant separate callbacks from happenening.
 302         if(
 303                 (cpBodyGetType(a) == CP_BODY_TYPE_STATIC || cpBodyIsSleeping(a)) &&
 304                 (cpBodyGetType(b) == CP_BODY_TYPE_STATIC || cpBodyIsSleeping(b))
 305         ){
 306                 return cpTrue;
 307         }
 308
 309         // Arbiter was used last frame, but not this one
 310         if(ticks >= 1 && arb->state != CP_ARBITER_STATE_CACHED){
 311                 arb->state = CP_ARBITER_STATE_CACHED;
 312                 cpCollisionHandler *handler = arb->handler;
 313                 handler->separateFunc(arb, space, handler->userData);
 314         }
 315
 316         if(ticks >= space->collisionPersistence){
 317                 arb->contacts = NULL;
 318                 arb->count = 0;
 319
 320                 cpArrayPush(space->pooledArbiters, arb);
 321                 return cpFalse;
 322         }
 323
 324         return cpTrue;
 325 }
 326
 327 //MARK: All Important cpSpaceStep() Function
 328
 329  void
 330 cpShapeUpdateFunc(cpShape *shape, void *unused)
 331 {
 332         cpShapeCacheBB(shape);
 333 }
 334
 335 void
 336 cpSpaceStep(cpSpace *space, cpFloat dt)
 337 {
 338         // don't step if the timestep is 0!
 339         if(dt == 0.0f) return;
 340
 341         space->stamp++;
 342
 343         cpFloat prev_dt = space->curr_dt;
 344         space->curr_dt = dt;
 345
 346         cpArray *bodies = space->dynamicBodies;
 347         cpArray *constraints = space->constraints;
 348         cpArray *arbiters = space->arbiters;
 349
 350         // Reset and empty the arbiter lists.
 351         for(int i=0; i<arbiters->num; i++){
 352                 cpArbiter *arb = (cpArbiter *)arbiters->arr[i];
 353                 arb->state = CP_ARBITER_STATE_NORMAL;
 354
 355                 // If both bodies are awake, unthread the arbiter from the contact graph.
 356                 if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)){
 357                         cpArbiterUnthread(arb);
 358                 }
 359         }
 360         arbiters->num = 0;
 361
 362         cpSpaceLock(space); {
 363                 // Integrate positions
 364                 for(int i=0; i<bodies->num; i++){
 365                         cpBody *body = (cpBody *)bodies->arr[i];
 366                         body->position_func(body, dt);
 367                 }
 368
 369                 // Find colliding pairs.
 370                 cpSpacePushFreshContactBuffer(space);
 371                 cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL);
 372                 cpSpatialIndexReindexQuery(space->dynamicShapes, (cpSpatialIndexQueryFunc)cpSpaceCollideShapes, space);
 373         } cpSpaceUnlock(space, cpFalse);
 374
 375         // Rebuild the contact graph (and detect sleeping components if sleeping is enabled)
 376         cpSpaceProcessComponents(space, dt);
 377
 378         cpSpaceLock(space); {
 379                 // Clear out old cached arbiters and call separate callbacks
 380                 cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space);
 381
 382                 // Prestep the arbiters and constraints.
 383                 cpFloat slop = space->collisionSlop;
 384                 cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt);
 385                 for(int i=0; i<arbiters->num; i++){
 386                         cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef);
 387                 }
 388
 389                 for(int i=0; i<constraints->num; i++){
 390                         cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
 391
 392                         cpConstraintPreSolveFunc preSolve = constraint->preSolve;
 393                         if(preSolve) preSolve(constraint, space);
 394
 395                         constraint->klass->preStep(constraint, dt);
 396                 }
 397
 398                 // Integrate velocities.
 399                 cpFloat damping = cpfpow(space->damping, dt);
 400                 cpVect gravity = space->gravity;
 401                 for(int i=0; i<bodies->num; i++){
 402                         cpBody *body = (cpBody *)bodies->arr[i];
 403                         body->velocity_func(body, gravity, damping, dt);
 404                 }
 405
 406                 // Apply cached impulses
 407                 cpFloat dt_coef = (prev_dt == 0.0f ? 0.0f : dt/prev_dt);
 408                 for(int i=0; i<arbiters->num; i++){
 409                         cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef);
 410                 }
 411
 412                 for(int i=0; i<constraints->num; i++){
 413                         cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
 414                         constraint->klass->applyCachedImpulse(constraint, dt_coef);
 415                 }
 416
 417                 // Run the impulse solver.
 418                 for(int i=0; i<space->iterations; i++){
 419                         for(int j=0; j<arbiters->num; j++){
 420                                 cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j]);
 421                         }
 422
 423                         for(int j=0; j<constraints->num; j++){
 424                                 cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
 425                                 constraint->klass->applyImpulse(constraint, dt);
 426                         }
 427                 }
 428
 429                 // Run the constraint post-solve callbacks
 430                 for(int i=0; i<constraints->num; i++){
 431                         cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
 432
 433                         cpConstraintPostSolveFunc postSolve = constraint->postSolve;
 434                         if(postSolve) postSolve(constraint, space);
 435                 }
 436
 437                 // run the post-solve callbacks
 438                 for(int i=0; i<arbiters->num; i++){
 439                         cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
 440
 441                         cpCollisionHandler *handler = arb->handler;
 442                         handler->postSolveFunc(arb, space, handler->userData);
 443                 }
 444         } cpSpaceUnlock(space, cpTrue);
 445 }