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