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