dali-physics/third-party/chipmunk2d/src/cpShape.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 #include "chipmunk/chipmunk_unsafe.h"
  24
  25 #define CP_DefineShapeGetter(struct, type, member, name) \
  26 CP_DeclareShapeGetter(struct, type, name){ \
  27         cpAssertHard(shape->klass == &struct##Class, "shape is not a "#struct); \
  28         return ((struct *)shape)->member; \
  29 }
  30
  31 cpShape *
  32 cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body, struct cpShapeMassInfo massInfo)
  33 {
  34         shape->klass = klass;
  35
  36         shape->body = body;
  37         shape->massInfo = massInfo;
  38
  39         shape->sensor = 0;
  40
  41         shape->e = 0.0f;
  42         shape->u = 0.0f;
  43         shape->surfaceV = cpvzero;
  44
  45         shape->type = 0;
  46         shape->filter.group = CP_NO_GROUP;
  47         shape->filter.categories = CP_ALL_CATEGORIES;
  48         shape->filter.mask = CP_ALL_CATEGORIES;
  49
  50         shape->userData = NULL;
  51
  52         shape->space = NULL;
  53
  54         shape->next = NULL;
  55         shape->prev = NULL;
  56
  57         return shape;
  58 }
  59
  60 void
  61 cpShapeDestroy(cpShape *shape)
  62 {
  63         if(shape->klass && shape->klass->destroy) shape->klass->destroy(shape);
  64 }
  65
  66 void
  67 cpShapeFree(cpShape *shape)
  68 {
  69         if(shape){
  70                 cpShapeDestroy(shape);
  71                 cpfree(shape);
  72         }
  73 }
  74
  75 cpSpace *
  76 cpShapeGetSpace(const cpShape *shape)
  77 {
  78         return shape->space;
  79 }
  80
  81 cpBody *
  82 cpShapeGetBody(const cpShape *shape)
  83 {
  84         return shape->body;
  85 }
  86
  87 void
  88 cpShapeSetBody(cpShape *shape, cpBody *body)
  89 {
  90         cpAssertHard(!cpShapeActive(shape), "You cannot change the body on an active shape. You must remove the shape from the space before changing the body.");
  91         shape->body = body;
  92 }
  93
  94 cpFloat cpShapeGetMass(cpShape *shape){ return shape->massInfo.m; }
  95
  96 void
  97 cpShapeSetMass(cpShape *shape, cpFloat mass){
  98         cpBody *body = shape->body;
  99         cpBodyActivate(body);
 100
 101         shape->massInfo.m = mass;
 102         cpBodyAccumulateMassFromShapes(body);
 103 }
 104
 105 cpFloat cpShapeGetDensity(cpShape *shape){ return shape->massInfo.m/shape->massInfo.area; }
 106 void cpShapeSetDensity(cpShape *shape, cpFloat density){ cpShapeSetMass(shape, density*shape->massInfo.area); }
 107
 108 cpFloat cpShapeGetMoment(cpShape *shape){ return shape->massInfo.m*shape->massInfo.i; }
 109 cpFloat cpShapeGetArea(cpShape *shape){ return shape->massInfo.area; }
 110 cpVect cpShapeGetCenterOfGravity(cpShape *shape) { return shape->massInfo.cog; }
 111
 112 cpBB
 113 cpShapeGetBB(const cpShape *shape)
 114 {
 115         return shape->bb;
 116 }
 117
 118 cpBool
 119 cpShapeGetSensor(const cpShape *shape)
 120 {
 121         return shape->sensor;
 122 }
 123
 124 void
 125 cpShapeSetSensor(cpShape *shape, cpBool sensor)
 126 {
 127         cpBodyActivate(shape->body);
 128         shape->sensor = sensor;
 129 }
 130
 131 cpFloat
 132 cpShapeGetElasticity(const cpShape *shape)
 133 {
 134         return shape->e;
 135 }
 136
 137 void
 138 cpShapeSetElasticity(cpShape *shape, cpFloat elasticity)
 139 {
 140         cpAssertHard(elasticity >= 0.0f, "Elasticity must be positive.");
 141         cpBodyActivate(shape->body);
 142         shape->e = elasticity;
 143 }
 144
 145 cpFloat
 146 cpShapeGetFriction(const cpShape *shape)
 147 {
 148         return shape->u;
 149 }
 150
 151 void
 152 cpShapeSetFriction(cpShape *shape, cpFloat friction)
 153 {
 154         cpAssertHard(friction >= 0.0f, "Friction must be postive.");
 155         cpBodyActivate(shape->body);
 156         shape->u = friction;
 157 }
 158
 159 cpVect
 160 cpShapeGetSurfaceVelocity(const cpShape *shape)
 161 {
 162         return shape->surfaceV;
 163 }
 164
 165 void
 166 cpShapeSetSurfaceVelocity(cpShape *shape, cpVect surfaceVelocity)
 167 {
 168         cpBodyActivate(shape->body);
 169         shape->surfaceV = surfaceVelocity;
 170 }
 171
 172 cpDataPointer
 173 cpShapeGetUserData(const cpShape *shape)
 174 {
 175         return shape->userData;
 176 }
 177
 178 void
 179 cpShapeSetUserData(cpShape *shape, cpDataPointer userData)
 180 {
 181         shape->userData = userData;
 182 }
 183
 184 cpCollisionType
 185 cpShapeGetCollisionType(const cpShape *shape)
 186 {
 187         return shape->type;
 188 }
 189
 190 void
 191 cpShapeSetCollisionType(cpShape *shape, cpCollisionType collisionType)
 192 {
 193         cpBodyActivate(shape->body);
 194         shape->type = collisionType;
 195 }
 196
 197 cpShapeFilter
 198 cpShapeGetFilter(const cpShape *shape)
 199 {
 200         return shape->filter;
 201 }
 202
 203 void
 204 cpShapeSetFilter(cpShape *shape, cpShapeFilter filter)
 205 {
 206         cpBodyActivate(shape->body);
 207         shape->filter = filter;
 208 }
 209
 210 cpBB
 211 cpShapeCacheBB(cpShape *shape)
 212 {
 213         return cpShapeUpdate(shape, shape->body->transform);
 214 }
 215
 216 cpBB
 217 cpShapeUpdate(cpShape *shape, cpTransform transform)
 218 {
 219         return (shape->bb = shape->klass->cacheData(shape, transform));
 220 }
 221
 222 cpFloat
 223 cpShapePointQuery(const cpShape *shape, cpVect p, cpPointQueryInfo *info)
 224 {
 225         cpPointQueryInfo blank = {NULL, cpvzero, INFINITY, cpvzero};
 226         if(info){
 227                 (*info) = blank;
 228         } else {
 229                 info = &blank;
 230         }
 231
 232         shape->klass->pointQuery(shape, p, info);
 233         return info->distance;
 234 }
 235
 236
 237 cpBool
 238 cpShapeSegmentQuery(const cpShape *shape, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info){
 239         cpSegmentQueryInfo blank = {NULL, b, cpvzero, 1.0f};
 240         if(info){
 241                 (*info) = blank;
 242         } else {
 243                 info = &blank;
 244         }
 245
 246         cpPointQueryInfo nearest;
 247         shape->klass->pointQuery(shape, a, &nearest);
 248         if(nearest.distance <= radius){
 249                 info->shape = shape;
 250                 info->alpha = 0.0;
 251                 info->normal = cpvnormalize(cpvsub(a, nearest.point));
 252         } else {
 253                 shape->klass->segmentQuery(shape, a, b, radius, info);
 254         }
 255
 256         return (info->shape != NULL);
 257 }
 258
 259 cpContactPointSet
 260 cpShapesCollide(const cpShape *a, const cpShape *b)
 261 {
 262         struct cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER];
 263         struct cpCollisionInfo info = cpCollide(a, b, 0, contacts);
 264
 265         cpContactPointSet set;
 266         set.count = info.count;
 267
 268         // cpCollideShapes() may have swapped the contact order. Flip the normal.
 269         cpBool swapped = (a != info.a);
 270         set.normal = (swapped ? cpvneg(info.n) : info.n);
 271
 272         for(int i=0; i<info.count; i++){
 273                 // cpCollideShapesInfo() returns contacts with absolute positions.
 274                 cpVect p1 = contacts[i].r1;
 275                 cpVect p2 = contacts[i].r2;
 276
 277                 set.points[i].pointA = (swapped ? p2 : p1);
 278                 set.points[i].pointB = (swapped ? p1 : p2);
 279                 set.points[i].distance = cpvdot(cpvsub(p2, p1), set.normal);
 280         }
 281
 282         return set;
 283 }
 284
 285 cpCircleShape *
 286 cpCircleShapeAlloc(void)
 287 {
 288         return (cpCircleShape *)cpcalloc(1, sizeof(cpCircleShape));
 289 }
 290
 291 static cpBB
 292 cpCircleShapeCacheData(cpCircleShape *circle, cpTransform transform)
 293 {
 294         cpVect c = circle->tc = cpTransformPoint(transform, circle->c);
 295         return cpBBNewForCircle(c, circle->r);
 296 }
 297
 298 static void
 299 cpCircleShapePointQuery(cpCircleShape *circle, cpVect p, cpPointQueryInfo *info)
 300 {
 301         cpVect delta = cpvsub(p, circle->tc);
 302         cpFloat d = cpvlength(delta);
 303         cpFloat r = circle->r;
 304
 305         info->shape = (cpShape *)circle;
 306         cpFloat r_over_d = d > 0.0f ? r/d : r;
 307         info->point = cpvadd(circle->tc, cpvmult(delta, r_over_d)); // TODO: div/0
 308         info->distance = d - r;
 309
 310         // Use up for the gradient if the distance is very small.
 311         info->gradient = (d > MAGIC_EPSILON ? cpvmult(delta, 1.0f/d) : cpv(0.0f, 1.0f));
 312 }
 313
 314 static void
 315 cpCircleShapeSegmentQuery(cpCircleShape *circle, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info)
 316 {
 317         CircleSegmentQuery((cpShape *)circle, circle->tc, circle->r, a, b, radius, info);
 318 }
 319
 320 static struct cpShapeMassInfo
 321 cpCircleShapeMassInfo(cpFloat mass, cpFloat radius, cpVect center)
 322 {
 323         struct cpShapeMassInfo info = {
 324                 mass, cpMomentForCircle(1.0f, 0.0f, radius, cpvzero),
 325                 center,
 326                 cpAreaForCircle(0.0f, radius),
 327         };
 328
 329         return info;
 330 }
 331
 332 static const cpShapeClass cpCircleShapeClass = {
 333         CP_CIRCLE_SHAPE,
 334         (cpShapeCacheDataImpl)cpCircleShapeCacheData,
 335         NULL,
 336         (cpShapePointQueryImpl)cpCircleShapePointQuery,
 337         (cpShapeSegmentQueryImpl)cpCircleShapeSegmentQuery,
 338 };
 339
 340 cpCircleShape *
 341 cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset)
 342 {
 343         circle->c = offset;
 344         circle->r = radius;
 345
 346         cpShapeInit((cpShape *)circle, &cpCircleShapeClass, body, cpCircleShapeMassInfo(0.0f, radius, offset));
 347
 348         return circle;
 349 }
 350
 351 cpShape *
 352 cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset)
 353 {
 354         return (cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), body, radius, offset);
 355 }
 356
 357 cpVect
 358 cpCircleShapeGetOffset(const cpShape *shape)
 359 {
 360         cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
 361         return ((cpCircleShape *)shape)->c;
 362 }
 363
 364 cpFloat
 365 cpCircleShapeGetRadius(const cpShape *shape)
 366 {
 367         cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
 368         return ((cpCircleShape *)shape)->r;
 369 }
 370
 371
 372 cpSegmentShape *
 373 cpSegmentShapeAlloc(void)
 374 {
 375         return (cpSegmentShape *)cpcalloc(1, sizeof(cpSegmentShape));
 376 }
 377
 378 static cpBB
 379 cpSegmentShapeCacheData(cpSegmentShape *seg, cpTransform transform)
 380 {
 381         seg->ta = cpTransformPoint(transform, seg->a);
 382         seg->tb = cpTransformPoint(transform, seg->b);
 383         seg->tn = cpTransformVect(transform, seg->n);
 384
 385         cpFloat l,r,b,t;
 386
 387         if(seg->ta.x < seg->tb.x){
 388                 l = seg->ta.x;
 389                 r = seg->tb.x;
 390         } else {
 391                 l = seg->tb.x;
 392                 r = seg->ta.x;
 393         }
 394
 395         if(seg->ta.y < seg->tb.y){
 396                 b = seg->ta.y;
 397                 t = seg->tb.y;
 398         } else {
 399                 b = seg->tb.y;
 400                 t = seg->ta.y;
 401         }
 402
 403         cpFloat rad = seg->r;
 404         return cpBBNew(l - rad, b - rad, r + rad, t + rad);
 405 }
 406
 407 static void
 408 cpSegmentShapePointQuery(cpSegmentShape *seg, cpVect p, cpPointQueryInfo *info)
 409 {
 410         cpVect closest = cpClosetPointOnSegment(p, seg->ta, seg->tb);
 411
 412         cpVect delta = cpvsub(p, closest);
 413         cpFloat d = cpvlength(delta);
 414         cpFloat r = seg->r;
 415         cpVect g = cpvmult(delta, 1.0f/d);
 416
 417         info->shape = (cpShape *)seg;
 418         info->point = (d ? cpvadd(closest, cpvmult(g, r)) : closest);
 419         info->distance = d - r;
 420
 421         // Use the segment's normal if the distance is very small.
 422         info->gradient = (d > MAGIC_EPSILON ? g : seg->n);
 423 }
 424
 425 static void
 426 cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info)
 427 {
 428         cpVect n = seg->tn;
 429         cpFloat d = cpvdot(cpvsub(seg->ta, a), n);
 430         cpFloat r = seg->r + r2;
 431
 432         cpVect flipped_n = (d > 0.0f ? cpvneg(n) : n);
 433         cpVect seg_offset = cpvsub(cpvmult(flipped_n, r), a);
 434
 435         // Make the endpoints relative to 'a' and move them by the thickness of the segment.
 436         cpVect seg_a = cpvadd(seg->ta, seg_offset);
 437         cpVect seg_b = cpvadd(seg->tb, seg_offset);
 438         cpVect delta = cpvsub(b, a);
 439
 440         if(cpvcross(delta, seg_a)*cpvcross(delta, seg_b) <= 0.0f){
 441                 cpFloat d_offset = d + (d > 0.0f ? -r : r);
 442                 cpFloat ad = -d_offset;
 443                 cpFloat bd = cpvdot(delta, n) - d_offset;
 444
 445                 if(ad*bd < 0.0f){
 446                         cpFloat t = ad/(ad - bd);
 447
 448                         info->shape = (cpShape *)seg;
 449                         info->point = cpvsub(cpvlerp(a, b, t), cpvmult(flipped_n, r2));
 450                         info->normal = flipped_n;
 451                         info->alpha = t;
 452                 }
 453         } else if(r != 0.0f){
 454                 cpSegmentQueryInfo info1 = {NULL, b, cpvzero, 1.0f};
 455                 cpSegmentQueryInfo info2 = {NULL, b, cpvzero, 1.0f};
 456                 CircleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, r2, &info1);
 457                 CircleSegmentQuery((cpShape *)seg, seg->tb, seg->r, a, b, r2, &info2);
 458
 459                 if(info1.alpha < info2.alpha){
 460                         (*info) = info1;
 461                 } else {
 462                         (*info) = info2;
 463                 }
 464         }
 465 }
 466
 467 static struct cpShapeMassInfo
 468 cpSegmentShapeMassInfo(cpFloat mass, cpVect a, cpVect b, cpFloat r)
 469 {
 470         struct cpShapeMassInfo info = {
 471                 mass, cpMomentForBox(1.0f, cpvdist(a, b) + 2.0f*r, 2.0f*r), // TODO is an approximation.
 472                 cpvlerp(a, b, 0.5f),
 473                 cpAreaForSegment(a, b, r),
 474         };
 475
 476         return info;
 477 }
 478
 479 static const cpShapeClass cpSegmentShapeClass = {
 480         CP_SEGMENT_SHAPE,
 481         (cpShapeCacheDataImpl)cpSegmentShapeCacheData,
 482         NULL,
 483         (cpShapePointQueryImpl)cpSegmentShapePointQuery,
 484         (cpShapeSegmentQueryImpl)cpSegmentShapeSegmentQuery,
 485 };
 486
 487 cpSegmentShape *
 488 cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat r)
 489 {
 490         seg->a = a;
 491         seg->b = b;
 492         seg->n = cpvrperp(cpvnormalize(cpvsub(b, a)));
 493
 494         seg->r = r;
 495
 496         seg->a_tangent = cpvzero;
 497         seg->b_tangent = cpvzero;
 498
 499         cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body, cpSegmentShapeMassInfo(0.0f, a, b, r));
 500
 501         return seg;
 502 }
 503
 504 cpShape*
 505 cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r)
 506 {
 507         return (cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), body, a, b, r);
 508 }
 509
 510 cpVect
 511 cpSegmentShapeGetA(const cpShape *shape)
 512 {
 513         cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
 514         return ((cpSegmentShape *)shape)->a;
 515 }
 516
 517 cpVect
 518 cpSegmentShapeGetB(const cpShape *shape)
 519 {
 520         cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
 521         return ((cpSegmentShape *)shape)->b;
 522 }
 523
 524 cpVect
 525 cpSegmentShapeGetNormal(const cpShape *shape)
 526 {
 527         cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
 528         return ((cpSegmentShape *)shape)->n;
 529 }
 530
 531 cpFloat
 532 cpSegmentShapeGetRadius(const cpShape *shape)
 533 {
 534         cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
 535         return ((cpSegmentShape *)shape)->r;
 536 }
 537
 538 void
 539 cpSegmentShapeSetNeighbors(cpShape *shape, cpVect prev, cpVect next)
 540 {
 541         cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
 542         cpSegmentShape *seg = (cpSegmentShape *)shape;
 543
 544         seg->a_tangent = cpvsub(prev, seg->a);
 545         seg->b_tangent = cpvsub(next, seg->b);
 546 }
 547
 548 // Unsafe API (chipmunk_unsafe.h)
 549
 550 // TODO setters should wake the shape up?
 551
 552 void
 553 cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
 554 {
 555         cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
 556         cpCircleShape *circle = (cpCircleShape *)shape;
 557
 558         circle->r = radius;
 559
 560         cpFloat mass = shape->massInfo.m;
 561         shape->massInfo = cpCircleShapeMassInfo(mass, circle->r, circle->c);
 562         if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
 563 }
 564
 565 void
 566 cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
 567 {
 568         cpAssertHard(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
 569         cpCircleShape *circle = (cpCircleShape *)shape;
 570
 571         circle->c = offset;
 572
 573         cpFloat mass = shape->massInfo.m;
 574         shape->massInfo = cpCircleShapeMassInfo(shape->massInfo.m, circle->r, circle->c);
 575         if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
 576 }
 577
 578 void
 579 cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
 580 {
 581         cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
 582         cpSegmentShape *seg = (cpSegmentShape *)shape;
 583
 584         seg->a = a;
 585         seg->b = b;
 586         seg->n = cpvperp(cpvnormalize(cpvsub(b, a)));
 587
 588         cpFloat mass = shape->massInfo.m;
 589         shape->massInfo = cpSegmentShapeMassInfo(shape->massInfo.m, seg->a, seg->b, seg->r);
 590         if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
 591 }
 592
 593 void
 594 cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
 595 {
 596         cpAssertHard(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
 597         cpSegmentShape *seg = (cpSegmentShape *)shape;
 598
 599         seg->r = radius;
 600
 601         cpFloat mass = shape->massInfo.m;
 602         shape->massInfo = cpSegmentShapeMassInfo(shape->massInfo.m, seg->a, seg->b, seg->r);
 603         if(mass > 0.0f) cpBodyAccumulateMassFromShapes(shape->body);
 604 }