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