dali-physics/third-party/chipmunk2d/src/cpArbiter.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 // TODO: make this generic so I can reuse it for constraints also.
  25 static inline void
  26 unthreadHelper(cpArbiter *arb, cpBody *body)
  27 {
  28         struct cpArbiterThread *thread = cpArbiterThreadForBody(arb, body);
  29         cpArbiter *prev = thread->prev;
  30         cpArbiter *next = thread->next;
  31
  32         if(prev){
  33                 cpArbiterThreadForBody(prev, body)->next = next;
  34         } else if(body->arbiterList == arb) {
  35                 // IFF prev is NULL and body->arbiterList == arb, is arb at the head of the list.
  36                 // This function may be called for an arbiter that was never in a list.
  37                 // In that case, we need to protect it from wiping out the body->arbiterList pointer.
  38                 body->arbiterList = next;
  39         }
  40
  41         if(next) cpArbiterThreadForBody(next, body)->prev = prev;
  42
  43         thread->prev = NULL;
  44         thread->next = NULL;
  45 }
  46
  47 void
  48 cpArbiterUnthread(cpArbiter *arb)
  49 {
  50         unthreadHelper(arb, arb->body_a);
  51         unthreadHelper(arb, arb->body_b);
  52 }
  53
  54 cpBool cpArbiterIsFirstContact(const cpArbiter *arb)
  55 {
  56         return arb->state == CP_ARBITER_STATE_FIRST_COLLISION;
  57 }
  58
  59 cpBool cpArbiterIsRemoval(const cpArbiter *arb)
  60 {
  61         return arb->state == CP_ARBITER_STATE_INVALIDATED;
  62 }
  63
  64 int cpArbiterGetCount(const cpArbiter *arb)
  65 {
  66         // Return 0 contacts if we are in a separate callback.
  67         return (arb->state < CP_ARBITER_STATE_CACHED ? arb->count : 0);
  68 }
  69
  70 cpVect
  71 cpArbiterGetNormal(const cpArbiter *arb)
  72 {
  73         return cpvmult(arb->n, arb->swapped ? -1.0f : 1.0);
  74 }
  75
  76 cpVect
  77 cpArbiterGetPointA(const cpArbiter *arb, int i)
  78 {
  79         cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
  80         return cpvadd(arb->body_a->p, arb->contacts[i].r1);
  81 }
  82
  83 cpVect
  84 cpArbiterGetPointB(const cpArbiter *arb, int i)
  85 {
  86         cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
  87         return cpvadd(arb->body_b->p, arb->contacts[i].r2);
  88 }
  89
  90 cpFloat
  91 cpArbiterGetDepth(const cpArbiter *arb, int i)
  92 {
  93         cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
  94
  95         struct cpContact *con = &arb->contacts[i];
  96         return cpvdot(cpvadd(cpvsub(con->r2, con->r1), cpvsub(arb->body_b->p, arb->body_a->p)), arb->n);
  97 }
  98
  99 cpContactPointSet
 100 cpArbiterGetContactPointSet(const cpArbiter *arb)
 101 {
 102         cpContactPointSet set;
 103         set.count = cpArbiterGetCount(arb);
 104
 105         cpBool swapped = arb->swapped;
 106         cpVect n = arb->n;
 107         set.normal = (swapped ? cpvneg(n) : n);
 108
 109         for(int i=0; i<set.count; i++){
 110                 // Contact points are relative to body CoGs;
 111                 cpVect p1 = cpvadd(arb->body_a->p, arb->contacts[i].r1);
 112                 cpVect p2 = cpvadd(arb->body_b->p, arb->contacts[i].r2);
 113
 114                 set.points[i].pointA = (swapped ? p2 : p1);
 115                 set.points[i].pointB = (swapped ? p1 : p2);
 116                 set.points[i].distance = cpvdot(cpvsub(p2, p1), n);
 117         }
 118
 119         return set;
 120 }
 121
 122 void
 123 cpArbiterSetContactPointSet(cpArbiter *arb, cpContactPointSet *set)
 124 {
 125         int count = set->count;
 126         cpAssertHard(count == arb->count, "The number of contact points cannot be changed.");
 127
 128         cpBool swapped = arb->swapped;
 129         arb->n = (swapped ? cpvneg(set->normal) : set->normal);
 130
 131         for(int i=0; i<count; i++){
 132                 // Convert back to CoG relative offsets.
 133                 cpVect p1 = set->points[i].pointA;
 134                 cpVect p2 = set->points[i].pointB;
 135
 136                 arb->contacts[i].r1 = cpvsub(swapped ? p2 : p1, arb->body_a->p);
 137                 arb->contacts[i].r2 = cpvsub(swapped ? p1 : p2, arb->body_b->p);
 138         }
 139 }
 140
 141 cpVect
 142 cpArbiterTotalImpulse(const cpArbiter *arb)
 143 {
 144         struct cpContact *contacts = arb->contacts;
 145         cpVect n = arb->n;
 146         cpVect sum = cpvzero;
 147
 148         for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
 149                 struct cpContact *con = &contacts[i];
 150                 sum = cpvadd(sum, cpvrotate(n, cpv(con->jnAcc, con->jtAcc)));
 151         }
 152
 153         return (arb->swapped ? sum : cpvneg(sum));
 154         return cpvzero;
 155 }
 156
 157 cpFloat
 158 cpArbiterTotalKE(const cpArbiter *arb)
 159 {
 160         cpFloat eCoef = (1 - arb->e)/(1 + arb->e);
 161         cpFloat sum = 0.0;
 162
 163         struct cpContact *contacts = arb->contacts;
 164         for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
 165                 struct cpContact *con = &contacts[i];
 166                 cpFloat jnAcc = con->jnAcc;
 167                 cpFloat jtAcc = con->jtAcc;
 168
 169                 sum += eCoef*jnAcc*jnAcc/con->nMass + jtAcc*jtAcc/con->tMass;
 170         }
 171
 172         return sum;
 173 }
 174
 175 cpBool
 176 cpArbiterIgnore(cpArbiter *arb)
 177 {
 178         arb->state = CP_ARBITER_STATE_IGNORE;
 179         return cpFalse;
 180 }
 181
 182 cpFloat
 183 cpArbiterGetRestitution(const cpArbiter *arb)
 184 {
 185         return arb->e;
 186 }
 187
 188 void
 189 cpArbiterSetRestitution(cpArbiter *arb, cpFloat restitution)
 190 {
 191         arb->e = restitution;
 192 }
 193
 194 cpFloat
 195 cpArbiterGetFriction(const cpArbiter *arb)
 196 {
 197         return arb->u;
 198 }
 199
 200 void
 201 cpArbiterSetFriction(cpArbiter *arb, cpFloat friction)
 202 {
 203         arb->u = friction;
 204 }
 205
 206 cpVect
 207 cpArbiterGetSurfaceVelocity(cpArbiter *arb)
 208 {
 209         return cpvmult(arb->surface_vr, arb->swapped ? -1.0f : 1.0);
 210 }
 211
 212 void
 213 cpArbiterSetSurfaceVelocity(cpArbiter *arb, cpVect vr)
 214 {
 215         arb->surface_vr = cpvmult(vr, arb->swapped ? -1.0f : 1.0);
 216 }
 217
 218 cpDataPointer
 219 cpArbiterGetUserData(const cpArbiter *arb)
 220 {
 221         return arb->data;
 222 }
 223
 224 void
 225 cpArbiterSetUserData(cpArbiter *arb, cpDataPointer userData)
 226 {
 227         arb->data = userData;
 228 }
 229
 230 void
 231 cpArbiterGetShapes(const cpArbiter *arb, cpShape **a, cpShape **b)
 232 {
 233         if(arb->swapped){
 234                 (*a) = (cpShape *)arb->b, (*b) = (cpShape *)arb->a;
 235         } else {
 236                 (*a) = (cpShape *)arb->a, (*b) = (cpShape *)arb->b;
 237         }
 238 }
 239
 240 void cpArbiterGetBodies(const cpArbiter *arb, cpBody **a, cpBody **b)
 241 {
 242         CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b);
 243         (*a) = shape_a->body;
 244         (*b) = shape_b->body;
 245 }
 246
 247 cpBool
 248 cpArbiterCallWildcardBeginA(cpArbiter *arb, cpSpace *space)
 249 {
 250         cpCollisionHandler *handler = arb->handlerA;
 251         return handler->beginFunc(arb, space, handler->userData);
 252 }
 253
 254 cpBool
 255 cpArbiterCallWildcardBeginB(cpArbiter *arb, cpSpace *space)
 256 {
 257         cpCollisionHandler *handler = arb->handlerB;
 258         arb->swapped = !arb->swapped;
 259         cpBool retval = handler->beginFunc(arb, space, handler->userData);
 260         arb->swapped = !arb->swapped;
 261         return retval;
 262 }
 263
 264 cpBool
 265 cpArbiterCallWildcardPreSolveA(cpArbiter *arb, cpSpace *space)
 266 {
 267         cpCollisionHandler *handler = arb->handlerA;
 268         return handler->preSolveFunc(arb, space, handler->userData);
 269 }
 270
 271 cpBool
 272 cpArbiterCallWildcardPreSolveB(cpArbiter *arb, cpSpace *space)
 273 {
 274         cpCollisionHandler *handler = arb->handlerB;
 275         arb->swapped = !arb->swapped;
 276         cpBool retval = handler->preSolveFunc(arb, space, handler->userData);
 277         arb->swapped = !arb->swapped;
 278         return retval;
 279 }
 280
 281 void
 282 cpArbiterCallWildcardPostSolveA(cpArbiter *arb, cpSpace *space)
 283 {
 284         cpCollisionHandler *handler = arb->handlerA;
 285         handler->postSolveFunc(arb, space, handler->userData);
 286 }
 287
 288 void
 289 cpArbiterCallWildcardPostSolveB(cpArbiter *arb, cpSpace *space)
 290 {
 291         cpCollisionHandler *handler = arb->handlerB;
 292         arb->swapped = !arb->swapped;
 293         handler->postSolveFunc(arb, space, handler->userData);
 294         arb->swapped = !arb->swapped;
 295 }
 296
 297 void
 298 cpArbiterCallWildcardSeparateA(cpArbiter *arb, cpSpace *space)
 299 {
 300         cpCollisionHandler *handler = arb->handlerA;
 301         handler->separateFunc(arb, space, handler->userData);
 302 }
 303
 304 void
 305 cpArbiterCallWildcardSeparateB(cpArbiter *arb, cpSpace *space)
 306 {
 307         cpCollisionHandler *handler = arb->handlerB;
 308         arb->swapped = !arb->swapped;
 309         handler->separateFunc(arb, space, handler->userData);
 310         arb->swapped = !arb->swapped;
 311 }
 312
 313 cpArbiter*
 314 cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b)
 315 {
 316         arb->handler = NULL;
 317         arb->swapped = cpFalse;
 318
 319         arb->handler = NULL;
 320         arb->handlerA = NULL;
 321         arb->handlerB = NULL;
 322
 323         arb->e = 0.0f;
 324         arb->u = 0.0f;
 325         arb->surface_vr = cpvzero;
 326
 327         arb->count = 0;
 328         arb->contacts = NULL;
 329
 330         arb->a = a; arb->body_a = a->body;
 331         arb->b = b; arb->body_b = b->body;
 332
 333         arb->thread_a.next = NULL;
 334         arb->thread_b.next = NULL;
 335         arb->thread_a.prev = NULL;
 336         arb->thread_b.prev = NULL;
 337
 338         arb->stamp = 0;
 339         arb->state = CP_ARBITER_STATE_FIRST_COLLISION;
 340
 341         arb->data = NULL;
 342
 343         return arb;
 344 }
 345
 346 static inline cpCollisionHandler *
 347 cpSpaceLookupHandler(cpSpace *space, cpCollisionType a, cpCollisionType b, cpCollisionHandler *defaultValue)
 348 {
 349         cpCollisionType types[] = {a, b};
 350         cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collisionHandlers, CP_HASH_PAIR(a, b), types);
 351         return (handler ? handler : defaultValue);
 352 }
 353
 354 void
 355 cpArbiterUpdate(cpArbiter *arb, struct cpCollisionInfo *info, cpSpace *space)
 356 {
 357         const cpShape *a = info->a, *b = info->b;
 358
 359         // For collisions between two similar primitive types, the order could have been swapped since the last frame.
 360         arb->a = a; arb->body_a = a->body;
 361         arb->b = b; arb->body_b = b->body;
 362
 363         // Iterate over the possible pairs to look for hash value matches.
 364         for(int i=0; i<info->count; i++){
 365                 struct cpContact *con = &info->arr[i];
 366
 367                 // r1 and r2 store absolute offsets at init time.
 368                 // Need to convert them to relative offsets.
 369                 con->r1 = cpvsub(con->r1, a->body->p);
 370                 con->r2 = cpvsub(con->r2, b->body->p);
 371
 372                 // Cached impulses are not zeroed at init time.
 373                 con->jnAcc = con->jtAcc = 0.0f;
 374
 375                 for(int j=0; j<arb->count; j++){
 376                         struct cpContact *old = &arb->contacts[j];
 377
 378                         // This could trigger false positives, but is fairly unlikely nor serious if it does.
 379                         if(con->hash == old->hash){
 380                                 // Copy the persistant contact information.
 381                                 con->jnAcc = old->jnAcc;
 382                                 con->jtAcc = old->jtAcc;
 383                         }
 384                 }
 385         }
 386
 387         arb->contacts = info->arr;
 388         arb->count = info->count;
 389         arb->n = info->n;
 390
 391         arb->e = a->e * b->e;
 392         arb->u = a->u * b->u;
 393
 394         cpVect surface_vr = cpvsub(b->surfaceV, a->surfaceV);
 395         arb->surface_vr = cpvsub(surface_vr, cpvmult(info->n, cpvdot(surface_vr, info->n)));
 396
 397         cpCollisionType typeA = info->a->type, typeB = info->b->type;
 398         cpCollisionHandler *defaultHandler = &space->defaultHandler;
 399         cpCollisionHandler *handler = arb->handler = cpSpaceLookupHandler(space, typeA, typeB, defaultHandler);
 400
 401         // Check if the types match, but don't swap for a default handler which use the wildcard for type A.
 402         cpBool swapped = arb->swapped = (typeA != handler->typeA && handler->typeA != CP_WILDCARD_COLLISION_TYPE);
 403
 404         if(handler != defaultHandler || space->usesWildcards){
 405                 // The order of the main handler swaps the wildcard handlers too. Uffda.
 406                 arb->handlerA = cpSpaceLookupHandler(space, (swapped ? typeB : typeA), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing);
 407                 arb->handlerB = cpSpaceLookupHandler(space, (swapped ? typeA : typeB), CP_WILDCARD_COLLISION_TYPE, &cpCollisionHandlerDoNothing);
 408         }
 409
 410         // mark it as new if it's been cached
 411         if(arb->state == CP_ARBITER_STATE_CACHED) arb->state = CP_ARBITER_STATE_FIRST_COLLISION;
 412 }
 413
 414 void
 415 cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias)
 416 {
 417         cpBody *a = arb->body_a;
 418         cpBody *b = arb->body_b;
 419         cpVect n = arb->n;
 420         cpVect body_delta = cpvsub(b->p, a->p);
 421
 422         for(int i=0; i<arb->count; i++){
 423                 struct cpContact *con = &arb->contacts[i];
 424
 425                 // Calculate the mass normal and mass tangent.
 426                 con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, n);
 427                 con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(n));
 428
 429                 // Calculate the target bias velocity.
 430                 cpFloat dist = cpvdot(cpvadd(cpvsub(con->r2, con->r1), body_delta), n);
 431                 con->bias = -bias*cpfmin(0.0f, dist + slop)/dt;
 432                 con->jBias = 0.0f;
 433
 434                 // Calculate the target bounce velocity.
 435                 con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, n)*arb->e;
 436         }
 437 }
 438
 439 void
 440 cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef)
 441 {
 442         if(cpArbiterIsFirstContact(arb)) return;
 443
 444         cpBody *a = arb->body_a;
 445         cpBody *b = arb->body_b;
 446         cpVect n = arb->n;
 447
 448         for(int i=0; i<arb->count; i++){
 449                 struct cpContact *con = &arb->contacts[i];
 450                 cpVect j = cpvrotate(n, cpv(con->jnAcc, con->jtAcc));
 451                 apply_impulses(a, b, con->r1, con->r2, cpvmult(j, dt_coef));
 452         }
 453 }
 454
 455 // TODO: is it worth splitting velocity/position correction?
 456
 457 void
 458 cpArbiterApplyImpulse(cpArbiter *arb)
 459 {
 460         cpBody *a = arb->body_a;
 461         cpBody *b = arb->body_b;
 462         cpVect n = arb->n;
 463         cpVect surface_vr = arb->surface_vr;
 464         cpFloat friction = arb->u;
 465
 466         for(int i=0; i<arb->count; i++){
 467                 struct cpContact *con = &arb->contacts[i];
 468                 cpFloat nMass = con->nMass;
 469                 cpVect r1 = con->r1;
 470                 cpVect r2 = con->r2;
 471
 472                 cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias));
 473                 cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias));
 474                 cpVect vr = cpvadd(relative_velocity(a, b, r1, r2), surface_vr);
 475
 476                 cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
 477                 cpFloat vrn = cpvdot(vr, n);
 478                 cpFloat vrt = cpvdot(vr, cpvperp(n));
 479
 480                 cpFloat jbn = (con->bias - vbn)*nMass;
 481                 cpFloat jbnOld = con->jBias;
 482                 con->jBias = cpfmax(jbnOld + jbn, 0.0f);
 483
 484                 cpFloat jn = -(con->bounce + vrn)*nMass;
 485                 cpFloat jnOld = con->jnAcc;
 486                 con->jnAcc = cpfmax(jnOld + jn, 0.0f);
 487
 488                 cpFloat jtMax = friction*con->jnAcc;
 489                 cpFloat jt = -vrt*con->tMass;
 490                 cpFloat jtOld = con->jtAcc;
 491                 con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);
 492
 493                 apply_bias_impulses(a, b, r1, r2, cpvmult(n, con->jBias - jbnOld));
 494                 apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(con->jnAcc - jnOld, con->jtAcc - jtOld)));
 495         }
 496 }