dali-physics/third-party/chipmunk2d/src/cpBody.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 <float.h>
  23 #include <stdarg.h>
  24
  25 #include "chipmunk/chipmunk_private.h"
  26
  27 cpBody*
  28 cpBodyAlloc(void)
  29 {
  30         return (cpBody *)cpcalloc(1, sizeof(cpBody));
  31 }
  32
  33 cpBody *
  34 cpBodyInit(cpBody *body, cpFloat mass, cpFloat moment)
  35 {
  36         body->space = NULL;
  37         body->shapeList = NULL;
  38         body->arbiterList = NULL;
  39         body->constraintList = NULL;
  40
  41         body->velocity_func = cpBodyUpdateVelocity;
  42         body->position_func = cpBodyUpdatePosition;
  43
  44         body->sleeping.root = NULL;
  45         body->sleeping.next = NULL;
  46         body->sleeping.idleTime = 0.0f;
  47
  48         body->p = cpvzero;
  49         body->v = cpvzero;
  50         body->f = cpvzero;
  51
  52         body->w = 0.0f;
  53         body->t = 0.0f;
  54
  55         body->v_bias = cpvzero;
  56         body->w_bias = 0.0f;
  57
  58         body->userData = NULL;
  59
  60         // Setters must be called after full initialization so the sanity checks don't assert on garbage data.
  61         cpBodySetMass(body, mass);
  62         cpBodySetMoment(body, moment);
  63         cpBodySetAngle(body, 0.0f);
  64
  65         return body;
  66 }
  67
  68 cpBody*
  69 cpBodyNew(cpFloat mass, cpFloat moment)
  70 {
  71         return cpBodyInit(cpBodyAlloc(), mass, moment);
  72 }
  73
  74 cpBody*
  75 cpBodyNewKinematic()
  76 {
  77         cpBody *body = cpBodyNew(0.0f, 0.0f);
  78         cpBodySetType(body, CP_BODY_TYPE_KINEMATIC);
  79
  80         return body;
  81 }
  82
  83 cpBody*
  84 cpBodyNewStatic()
  85 {
  86         cpBody *body = cpBodyNew(0.0f, 0.0f);
  87         cpBodySetType(body, CP_BODY_TYPE_STATIC);
  88
  89         return body;
  90 }
  91
  92 void cpBodyDestroy(cpBody *body){}
  93
  94 void
  95 cpBodyFree(cpBody *body)
  96 {
  97         if(body){
  98                 cpBodyDestroy(body);
  99                 cpfree(body);
 100         }
 101 }
 102
 103 #ifdef NDEBUG
 104         #define cpAssertSaneBody(body)
 105 #else
 106         static void cpv_assert_nan(cpVect v, char *message){cpAssertHard(v.x == v.x && v.y == v.y, message);}
 107         static void cpv_assert_infinite(cpVect v, char *message){cpAssertHard(cpfabs(v.x) != INFINITY && cpfabs(v.y) != INFINITY, message);}
 108         static void cpv_assert_sane(cpVect v, char *message){cpv_assert_nan(v, message); cpv_assert_infinite(v, message);}
 109
 110         static void
 111         cpBodySanityCheck(const cpBody *body)
 112         {
 113                 cpAssertHard(body->m == body->m && body->m_inv == body->m_inv, "Body's mass is NaN.");
 114                 cpAssertHard(body->i == body->i && body->i_inv == body->i_inv, "Body's moment is NaN.");
 115                 cpAssertHard(body->m >= 0.0f, "Body's mass is negative.");
 116                 cpAssertHard(body->i >= 0.0f, "Body's moment is negative.");
 117
 118                 cpv_assert_sane(body->p, "Body's position is invalid.");
 119                 cpv_assert_sane(body->v, "Body's velocity is invalid.");
 120                 cpv_assert_sane(body->f, "Body's force is invalid.");
 121
 122                 cpAssertHard(body->a == body->a && cpfabs(body->a) != INFINITY, "Body's angle is invalid.");
 123                 cpAssertHard(body->w == body->w && cpfabs(body->w) != INFINITY, "Body's angular velocity is invalid.");
 124                 cpAssertHard(body->t == body->t && cpfabs(body->t) != INFINITY, "Body's torque is invalid.");
 125         }
 126
 127         #define cpAssertSaneBody(body) cpBodySanityCheck(body)
 128 #endif
 129
 130 cpBool
 131 cpBodyIsSleeping(const cpBody *body)
 132 {
 133         return (body->sleeping.root != ((cpBody*)0));
 134 }
 135
 136 cpBool
 137 cpBodyIsSleepThresholdExceeded(const cpBody *body, const cpShape *shape)
 138 {
 139   return body->sleeping.idleTime > shape->space->sleepTimeThreshold;
 140 }
 141
 142 cpBodyType
 143 cpBodyGetType(cpBody *body)
 144 {
 145         if(body->sleeping.idleTime == INFINITY){
 146                 return CP_BODY_TYPE_STATIC;
 147         } else if(body->m == INFINITY){
 148                 return CP_BODY_TYPE_KINEMATIC;
 149         } else {
 150                 return CP_BODY_TYPE_DYNAMIC;
 151         }
 152 }
 153
 154 void
 155 cpBodySetType(cpBody *body, cpBodyType type)
 156 {
 157         cpBodyType oldType = cpBodyGetType(body);
 158         if(oldType == type) return;
 159
 160         // Static bodies have their idle timers set to infinity.
 161         // Non-static bodies should have their idle timer reset.
 162         body->sleeping.idleTime = (type == CP_BODY_TYPE_STATIC ? INFINITY : 0.0f);
 163
 164         if(type == CP_BODY_TYPE_DYNAMIC){
 165                 body->m = body->i = 0.0f;
 166                 body->m_inv = body->i_inv = INFINITY;
 167
 168                 cpBodyAccumulateMassFromShapes(body);
 169         } else {
 170                 body->m = body->i = INFINITY;
 171                 body->m_inv = body->i_inv = 0.0f;
 172
 173                 body->v = cpvzero;
 174                 body->w = 0.0f;
 175         }
 176
 177         // If the body is added to a space already, we'll need to update some space data structures.
 178         cpSpace *space = cpBodyGetSpace(body);
 179         if(space != NULL){
 180                 cpAssertSpaceUnlocked(space);
 181
 182                 if(oldType == CP_BODY_TYPE_STATIC){
 183                         // TODO This is probably not necessary
 184 //                      cpBodyActivateStatic(body, NULL);
 185                 } else {
 186                         cpBodyActivate(body);
 187                 }
 188
 189                 // Move the bodies to the correct array.
 190                 cpArray *fromArray = cpSpaceArrayForBodyType(space, oldType);
 191                 cpArray *toArray = cpSpaceArrayForBodyType(space, type);
 192                 if(fromArray != toArray){
 193                         cpArrayDeleteObj(fromArray, body);
 194                         cpArrayPush(toArray, body);
 195                 }
 196
 197                 // Move the body's shapes to the correct spatial index.
 198                 cpSpatialIndex *fromIndex = (oldType == CP_BODY_TYPE_STATIC ? space->staticShapes : space->dynamicShapes);
 199                 cpSpatialIndex *toIndex = (type == CP_BODY_TYPE_STATIC ? space->staticShapes : space->dynamicShapes);
 200                 if(fromIndex != toIndex){
 201                         CP_BODY_FOREACH_SHAPE(body, shape){
 202                                 cpSpatialIndexRemove(fromIndex, shape, shape->hashid);
 203                                 cpSpatialIndexInsert(toIndex, shape, shape->hashid);
 204                         }
 205                 }
 206         }
 207 }
 208
 209
 210
 211 // Should *only* be called when shapes with mass info are modified, added or removed.
 212 void
 213 cpBodyAccumulateMassFromShapes(cpBody *body)
 214 {
 215         if(body == NULL || cpBodyGetType(body) != CP_BODY_TYPE_DYNAMIC) return;
 216
 217         // Reset the body's mass data.
 218         body->m = body->i = 0.0f;
 219         body->cog = cpvzero;
 220
 221         // Cache the position to realign it at the end.
 222         cpVect pos = cpBodyGetPosition(body);
 223
 224         // Accumulate mass from shapes.
 225         CP_BODY_FOREACH_SHAPE(body, shape){
 226                 struct cpShapeMassInfo *info = &shape->massInfo;
 227                 cpFloat m = info->m;
 228
 229                 if(m > 0.0f){
 230                         cpFloat msum = body->m + m;
 231
 232                         body->i += m*info->i + cpvdistsq(body->cog, info->cog)*(m*body->m)/msum;
 233                         body->cog = cpvlerp(body->cog, info->cog, m/msum);
 234                         body->m = msum;
 235                 }
 236         }
 237
 238         // Recalculate the inverses.
 239         body->m_inv = 1.0f/body->m;
 240         body->i_inv = 1.0f/body->i;
 241
 242         // Realign the body since the CoG has probably moved.
 243         cpBodySetPosition(body, pos);
 244         cpAssertSaneBody(body);
 245 }
 246
 247 cpSpace *
 248 cpBodyGetSpace(const cpBody *body)
 249 {
 250         return body->space;
 251 }
 252
 253 cpFloat
 254 cpBodyGetMass(const cpBody *body)
 255 {
 256         return body->m;
 257 }
 258
 259 void
 260 cpBodySetMass(cpBody *body, cpFloat mass)
 261 {
 262         cpAssertHard(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC, "You cannot set the mass of kinematic or static bodies.");
 263         cpAssertHard(0.0f <= mass && mass < INFINITY, "Mass must be positive and finite.");
 264
 265         cpBodyActivate(body);
 266         body->m = mass;
 267         body->m_inv = mass == 0.0f ? INFINITY : 1.0f/mass;
 268         cpAssertSaneBody(body);
 269 }
 270
 271 cpFloat
 272 cpBodyGetMoment(const cpBody *body)
 273 {
 274         return body->i;
 275 }
 276
 277 void
 278 cpBodySetMoment(cpBody *body, cpFloat moment)
 279 {
 280         cpAssertHard(moment >= 0.0f, "Moment of Inertia must be positive.");
 281
 282         cpBodyActivate(body);
 283         body->i = moment;
 284         body->i_inv = moment == 0.0f ? INFINITY : 1.0f/moment;
 285         cpAssertSaneBody(body);
 286 }
 287
 288 cpVect
 289 cpBodyGetRotation(const cpBody *body)
 290 {
 291         return cpv(body->transform.a, body->transform.b);
 292 }
 293
 294 void
 295 cpBodyAddShape(cpBody *body, cpShape *shape)
 296 {
 297         cpShape *next = body->shapeList;
 298         if(next) next->prev = shape;
 299
 300         shape->next = next;
 301         body->shapeList = shape;
 302
 303         if(shape->massInfo.m > 0.0f){
 304                 cpBodyAccumulateMassFromShapes(body);
 305         }
 306 }
 307
 308 void
 309 cpBodyRemoveShape(cpBody *body, cpShape *shape)
 310 {
 311   cpShape *prev = shape->prev;
 312   cpShape *next = shape->next;
 313
 314   if(prev){
 315                 prev->next = next;
 316   } else {
 317                 body->shapeList = next;
 318   }
 319
 320   if(next){
 321                 next->prev = prev;
 322         }
 323
 324   shape->prev = NULL;
 325   shape->next = NULL;
 326
 327         if(cpBodyGetType(body) == CP_BODY_TYPE_DYNAMIC && shape->massInfo.m > 0.0f){
 328                 cpBodyAccumulateMassFromShapes(body);
 329         }
 330 }
 331
 332 static cpConstraint *
 333 filterConstraints(cpConstraint *node, cpBody *body, cpConstraint *filter)
 334 {
 335         if(node == filter){
 336                 return cpConstraintNext(node, body);
 337         } else if(node->a == body){
 338                 node->next_a = filterConstraints(node->next_a, body, filter);
 339         } else {
 340                 node->next_b = filterConstraints(node->next_b, body, filter);
 341         }
 342
 343         return node;
 344 }
 345
 346 void
 347 cpBodyRemoveConstraint(cpBody *body, cpConstraint *constraint)
 348 {
 349         body->constraintList = filterConstraints(body->constraintList, body, constraint);
 350 }
 351
 352 // 'p' is the position of the CoG
 353 static void
 354 SetTransform(cpBody *body, cpVect p, cpFloat a)
 355 {
 356         cpVect rot = cpvforangle(a);
 357         cpVect c = body->cog;
 358
 359         body->transform = cpTransformNewTranspose(
 360                 rot.x, -rot.y, p.x - (c.x*rot.x - c.y*rot.y),
 361                 rot.y,  rot.x, p.y - (c.x*rot.y + c.y*rot.x)
 362         );
 363 }
 364
 365 static inline cpFloat
 366 SetAngle(cpBody *body, cpFloat a)
 367 {
 368         body->a = a;
 369         cpAssertSaneBody(body);
 370
 371         return a;
 372 }
 373
 374 cpVect
 375 cpBodyGetPosition(const cpBody *body)
 376 {
 377         return cpTransformPoint(body->transform, cpvzero);
 378 }
 379
 380 void
 381 cpBodySetPosition(cpBody *body, cpVect position)
 382 {
 383         cpBodyActivate(body);
 384         cpVect p = body->p = cpvadd(cpTransformVect(body->transform, body->cog), position);
 385         cpAssertSaneBody(body);
 386
 387         SetTransform(body, p, body->a);
 388 }
 389
 390 cpVect
 391 cpBodyGetCenterOfGravity(const cpBody *body)
 392 {
 393         return body->cog;
 394 }
 395
 396 void
 397 cpBodySetCenterOfGravity(cpBody *body, cpVect cog)
 398 {
 399         cpBodyActivate(body);
 400         body->cog = cog;
 401         cpAssertSaneBody(body);
 402 }
 403
 404 cpVect
 405 cpBodyGetVelocity(const cpBody *body)
 406 {
 407         return body->v;
 408 }
 409
 410 void
 411 cpBodySetVelocity(cpBody *body, cpVect velocity)
 412 {
 413         cpBodyActivate(body);
 414         body->v = velocity;
 415         cpAssertSaneBody(body);
 416 }
 417
 418 cpVect
 419 cpBodyGetForce(const cpBody *body)
 420 {
 421         return body->f;
 422 }
 423
 424 void
 425 cpBodySetForce(cpBody *body, cpVect force)
 426 {
 427         cpBodyActivate(body);
 428         body->f = force;
 429         cpAssertSaneBody(body);
 430 }
 431
 432 cpFloat
 433 cpBodyGetAngle(const cpBody *body)
 434 {
 435         return body->a;
 436 }
 437
 438 void
 439 cpBodySetAngle(cpBody *body, cpFloat angle)
 440 {
 441         cpBodyActivate(body);
 442         SetAngle(body, angle);
 443
 444         SetTransform(body, body->p, angle);
 445 }
 446
 447 cpFloat
 448 cpBodyGetAngularVelocity(const cpBody *body)
 449 {
 450         return body->w;
 451 }
 452
 453 void
 454 cpBodySetAngularVelocity(cpBody *body, cpFloat angularVelocity)
 455 {
 456         cpBodyActivate(body);
 457         body->w = angularVelocity;
 458         cpAssertSaneBody(body);
 459 }
 460
 461 cpFloat
 462 cpBodyGetTorque(const cpBody *body)
 463 {
 464         return body->t;
 465 }
 466
 467 void
 468 cpBodySetTorque(cpBody *body, cpFloat torque)
 469 {
 470         cpBodyActivate(body);
 471         body->t = torque;
 472         cpAssertSaneBody(body);
 473 }
 474
 475 cpDataPointer
 476 cpBodyGetUserData(const cpBody *body)
 477 {
 478         return body->userData;
 479 }
 480
 481 void
 482 cpBodySetUserData(cpBody *body, cpDataPointer userData)
 483 {
 484         body->userData = userData;
 485 }
 486
 487 void
 488 cpBodySetVelocityUpdateFunc(cpBody *body, cpBodyVelocityFunc velocityFunc)
 489 {
 490         body->velocity_func = velocityFunc;
 491 }
 492
 493 void
 494 cpBodySetPositionUpdateFunc(cpBody *body, cpBodyPositionFunc positionFunc)
 495 {
 496         body->position_func = positionFunc;
 497 }
 498
 499 void
 500 cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
 501 {
 502         // Skip kinematic bodies.
 503         if(cpBodyGetType(body) == CP_BODY_TYPE_KINEMATIC) return;
 504
 505         cpAssertSoft(body->m > 0.0f && body->i > 0.0f, "Body's mass and moment must be positive to simulate. (Mass: %f Moment: %f)", body->m, body->i);
 506
 507         body->v = cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt));
 508         body->w = body->w*damping + body->t*body->i_inv*dt;
 509
 510         // Reset forces.
 511         body->f = cpvzero;
 512         body->t = 0.0f;
 513
 514         cpAssertSaneBody(body);
 515 }
 516
 517 void
 518 cpBodyUpdatePosition(cpBody *body, cpFloat dt)
 519 {
 520         cpVect p = body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt));
 521         cpFloat a = SetAngle(body, body->a + (body->w + body->w_bias)*dt);
 522         SetTransform(body, p, a);
 523
 524         body->v_bias = cpvzero;
 525         body->w_bias = 0.0f;
 526
 527         cpAssertSaneBody(body);
 528 }
 529
 530 cpVect
 531 cpBodyLocalToWorld(const cpBody *body, const cpVect point)
 532 {
 533         return cpTransformPoint(body->transform, point);
 534 }
 535
 536 cpVect
 537 cpBodyWorldToLocal(const cpBody *body, const cpVect point)
 538 {
 539         return cpTransformPoint(cpTransformRigidInverse(body->transform), point);
 540 }
 541
 542 void
 543 cpBodyApplyForceAtWorldPoint(cpBody *body, cpVect force, cpVect point)
 544 {
 545         cpBodyActivate(body);
 546         body->f = cpvadd(body->f, force);
 547
 548         cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog));
 549         body->t += cpvcross(r, force);
 550 }
 551
 552 void
 553 cpBodyApplyForceAtLocalPoint(cpBody *body, cpVect force, cpVect point)
 554 {
 555         cpBodyApplyForceAtWorldPoint(body, cpTransformVect(body->transform, force), cpTransformPoint(body->transform, point));
 556 }
 557
 558 void
 559 cpBodyApplyImpulseAtWorldPoint(cpBody *body, cpVect impulse, cpVect point)
 560 {
 561         cpBodyActivate(body);
 562
 563         cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog));
 564         apply_impulse(body, impulse, r);
 565 }
 566
 567 void
 568 cpBodyApplyImpulseAtLocalPoint(cpBody *body, cpVect impulse, cpVect point)
 569 {
 570         cpBodyApplyImpulseAtWorldPoint(body, cpTransformVect(body->transform, impulse), cpTransformPoint(body->transform, point));
 571 }
 572
 573 cpVect
 574 cpBodyGetVelocityAtLocalPoint(const cpBody *body, cpVect point)
 575 {
 576         cpVect r = cpTransformVect(body->transform, cpvsub(point, body->cog));
 577         return cpvadd(body->v, cpvmult(cpvperp(r), body->w));
 578 }
 579
 580 cpVect
 581 cpBodyGetVelocityAtWorldPoint(const cpBody *body, cpVect point)
 582 {
 583         cpVect r = cpvsub(point, cpTransformPoint(body->transform, body->cog));
 584         return cpvadd(body->v, cpvmult(cpvperp(r), body->w));
 585 }
 586
 587 cpFloat
 588 cpBodyKineticEnergy(const cpBody *body)
 589 {
 590         // Need to do some fudging to avoid NaNs
 591         cpFloat vsq = cpvdot(body->v, body->v);
 592         cpFloat wsq = body->w*body->w;
 593         return (vsq ? vsq*body->m : 0.0f) + (wsq ? wsq*body->i : 0.0f);
 594 }
 595
 596 void
 597 cpBodyEachShape(cpBody *body, cpBodyShapeIteratorFunc func, void *data)
 598 {
 599         cpShape *shape = body->shapeList;
 600         while(shape){
 601                 cpShape *next = shape->next;
 602                 func(body, shape, data);
 603                 shape = next;
 604         }
 605 }
 606
 607 void
 608 cpBodyEachConstraint(cpBody *body, cpBodyConstraintIteratorFunc func, void *data)
 609 {
 610         cpConstraint *constraint = body->constraintList;
 611         while(constraint){
 612                 cpConstraint *next = cpConstraintNext(constraint, body);
 613                 func(body, constraint, data);
 614                 constraint = next;
 615         }
 616 }
 617
 618 void
 619 cpBodyEachArbiter(cpBody *body, cpBodyArbiterIteratorFunc func, void *data)
 620 {
 621         cpArbiter *arb = body->arbiterList;
 622         while(arb){
 623                 cpArbiter *next = cpArbiterNext(arb, body);
 624
 625                 cpBool swapped = arb->swapped; {
 626                         arb->swapped = (body == arb->body_b);
 627                         func(body, arb, data);
 628                 } arb->swapped = swapped;
 629
 630                 arb = next;
 631         }
 632 }