dali-physics/third-party/chipmunk2d/src/cpCollision.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 <stdio.h>
  23 #include <string.h>
  24
  25 #include "chipmunk/chipmunk_private.h"
  26 #include "chipmunk/cpRobust.h"
  27
  28 #if DEBUG && 0
  29 #include "ChipmunkDemo.h"
  30 #define DRAW_ALL 0
  31 #define DRAW_GJK (0 || DRAW_ALL)
  32 #define DRAW_EPA (0 || DRAW_ALL)
  33 #define DRAW_CLOSEST (0 || DRAW_ALL)
  34 #define DRAW_CLIP (0 || DRAW_ALL)
  35
  36 #define PRINT_LOG 0
  37 #endif
  38
  39 #define MAX_GJK_ITERATIONS 30
  40 #define MAX_EPA_ITERATIONS 30
  41 #define WARN_GJK_ITERATIONS 20
  42 #define WARN_EPA_ITERATIONS 20
  43
  44 static inline void
  45 cpCollisionInfoPushContact(struct cpCollisionInfo *info, cpVect p1, cpVect p2, cpHashValue hash)
  46 {
  47         cpAssertSoft(info->count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal error: Tried to push too many contacts.");
  48
  49         struct cpContact *con = &info->arr[info->count];
  50         con->r1 = p1;
  51         con->r2 = p2;
  52         con->hash = hash;
  53
  54         info->count++;
  55 }
  56
  57 //MARK: Support Points and Edges:
  58
  59 // Support points are the maximal points on a shape's perimeter along a certain axis.
  60 // The GJK and EPA algorithms use support points to iteratively sample the surface of the two shapes' minkowski difference.
  61
  62 static inline int
  63 PolySupportPointIndex(const int count, const struct cpSplittingPlane *planes, const cpVect n)
  64 {
  65         cpFloat max = -INFINITY;
  66         int index = 0;
  67
  68         for(int i=0; i<count; i++){
  69                 cpVect v = planes[i].v0;
  70                 cpFloat d = cpvdot(v, n);
  71                 if(d > max){
  72                         max = d;
  73                         index = i;
  74                 }
  75         }
  76
  77         return index;
  78 }
  79
  80 struct SupportPoint {
  81         cpVect p;
  82         // Save an index of the point so it can be cheaply looked up as a starting point for the next frame.
  83         cpCollisionID index;
  84 };
  85
  86 static inline struct SupportPoint
  87 SupportPointNew(cpVect p, cpCollisionID index)
  88 {
  89         struct SupportPoint point = {p, index};
  90         return point;
  91 }
  92
  93 typedef struct SupportPoint (*SupportPointFunc)(const cpShape *shape, const cpVect n);
  94
  95 static inline struct SupportPoint
  96 CircleSupportPoint(const cpCircleShape *circle, const cpVect n)
  97 {
  98         return SupportPointNew(circle->tc, 0);
  99 }
 100
 101 static inline struct SupportPoint
 102 SegmentSupportPoint(const cpSegmentShape *seg, const cpVect n)
 103 {
 104         if(cpvdot(seg->ta, n) > cpvdot(seg->tb, n)){
 105                 return SupportPointNew(seg->ta, 0);
 106         } else {
 107                 return SupportPointNew(seg->tb, 1);
 108         }
 109 }
 110
 111 static inline struct SupportPoint
 112 PolySupportPoint(const cpPolyShape *poly, const cpVect n)
 113 {
 114         const struct cpSplittingPlane *planes = poly->planes;
 115         int i = PolySupportPointIndex(poly->count, planes, n);
 116         return SupportPointNew(planes[i].v0, i);
 117 }
 118
 119 // A point on the surface of two shape's minkowski difference.
 120 struct MinkowskiPoint {
 121         // Cache the two original support points.
 122         cpVect a, b;
 123         // b - a
 124         cpVect ab;
 125         // Concatenate the two support point indexes.
 126         cpCollisionID id;
 127 };
 128
 129 static inline struct MinkowskiPoint
 130 MinkowskiPointNew(const struct SupportPoint a, const struct SupportPoint b)
 131 {
 132         struct MinkowskiPoint point = {a.p, b.p, cpvsub(b.p, a.p), (a.index & 0xFF)<<8 | (b.index & 0xFF)};
 133         return point;
 134 }
 135
 136 struct SupportContext {
 137         const cpShape *shape1, *shape2;
 138         SupportPointFunc func1, func2;
 139 };
 140
 141 // Calculate the maximal point on the minkowski difference of two shapes along a particular axis.
 142 static inline struct MinkowskiPoint
 143 Support(const struct SupportContext *ctx, const cpVect n)
 144 {
 145         struct SupportPoint a = ctx->func1(ctx->shape1, cpvneg(n));
 146         struct SupportPoint b = ctx->func2(ctx->shape2, n);
 147         return MinkowskiPointNew(a, b);
 148 }
 149
 150 struct EdgePoint {
 151         cpVect p;
 152         // Keep a hash value for Chipmunk's collision hashing mechanism.
 153         cpHashValue hash;
 154 };
 155
 156 // Support edges are the edges of a polygon or segment shape that are in contact.
 157 struct Edge {
 158         struct EdgePoint a, b;
 159         cpFloat r;
 160         cpVect n;
 161 };
 162
 163 static struct Edge
 164 SupportEdgeForPoly(const cpPolyShape *poly, const cpVect n)
 165 {
 166         int count = poly->count;
 167         int i1 = PolySupportPointIndex(poly->count, poly->planes, n);
 168
 169         // TODO: get rid of mod eventually, very expensive on ARM
 170         int i0 = (i1 - 1 + count)%count;
 171         int i2 = (i1 + 1)%count;
 172
 173         const struct cpSplittingPlane *planes = poly->planes;
 174         cpHashValue hashid = poly->shape.hashid;
 175         if(cpvdot(n, planes[i1].n) > cpvdot(n, planes[i2].n)){
 176                 struct Edge edge = {{planes[i0].v0, CP_HASH_PAIR(hashid, i0)}, {planes[i1].v0, CP_HASH_PAIR(hashid, i1)}, poly->r, planes[i1].n};
 177                 return edge;
 178         } else {
 179                 struct Edge edge = {{planes[i1].v0, CP_HASH_PAIR(hashid, i1)}, {planes[i2].v0, CP_HASH_PAIR(hashid, i2)}, poly->r, planes[i2].n};
 180                 return edge;
 181         }
 182 }
 183
 184 static struct Edge
 185 SupportEdgeForSegment(const cpSegmentShape *seg, const cpVect n)
 186 {
 187         cpHashValue hashid = seg->shape.hashid;
 188         if(cpvdot(seg->tn, n) > 0.0){
 189                 struct Edge edge = {{seg->ta, CP_HASH_PAIR(hashid, 0)}, {seg->tb, CP_HASH_PAIR(hashid, 1)}, seg->r, seg->tn};
 190                 return edge;
 191         } else {
 192                 struct Edge edge = {{seg->tb, CP_HASH_PAIR(hashid, 1)}, {seg->ta, CP_HASH_PAIR(hashid, 0)}, seg->r, cpvneg(seg->tn)};
 193                 return edge;
 194         }
 195 }
 196
 197 // Find the closest p(t) to (0, 0) where p(t) = a*(1-t)/2 + b*(1+t)/2
 198 // The range for t is [-1, 1] to avoid floating point issues if the parameters are swapped.
 199 static inline cpFloat
 200 ClosestT(const cpVect a, const cpVect b)
 201 {
 202         cpVect delta = cpvsub(b, a);
 203         return -cpfclamp(cpvdot(delta, cpvadd(a, b))/cpvlengthsq(delta), -1.0f, 1.0f);
 204 }
 205
 206 // Basically the same as cpvlerp(), except t = [-1, 1]
 207 static inline cpVect
 208 LerpT(const cpVect a, const cpVect b, const cpFloat t)
 209 {
 210         cpFloat ht = 0.5f*t;
 211         return cpvadd(cpvmult(a, 0.5f - ht), cpvmult(b, 0.5f + ht));
 212 }
 213
 214 // Closest points on the surface of two shapes.
 215 struct ClosestPoints {
 216         // Surface points in absolute coordinates.
 217         cpVect a, b;
 218         // Minimum separating axis of the two shapes.
 219         cpVect n;
 220         // Signed distance between the points.
 221         cpFloat d;
 222         // Concatenation of the id's of the minkoski points.
 223         cpCollisionID id;
 224 };
 225
 226 // Calculate the closest points on two shapes given the closest edge on their minkowski difference to (0, 0)
 227 static inline struct ClosestPoints
 228 ClosestPointsNew(const struct MinkowskiPoint v0, const struct MinkowskiPoint v1)
 229 {
 230         // Find the closest p(t) on the minkowski difference to (0, 0)
 231         cpFloat t = ClosestT(v0.ab, v1.ab);
 232         cpVect p = LerpT(v0.ab, v1.ab, t);
 233
 234         // Interpolate the original support points using the same 't' value as above.
 235         // This gives you the closest surface points in absolute coordinates. NEAT!
 236         cpVect pa = LerpT(v0.a, v1.a, t);
 237         cpVect pb = LerpT(v0.b, v1.b, t);
 238         cpCollisionID id = (v0.id & 0xFFFF)<<16 | (v1.id & 0xFFFF);
 239
 240         // First try calculating the MSA from the minkowski difference edge.
 241         // This gives us a nice, accurate MSA when the surfaces are close together.
 242         cpVect delta = cpvsub(v1.ab, v0.ab);
 243         cpVect n = cpvnormalize(cpvrperp(delta));
 244         cpFloat d = cpvdot(n, p);
 245
 246         if(d <= 0.0f || (-1.0f < t && t < 1.0f)){
 247                 // If the shapes are overlapping, or we have a regular vertex/edge collision, we are done.
 248                 struct ClosestPoints points = {pa, pb, n, d, id};
 249                 return points;
 250         } else {
 251                 // Vertex/vertex collisions need special treatment since the MSA won't be shared with an axis of the minkowski difference.
 252                 cpFloat d2 = cpvlength(p);
 253                 cpVect n2 = cpvmult(p, 1.0f/(d2 + CPFLOAT_MIN));
 254
 255                 struct ClosestPoints points = {pa, pb, n2, d2, id};
 256                 return points;
 257         }
 258 }
 259
 260 //MARK: EPA Functions
 261
 262 static inline cpFloat
 263 ClosestDist(const cpVect v0,const cpVect v1)
 264 {
 265         return cpvlengthsq(LerpT(v0, v1, ClosestT(v0, v1)));
 266 }
 267
 268 // Recursive implementation of the EPA loop.
 269 // Each recursion adds a point to the convex hull until it's known that we have the closest point on the surface.
 270 static struct ClosestPoints
 271 EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration)
 272 {
 273         int mini = 0;
 274         cpFloat minDist = INFINITY;
 275
 276         // TODO: precalculate this when building the hull and save a step.
 277         // Find the closest segment hull[i] and hull[i + 1] to (0, 0)
 278         for(int j=0, i=count-1; j<count; i=j, j++){
 279                 cpFloat d = ClosestDist(hull[i].ab, hull[j].ab);
 280                 if(d < minDist){
 281                         minDist = d;
 282                         mini = i;
 283                 }
 284         }
 285
 286         struct MinkowskiPoint v0 = hull[mini];
 287         struct MinkowskiPoint v1 = hull[(mini + 1)%count];
 288         cpAssertSoft(!cpveql(v0.ab, v1.ab), "Internal Error: EPA vertexes are the same (%d and %d)", mini, (mini + 1)%count);
 289
 290         // Check if there is a point on the minkowski difference beyond this edge.
 291         struct MinkowskiPoint p = Support(ctx, cpvperp(cpvsub(v1.ab, v0.ab)));
 292
 293 #if DRAW_EPA
 294         cpVect verts[count];
 295         for(int i=0; i<count; i++) verts[i] = hull[i].ab;
 296
 297         ChipmunkDebugDrawPolygon(count, verts, 0.0, RGBAColor(1, 1, 0, 1), RGBAColor(1, 1, 0, 0.25));
 298         ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 0, 0, 1));
 299
 300         ChipmunkDebugDrawDot(5, p.ab, LAColor(1, 1));
 301 #endif
 302
 303         // The usual exit condition is a duplicated vertex.
 304         // Much faster to check the ids than to check the signed area.
 305         cpBool duplicate = (p.id == v0.id || p.id == v1.id);
 306
 307         if(!duplicate && cpCheckPointGreater(v0.ab, v1.ab, p.ab) && iteration < MAX_EPA_ITERATIONS){
 308                 // Rebuild the convex hull by inserting p.
 309                 struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
 310                 int count2 = 1;
 311                 hull2[0] = p;
 312
 313                 for(int i=0; i<count; i++){
 314                         int index = (mini + 1 + i)%count;
 315
 316                         cpVect h0 = hull2[count2 - 1].ab;
 317                         cpVect h1 = hull[index].ab;
 318                         cpVect h2 = (i + 1 < count ? hull[(index + 1)%count] : p).ab;
 319
 320                         if(cpCheckPointGreater(h0, h2, h1)){
 321                                 hull2[count2] = hull[index];
 322                                 count2++;
 323                         }
 324                 }
 325
 326                 return EPARecurse(ctx, count2, hull2, iteration + 1);
 327         } else {
 328                 // Could not find a new point to insert, so we have found the closest edge of the minkowski difference.
 329                 cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
 330                 return ClosestPointsNew(v0, v1);
 331         }
 332 }
 333
 334 // Find the closest points on the surface of two overlapping shapes using the EPA algorithm.
 335 // EPA is called from GJK when two shapes overlap.
 336 // This is a moderately expensive step! Avoid it by adding radii to your shapes so their inner polygons won't overlap.
 337 static struct ClosestPoints
 338 EPA(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const struct MinkowskiPoint v2)
 339 {
 340         // TODO: allocate a NxM array here and do an in place convex hull reduction in EPARecurse?
 341         struct MinkowskiPoint hull[3] = {v0, v1, v2};
 342         return EPARecurse(ctx, 3, hull, 1);
 343 }
 344
 345 //MARK: GJK Functions.
 346
 347 // Recursive implementation of the GJK loop.
 348 static inline struct ClosestPoints
 349 GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
 350 {
 351         if(iteration > MAX_GJK_ITERATIONS){
 352                 cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
 353                 return ClosestPointsNew(v0, v1);
 354         }
 355
 356         if(cpCheckPointGreater(v1.ab, v0.ab, cpvzero)){
 357                 // Origin is behind axis. Flip and try again.
 358                 return GJKRecurse(ctx, v1, v0, iteration);
 359         } else {
 360                 cpFloat t = ClosestT(v0.ab, v1.ab);
 361                 cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(cpvsub(v1.ab, v0.ab)) : cpvneg(LerpT(v0.ab, v1.ab, t)));
 362                 struct MinkowskiPoint p = Support(ctx, n);
 363
 364 #if DRAW_GJK
 365                 ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
 366                 cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
 367                 ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
 368
 369                 ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
 370 #endif
 371
 372                 if(cpCheckPointGreater(p.ab, v0.ab, cpvzero) && cpCheckPointGreater(v1.ab, p.ab, cpvzero)){
 373                         // The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
 374                         cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
 375                         return EPA(ctx, v0, p, v1);
 376                 } else {
 377                         if(cpCheckAxis(v0.ab, v1.ab, p.ab, n)){
 378                                 // The edge v0, v1 that we already have is the closest to (0, 0) since p was not closer.
 379                                 cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
 380                                 return ClosestPointsNew(v0, v1);
 381                         } else {
 382                                 // p was closer to the origin than our existing edge.
 383                                 // Need to figure out which existing point to drop.
 384                                 if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){
 385                                         return GJKRecurse(ctx, v0, p, iteration + 1);
 386                                 } else {
 387                                         return GJKRecurse(ctx, p, v1, iteration + 1);
 388                                 }
 389                         }
 390                 }
 391         }
 392 }
 393
 394 // Get a SupportPoint from a cached shape and index.
 395 static struct SupportPoint
 396 ShapePoint(const cpShape *shape, const int i)
 397 {
 398         switch(shape->klass->type){
 399                 case CP_CIRCLE_SHAPE: {
 400                         return SupportPointNew(((cpCircleShape *)shape)->tc, 0);
 401                 } case CP_SEGMENT_SHAPE: {
 402                         cpSegmentShape *seg = (cpSegmentShape *)shape;
 403                         return SupportPointNew(i == 0 ? seg->ta : seg->tb, i);
 404                 } case CP_POLY_SHAPE: {
 405                         cpPolyShape *poly = (cpPolyShape *)shape;
 406                         // Poly shapes may change vertex count.
 407                         int index = (i < poly->count ? i : 0);
 408                         return SupportPointNew(poly->planes[index].v0, index);
 409                 } default: {
 410                         return SupportPointNew(cpvzero, 0);
 411                 }
 412         }
 413 }
 414
 415 // Find the closest points between two shapes using the GJK algorithm.
 416 static struct ClosestPoints
 417 GJK(const struct SupportContext *ctx, cpCollisionID *id)
 418 {
 419 #if DRAW_GJK || DRAW_EPA
 420         int count1 = 1;
 421         int count2 = 1;
 422
 423         switch(ctx->shape1->klass->type){
 424                 case CP_SEGMENT_SHAPE: count1 = 2; break;
 425                 case CP_POLY_SHAPE: count1 = ((cpPolyShape *)ctx->shape1)->count; break;
 426                 default: break;
 427         }
 428
 429         switch(ctx->shape2->klass->type){
 430                 case CP_SEGMENT_SHAPE: count1 = 2; break;
 431                 case CP_POLY_SHAPE: count2 = ((cpPolyShape *)ctx->shape2)->count; break;
 432                 default: break;
 433         }
 434
 435
 436         // draw the minkowski difference origin
 437         cpVect origin = cpvzero;
 438         ChipmunkDebugDrawDot(5.0, origin, RGBAColor(1,0,0,1));
 439
 440         int mdiffCount = count1*count2;
 441         cpVect *mdiffVerts = alloca(mdiffCount*sizeof(cpVect));
 442
 443         for(int i=0; i<count1; i++){
 444                 for(int j=0; j<count2; j++){
 445                         cpVect v = cpvsub(ShapePoint(ctx->shape2, j).p, ShapePoint(ctx->shape1, i).p);
 446                         mdiffVerts[i*count2 + j] = v;
 447                         ChipmunkDebugDrawDot(2.0, v, RGBAColor(1, 0, 0, 1));
 448                 }
 449         }
 450
 451         cpVect *hullVerts = alloca(mdiffCount*sizeof(cpVect));
 452         int hullCount = cpConvexHull(mdiffCount, mdiffVerts, hullVerts, NULL, 0.0);
 453
 454         ChipmunkDebugDrawPolygon(hullCount, hullVerts, 0.0, RGBAColor(1, 0, 0, 1), RGBAColor(1, 0, 0, 0.25));
 455 #endif
 456
 457         struct MinkowskiPoint v0, v1;
 458         if(*id){
 459                 // Use the minkowski points from the last frame as a starting point using the cached indexes.
 460                 v0 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>>24)&0xFF), ShapePoint(ctx->shape2, (*id>>16)&0xFF));
 461                 v1 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>> 8)&0xFF), ShapePoint(ctx->shape2, (*id    )&0xFF));
 462         } else {
 463                 // No cached indexes, use the shapes' bounding box centers as a guess for a starting axis.
 464                 cpVect axis = cpvperp(cpvsub(cpBBCenter(ctx->shape1->bb), cpBBCenter(ctx->shape2->bb)));
 465                 v0 = Support(ctx, axis);
 466                 v1 = Support(ctx, cpvneg(axis));
 467         }
 468
 469         struct ClosestPoints points = GJKRecurse(ctx, v0, v1, 1);
 470         *id = points.id;
 471         return points;
 472 }
 473
 474 //MARK: Contact Clipping
 475
 476 // Given two support edges, find contact point pairs on their surfaces.
 477 static inline void
 478 ContactPoints(const struct Edge e1, const struct Edge e2, const struct ClosestPoints points, struct cpCollisionInfo *info)
 479 {
 480         cpFloat mindist = e1.r + e2.r;
 481         if(points.d <= mindist){
 482 #ifdef DRAW_CLIP
 483         ChipmunkDebugDrawFatSegment(e1.a.p, e1.b.p, e1.r, RGBAColor(0, 1, 0, 1), LAColor(0, 0));
 484         ChipmunkDebugDrawFatSegment(e2.a.p, e2.b.p, e2.r, RGBAColor(1, 0, 0, 1), LAColor(0, 0));
 485 #endif
 486                 cpVect n = info->n = points.n;
 487
 488                 // Distances along the axis parallel to n
 489                 cpFloat d_e1_a = cpvcross(e1.a.p, n);
 490                 cpFloat d_e1_b = cpvcross(e1.b.p, n);
 491                 cpFloat d_e2_a = cpvcross(e2.a.p, n);
 492                 cpFloat d_e2_b = cpvcross(e2.b.p, n);
 493
 494                 // TODO + min isn't a complete fix.
 495                 cpFloat e1_denom = 1.0f/(d_e1_b - d_e1_a + CPFLOAT_MIN);
 496                 cpFloat e2_denom = 1.0f/(d_e2_b - d_e2_a + CPFLOAT_MIN);
 497
 498                 // Project the endpoints of the two edges onto the opposing edge, clamping them as necessary.
 499                 // Compare the projected points to the collision normal to see if the shapes overlap there.
 500                 {
 501                         cpVect p1 = cpvadd(cpvmult(n,  e1.r), cpvlerp(e1.a.p, e1.b.p, cpfclamp01((d_e2_b - d_e1_a)*e1_denom)));
 502                         cpVect p2 = cpvadd(cpvmult(n, -e2.r), cpvlerp(e2.a.p, e2.b.p, cpfclamp01((d_e1_a - d_e2_a)*e2_denom)));
 503                         cpFloat dist = cpvdot(cpvsub(p2, p1), n);
 504                         if(dist <= 0.0f){
 505                                 cpHashValue hash_1a2b = CP_HASH_PAIR(e1.a.hash, e2.b.hash);
 506                                 cpCollisionInfoPushContact(info, p1, p2, hash_1a2b);
 507                         }
 508                 }{
 509                         cpVect p1 = cpvadd(cpvmult(n,  e1.r), cpvlerp(e1.a.p, e1.b.p, cpfclamp01((d_e2_a - d_e1_a)*e1_denom)));
 510                         cpVect p2 = cpvadd(cpvmult(n, -e2.r), cpvlerp(e2.a.p, e2.b.p, cpfclamp01((d_e1_b - d_e2_a)*e2_denom)));
 511                         cpFloat dist = cpvdot(cpvsub(p2, p1), n);
 512                         if(dist <= 0.0f){
 513                                 cpHashValue hash_1b2a = CP_HASH_PAIR(e1.b.hash, e2.a.hash);
 514                                 cpCollisionInfoPushContact(info, p1, p2, hash_1b2a);
 515                         }
 516                 }
 517         }
 518 }
 519
 520 //MARK: Collision Functions
 521
 522 typedef void (*CollisionFunc)(const cpShape *a, const cpShape *b, struct cpCollisionInfo *info);
 523
 524 // Collide circle shapes.
 525 static void
 526 CircleToCircle(const cpCircleShape *c1, const cpCircleShape *c2, struct cpCollisionInfo *info)
 527 {
 528         cpFloat mindist = c1->r + c2->r;
 529         cpVect delta = cpvsub(c2->tc, c1->tc);
 530         cpFloat distsq = cpvlengthsq(delta);
 531
 532         if(distsq < mindist*mindist){
 533                 cpFloat dist = cpfsqrt(distsq);
 534                 cpVect n = info->n = (dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f));
 535                 cpCollisionInfoPushContact(info, cpvadd(c1->tc, cpvmult(n, c1->r)), cpvadd(c2->tc, cpvmult(n, -c2->r)), 0);
 536         }
 537 }
 538
 539 static void
 540 CircleToSegment(const cpCircleShape *circle, const cpSegmentShape *segment, struct cpCollisionInfo *info)
 541 {
 542         cpVect seg_a = segment->ta;
 543         cpVect seg_b = segment->tb;
 544         cpVect center = circle->tc;
 545
 546         // Find the closest point on the segment to the circle.
 547         cpVect seg_delta = cpvsub(seg_b, seg_a);
 548         cpFloat closest_t = cpfclamp01(cpvdot(seg_delta, cpvsub(center, seg_a))/cpvlengthsq(seg_delta));
 549         cpVect closest = cpvadd(seg_a, cpvmult(seg_delta, closest_t));
 550
 551         // Compare the radii of the two shapes to see if they are colliding.
 552         cpFloat mindist = circle->r + segment->r;
 553         cpVect delta = cpvsub(closest, center);
 554         cpFloat distsq = cpvlengthsq(delta);
 555         if(distsq < mindist*mindist){
 556                 cpFloat dist = cpfsqrt(distsq);
 557                 // Handle coincident shapes as gracefully as possible.
 558                 cpVect n = info->n = (dist ? cpvmult(delta, 1.0f/dist) : segment->tn);
 559
 560                 // Reject endcap collisions if tangents are provided.
 561                 cpVect rot = cpBodyGetRotation(segment->shape.body);
 562                 if(
 563                         (closest_t != 0.0f || cpvdot(n, cpvrotate(segment->a_tangent, rot)) >= 0.0) &&
 564                         (closest_t != 1.0f || cpvdot(n, cpvrotate(segment->b_tangent, rot)) >= 0.0)
 565                 ){
 566                         cpCollisionInfoPushContact(info, cpvadd(center, cpvmult(n, circle->r)), cpvadd(closest, cpvmult(n, -segment->r)), 0);
 567                 }
 568         }
 569 }
 570
 571 static void
 572 SegmentToSegment(const cpSegmentShape *seg1, const cpSegmentShape *seg2, struct cpCollisionInfo *info)
 573 {
 574         struct SupportContext context = {(cpShape *)seg1, (cpShape *)seg2, (SupportPointFunc)SegmentSupportPoint, (SupportPointFunc)SegmentSupportPoint};
 575         struct ClosestPoints points = GJK(&context, &info->id);
 576
 577 #if DRAW_CLOSEST
 578 #if PRINT_LOG
 579 //      ChipmunkDemoPrintString("Distance: %.2f\n", points.d);
 580 #endif
 581
 582         ChipmunkDebugDrawDot(6.0, points.a, RGBAColor(1, 1, 1, 1));
 583         ChipmunkDebugDrawDot(6.0, points.b, RGBAColor(1, 1, 1, 1));
 584         ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1));
 585         ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1));
 586 #endif
 587
 588         cpVect n = points.n;
 589         cpVect rot1 = cpBodyGetRotation(seg1->shape.body);
 590         cpVect rot2 = cpBodyGetRotation(seg2->shape.body);
 591
 592         // If the closest points are nearer than the sum of the radii...
 593         if(
 594                 points.d <= (seg1->r + seg2->r) && (
 595                         // Reject endcap collisions if tangents are provided.
 596                         (!cpveql(points.a, seg1->ta) || cpvdot(n, cpvrotate(seg1->a_tangent, rot1)) <= 0.0) &&
 597                         (!cpveql(points.a, seg1->tb) || cpvdot(n, cpvrotate(seg1->b_tangent, rot1)) <= 0.0) &&
 598                         (!cpveql(points.b, seg2->ta) || cpvdot(n, cpvrotate(seg2->a_tangent, rot2)) >= 0.0) &&
 599                         (!cpveql(points.b, seg2->tb) || cpvdot(n, cpvrotate(seg2->b_tangent, rot2)) >= 0.0)
 600                 )
 601         ){
 602                 ContactPoints(SupportEdgeForSegment(seg1, n), SupportEdgeForSegment(seg2, cpvneg(n)), points, info);
 603         }
 604 }
 605
 606 static void
 607 PolyToPoly(const cpPolyShape *poly1, const cpPolyShape *poly2, struct cpCollisionInfo *info)
 608 {
 609         struct SupportContext context = {(cpShape *)poly1, (cpShape *)poly2, (SupportPointFunc)PolySupportPoint, (SupportPointFunc)PolySupportPoint};
 610         struct ClosestPoints points = GJK(&context, &info->id);
 611
 612 #if DRAW_CLOSEST
 613 #if PRINT_LOG
 614 //      ChipmunkDemoPrintString("Distance: %.2f\n", points.d);
 615 #endif
 616
 617         ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1));
 618         ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1));
 619         ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1));
 620         ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1));
 621 #endif
 622
 623         // If the closest points are nearer than the sum of the radii...
 624         if(points.d - poly1->r - poly2->r <= 0.0){
 625                 ContactPoints(SupportEdgeForPoly(poly1, points.n), SupportEdgeForPoly(poly2, cpvneg(points.n)), points, info);
 626         }
 627 }
 628
 629 static void
 630 SegmentToPoly(const cpSegmentShape *seg, const cpPolyShape *poly, struct cpCollisionInfo *info)
 631 {
 632         struct SupportContext context = {(cpShape *)seg, (cpShape *)poly, (SupportPointFunc)SegmentSupportPoint, (SupportPointFunc)PolySupportPoint};
 633         struct ClosestPoints points = GJK(&context, &info->id);
 634
 635 #if DRAW_CLOSEST
 636 #if PRINT_LOG
 637 //      ChipmunkDemoPrintString("Distance: %.2f\n", points.d);
 638 #endif
 639
 640         ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1));
 641         ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1));
 642         ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1));
 643         ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1));
 644 #endif
 645
 646         cpVect n = points.n;
 647         cpVect rot = cpBodyGetRotation(seg->shape.body);
 648
 649         if(
 650                 // If the closest points are nearer than the sum of the radii...
 651                 points.d - seg->r - poly->r <= 0.0 && (
 652                         // Reject endcap collisions if tangents are provided.
 653                         (!cpveql(points.a, seg->ta) || cpvdot(n, cpvrotate(seg->a_tangent, rot)) <= 0.0) &&
 654                         (!cpveql(points.a, seg->tb) || cpvdot(n, cpvrotate(seg->b_tangent, rot)) <= 0.0)
 655                 )
 656         ){
 657                 ContactPoints(SupportEdgeForSegment(seg, n), SupportEdgeForPoly(poly, cpvneg(n)), points, info);
 658         }
 659 }
 660
 661 static void
 662 CircleToPoly(const cpCircleShape *circle, const cpPolyShape *poly, struct cpCollisionInfo *info)
 663 {
 664         struct SupportContext context = {(cpShape *)circle, (cpShape *)poly, (SupportPointFunc)CircleSupportPoint, (SupportPointFunc)PolySupportPoint};
 665         struct ClosestPoints points = GJK(&context, &info->id);
 666
 667 #if DRAW_CLOSEST
 668         ChipmunkDebugDrawDot(3.0, points.a, RGBAColor(1, 1, 1, 1));
 669         ChipmunkDebugDrawDot(3.0, points.b, RGBAColor(1, 1, 1, 1));
 670         ChipmunkDebugDrawSegment(points.a, points.b, RGBAColor(1, 1, 1, 1));
 671         ChipmunkDebugDrawSegment(points.a, cpvadd(points.a, cpvmult(points.n, 10.0)), RGBAColor(1, 0, 0, 1));
 672 #endif
 673
 674         // If the closest points are nearer than the sum of the radii...
 675         if(points.d <= circle->r + poly->r){
 676                 cpVect n = info->n = points.n;
 677                 cpCollisionInfoPushContact(info, cpvadd(points.a, cpvmult(n, circle->r)), cpvadd(points.b, cpvmult(n, poly->r)), 0);
 678         }
 679 }
 680
 681 static void
 682 CollisionError(const cpShape *circle, const cpShape *poly, struct cpCollisionInfo *info)
 683 {
 684         cpAssertHard(cpFalse, "Internal Error: Shape types are not sorted.");
 685 }
 686
 687
 688 static const CollisionFunc BuiltinCollisionFuncs[9] = {
 689         (CollisionFunc)CircleToCircle,
 690         CollisionError,
 691         CollisionError,
 692         (CollisionFunc)CircleToSegment,
 693         (CollisionFunc)SegmentToSegment,
 694         CollisionError,
 695         (CollisionFunc)CircleToPoly,
 696         (CollisionFunc)SegmentToPoly,
 697         (CollisionFunc)PolyToPoly,
 698 };
 699 static const CollisionFunc *CollisionFuncs = BuiltinCollisionFuncs;
 700
 701 struct cpCollisionInfo
 702 cpCollide(const cpShape *a, const cpShape *b, cpCollisionID id, struct cpContact *contacts)
 703 {
 704         struct cpCollisionInfo info = {a, b, id, cpvzero, 0, contacts};
 705
 706         // Make sure the shape types are in order.
 707         if(a->klass->type > b->klass->type){
 708                 info.a = b;
 709                 info.b = a;
 710         }
 711
 712         CollisionFuncs[info.a->klass->type + info.b->klass->type*CP_NUM_SHAPES](info.a, info.b, &info);
 713
 714 //      if(0){
 715 //              for(int i=0; i<info.count; i++){
 716 //                      cpVect r1 = info.arr[i].r1;
 717 //                      cpVect r2 = info.arr[i].r2;
 718 //                      cpVect mid = cpvlerp(r1, r2, 0.5f);
 719 //
 720 //                      ChipmunkDebugDrawSegment(r1, mid, RGBAColor(1, 0, 0, 1));
 721 //                      ChipmunkDebugDrawSegment(r2, mid, RGBAColor(0, 0, 1, 1));
 722 //              }
 723 //      }
 724
 725         return info;
 726 }