dali-physics/third-party/chipmunk2d/src/chipmunk.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 #include <stdarg.h>
  25 #if defined(ANDROID)
  26 #       include <android/log.h>
  27 #endif
  28
  29 #include "chipmunk/chipmunk_private.h"
  30
  31 void
  32 cpMessage(const char *condition, const char *file, int line, int isError, int isHardError, const char *message, ...)
  33 {
  34         fprintf(stderr, (isError ? "Aborting due to Chipmunk error: " : "Chipmunk warning: "));
  35
  36         va_list vargs;
  37         va_start(vargs, message); {
  38 #if defined(ANDROID)
  39                 __android_log_print( ANDROID_LOG_INFO, "Chipmunk", "%s(%d)", file, line );
  40                 __android_log_print( ANDROID_LOG_INFO, "Chipmunk", message, vargs );
  41 #else
  42                 vfprintf(stderr, message, vargs);
  43                 fprintf(stderr, "\n");
  44 #endif
  45         } va_end(vargs);
  46
  47 #if defined(ANDROID)
  48         __android_log_print(ANDROID_LOG_INFO, "Chipmunk", "\tFailed condition: %s\n", condition);
  49         __android_log_print(ANDROID_LOG_INFO, "Chipmunk", "\tSource:%s:%d\n", file, line);
  50 #else
  51         fprintf(stderr, "\tFailed condition: %s\n", condition);
  52         fprintf(stderr, "\tSource:%s:%d\n", file, line);
  53 #endif
  54 }
  55
  56 #define STR(s) #s
  57 #define XSTR(s) STR(s)
  58
  59 const char *cpVersionString = XSTR(CP_VERSION_MAJOR) "." XSTR(CP_VERSION_MINOR) "." XSTR(CP_VERSION_RELEASE);
  60
  61 //MARK: Misc Functions
  62
  63 cpFloat
  64 cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset)
  65 {
  66         return m*(0.5f*(r1*r1 + r2*r2) + cpvlengthsq(offset));
  67 }
  68
  69 cpFloat
  70 cpAreaForCircle(cpFloat r1, cpFloat r2)
  71 {
  72         return (cpFloat)CP_PI*cpfabs(r1*r1 - r2*r2);
  73 }
  74
  75 cpFloat
  76 cpMomentForSegment(cpFloat m, cpVect a, cpVect b, cpFloat r)
  77 {
  78         cpVect offset = cpvlerp(a, b, 0.5f);
  79
  80         // This approximates the shape as a box for rounded segments, but it's quite close.
  81         cpFloat length = cpvdist(b, a) + 2.0f*r;
  82         return m*((length*length + 4.0f*r*r)/12.0f + cpvlengthsq(offset));
  83 }
  84
  85 cpFloat
  86 cpAreaForSegment(cpVect a, cpVect b, cpFloat r)
  87 {
  88         return r*((cpFloat)CP_PI*r + 2.0f*cpvdist(a, b));
  89 }
  90
  91 cpFloat
  92 cpMomentForPoly(cpFloat m, int count, const cpVect *verts, cpVect offset, cpFloat r)
  93 {
  94         // TODO account for radius.
  95         if(count == 2) return cpMomentForSegment(m, verts[0], verts[1], 0.0f);
  96
  97         cpFloat sum1 = 0.0f;
  98         cpFloat sum2 = 0.0f;
  99         for(int i=0; i<count; i++){
 100                 cpVect v1 = cpvadd(verts[i], offset);
 101                 cpVect v2 = cpvadd(verts[(i+1)%count], offset);
 102
 103                 cpFloat a = cpvcross(v2, v1);
 104                 cpFloat b = cpvdot(v1, v1) + cpvdot(v1, v2) + cpvdot(v2, v2);
 105
 106                 sum1 += a*b;
 107                 sum2 += a;
 108         }
 109
 110         return (m*sum1)/(6.0f*sum2);
 111 }
 112
 113 cpFloat
 114 cpAreaForPoly(const int count, const cpVect *verts, cpFloat r)
 115 {
 116         cpFloat area = 0.0f;
 117         cpFloat perimeter = 0.0f;
 118         for(int i=0; i<count; i++){
 119                 cpVect v1 = verts[i];
 120                 cpVect v2 = verts[(i+1)%count];
 121
 122                 area += cpvcross(v1, v2);
 123                 perimeter += cpvdist(v1, v2);
 124         }
 125
 126         return r*(CP_PI*cpfabs(r) + perimeter) + area/2.0f;
 127 }
 128
 129 cpVect
 130 cpCentroidForPoly(const int count, const cpVect *verts)
 131 {
 132         cpFloat sum = 0.0f;
 133         cpVect vsum = cpvzero;
 134
 135         for(int i=0; i<count; i++){
 136                 cpVect v1 = verts[i];
 137                 cpVect v2 = verts[(i+1)%count];
 138                 cpFloat cross = cpvcross(v1, v2);
 139
 140                 sum += cross;
 141                 vsum = cpvadd(vsum, cpvmult(cpvadd(v1, v2), cross));
 142         }
 143
 144         return cpvmult(vsum, 1.0f/(3.0f*sum));
 145 }
 146
 147 //void
 148 //cpRecenterPoly(const int count, cpVect *verts){
 149 //      cpVect centroid = cpCentroidForPoly(count, verts);
 150 //
 151 //      for(int i=0; i<count; i++){
 152 //              verts[i] = cpvsub(verts[i], centroid);
 153 //      }
 154 //}
 155
 156 cpFloat
 157 cpMomentForBox(cpFloat m, cpFloat width, cpFloat height)
 158 {
 159         return m*(width*width + height*height)/12.0f;
 160 }
 161
 162 cpFloat
 163 cpMomentForBox2(cpFloat m, cpBB box)
 164 {
 165         cpFloat width = box.r - box.l;
 166         cpFloat height = box.t - box.b;
 167         cpVect offset = cpvmult(cpv(box.l + box.r, box.b + box.t), 0.5f);
 168
 169         // TODO: NaN when offset is 0 and m is INFINITY
 170         return cpMomentForBox(m, width, height) + m*cpvlengthsq(offset);
 171 }
 172
 173 //MARK: Quick Hull
 174
 175 void
 176 cpLoopIndexes(const cpVect *verts, int count, int *start, int *end)
 177 {
 178         (*start) = (*end) = 0;
 179         cpVect min = verts[0];
 180         cpVect max = min;
 181
 182   for(int i=1; i<count; i++){
 183     cpVect v = verts[i];
 184
 185     if(v.x < min.x || (v.x == min.x && v.y < min.y)){
 186       min = v;
 187       (*start) = i;
 188     } else if(v.x > max.x || (v.x == max.x && v.y > max.y)){
 189                         max = v;
 190                         (*end) = i;
 191                 }
 192         }
 193 }
 194
 195 #define SWAP(__A__, __B__) {cpVect __TMP__ = __A__; __A__ = __B__; __B__ = __TMP__;}
 196
 197 static int
 198 QHullPartition(cpVect *verts, int count, cpVect a, cpVect b, cpFloat tol)
 199 {
 200         if(count == 0) return 0;
 201
 202         cpFloat max = 0;
 203         int pivot = 0;
 204
 205         cpVect delta = cpvsub(b, a);
 206         cpFloat valueTol = tol*cpvlength(delta);
 207
 208         int head = 0;
 209         for(int tail = count-1; head <= tail;){
 210                 cpFloat value = cpvcross(cpvsub(verts[head], a), delta);
 211                 if(value > valueTol){
 212                         if(value > max){
 213                                 max = value;
 214                                 pivot = head;
 215                         }
 216
 217                         head++;
 218                 } else {
 219                         SWAP(verts[head], verts[tail]);
 220                         tail--;
 221                 }
 222         }
 223
 224         // move the new pivot to the front if it's not already there.
 225         if(pivot != 0) SWAP(verts[0], verts[pivot]);
 226         return head;
 227 }
 228
 229 static int
 230 QHullReduce(cpFloat tol, cpVect *verts, int count, cpVect a, cpVect pivot, cpVect b, cpVect *result)
 231 {
 232         if(count < 0){
 233                 return 0;
 234         } else if(count == 0) {
 235                 result[0] = pivot;
 236                 return 1;
 237         } else {
 238                 int left_count = QHullPartition(verts, count, a, pivot, tol);
 239                 int index = QHullReduce(tol, verts + 1, left_count - 1, a, verts[0], pivot, result);
 240
 241                 result[index++] = pivot;
 242
 243                 int right_count = QHullPartition(verts + left_count, count - left_count, pivot, b, tol);
 244                 return index + QHullReduce(tol, verts + left_count + 1, right_count - 1, pivot, verts[left_count], b, result + index);
 245         }
 246 }
 247
 248 // QuickHull seemed like a neat algorithm, and efficient-ish for large input sets.
 249 // My implementation performs an in place reduction using the result array as scratch space.
 250 int
 251 cpConvexHull(int count, const cpVect *verts, cpVect *result, int *first, cpFloat tol)
 252 {
 253         if(verts != result){
 254                 // Copy the line vertexes into the empty part of the result polyline to use as a scratch buffer.
 255                 memcpy(result, verts, count*sizeof(cpVect));
 256         }
 257
 258         // Degenerate case, all points are the same.
 259         int start, end;
 260         cpLoopIndexes(verts, count, &start, &end);
 261         if(start == end){
 262                 if(first) (*first) = 0;
 263                 return 1;
 264         }
 265
 266         SWAP(result[0], result[start]);
 267         SWAP(result[1], result[end == 0 ? start : end]);
 268
 269         cpVect a = result[0];
 270         cpVect b = result[1];
 271
 272         if(first) (*first) = start;
 273         return QHullReduce(tol, result + 2, count - 2, a, b, a, result + 1) + 1;
 274 }
 275
 276 //MARK: Alternate Block Iterators
 277
 278 #if defined(__has_extension)
 279 #if __has_extension(blocks)
 280
 281 static void IteratorFunc(void *ptr, void (^block)(void *ptr)){block(ptr);}
 282
 283 void cpSpaceEachBody_b(cpSpace *space, void (^block)(cpBody *body)){
 284         cpSpaceEachBody(space, (cpSpaceBodyIteratorFunc)IteratorFunc, block);
 285 }
 286
 287 void cpSpaceEachShape_b(cpSpace *space, void (^block)(cpShape *shape)){
 288         cpSpaceEachShape(space, (cpSpaceShapeIteratorFunc)IteratorFunc, block);
 289 }
 290
 291 void cpSpaceEachConstraint_b(cpSpace *space, void (^block)(cpConstraint *constraint)){
 292         cpSpaceEachConstraint(space, (cpSpaceConstraintIteratorFunc)IteratorFunc, block);
 293 }
 294
 295 static void BodyIteratorFunc(cpBody *body, void *ptr, void (^block)(void *ptr)){block(ptr);}
 296
 297 void cpBodyEachShape_b(cpBody *body, void (^block)(cpShape *shape)){
 298         cpBodyEachShape(body, (cpBodyShapeIteratorFunc)BodyIteratorFunc, block);
 299 }
 300
 301 void cpBodyEachConstraint_b(cpBody *body, void (^block)(cpConstraint *constraint)){
 302         cpBodyEachConstraint(body, (cpBodyConstraintIteratorFunc)BodyIteratorFunc, block);
 303 }
 304
 305 void cpBodyEachArbiter_b(cpBody *body, void (^block)(cpArbiter *arbiter)){
 306         cpBodyEachArbiter(body, (cpBodyArbiterIteratorFunc)BodyIteratorFunc, block);
 307 }
 308
 309 static void PointQueryIteratorFunc(cpShape *shape, cpVect p, cpFloat d, cpVect g, cpSpacePointQueryBlock block){block(shape, p, d, g);}
 310 void cpSpacePointQuery_b(cpSpace *space, cpVect point, cpFloat maxDistance, cpShapeFilter filter, cpSpacePointQueryBlock block){
 311         cpSpacePointQuery(space, point, maxDistance, filter, (cpSpacePointQueryFunc)PointQueryIteratorFunc, block);
 312 }
 313
 314 static void SegmentQueryIteratorFunc(cpShape *shape, cpVect p, cpVect n, cpFloat t, cpSpaceSegmentQueryBlock block){block(shape, p, n, t);}
 315 void cpSpaceSegmentQuery_b(cpSpace *space, cpVect start, cpVect end, cpFloat radius, cpShapeFilter filter, cpSpaceSegmentQueryBlock block){
 316         cpSpaceSegmentQuery(space, start, end, radius, filter, (cpSpaceSegmentQueryFunc)SegmentQueryIteratorFunc, block);
 317 }
 318
 319 void cpSpaceBBQuery_b(cpSpace *space, cpBB bb, cpShapeFilter filter, cpSpaceBBQueryBlock block){
 320         cpSpaceBBQuery(space, bb, filter, (cpSpaceBBQueryFunc)IteratorFunc, block);
 321 }
 322
 323 static void ShapeQueryIteratorFunc(cpShape *shape, cpContactPointSet *points, cpSpaceShapeQueryBlock block){block(shape, points);}
 324 cpBool cpSpaceShapeQuery_b(cpSpace *space, cpShape *shape, cpSpaceShapeQueryBlock block){
 325         return cpSpaceShapeQuery(space, shape, (cpSpaceShapeQueryFunc)ShapeQueryIteratorFunc, block);
 326 }
 327
 328 #endif
 329 #endif
 330
 331 #include "chipmunk/chipmunk_ffi.h"