Mercurial > projects > chipmunkd
annotate trunk/chipmunkd/cpCollision.d @ 28:4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
author | Extrawurst |
---|---|
date | Mon, 13 Dec 2010 21:40:56 +0100 |
parents | 4ceef5833c8c |
children |
rev | line source |
---|---|
4 | 1 |
2 // written in the D programming language | |
3 | |
4 module chipmunkd.cpCollision; | |
5 | |
15 | 6 import chipmunkd.chipmunk_types; |
4 | 7 import chipmunkd.chipmunk; |
8 import chipmunkd.cpShape; | |
9 | |
10 alias int function(const cpShape *, const cpShape *, cpContact *) collisionFunc; | |
11 | |
12 | |
13 // Add contact points for circle to circle collisions. | |
14 // Used by several collision tests. | |
15 static int | |
16 circle2circleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const cpFloat r2, cpContact *con) | |
17 { | |
18 cpFloat mindist = r1 + r2; | |
19 cpVect delta = cpvsub(p2, p1); | |
20 cpFloat distsq = cpvlengthsq(delta); | |
21 if(distsq >= mindist*mindist) return 0; | |
22 | |
23 cpFloat dist = cpfsqrt(distsq); | |
24 | |
25 // Allocate and initialize the contact. | |
26 cpContactInit( | |
27 con, | |
28 cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/(dist ? dist : INFINITY))), | |
29 (dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f)), | |
30 dist - mindist, | |
31 0 | |
32 ); | |
33 | |
34 return 1; | |
35 } | |
36 | |
37 // Collide circle shapes. | |
38 static int | |
39 circle2circle(const cpShape *shape1, const cpShape *shape2, cpContact *arr) | |
40 { | |
41 cpCircleShape *circ1 = cast(cpCircleShape*)shape1; | |
42 cpCircleShape *circ2 = cast(cpCircleShape*)shape2; | |
43 | |
44 return circle2circleQuery(circ1.tc, circ2.tc, circ1.r, circ2.r, arr); | |
45 } | |
46 | |
47 // Collide circles to segment shapes. | |
48 static int | |
49 circle2segment(const cpShape *circleShape, const cpShape *segmentShape, cpContact *con) | |
50 { | |
51 cpCircleShape *circ = cast(cpCircleShape *)circleShape; | |
52 cpSegmentShape *seg = cast(cpSegmentShape *)segmentShape; | |
53 | |
54 // Radius sum | |
55 cpFloat rsum = circ.r + seg.r; | |
56 | |
57 // Calculate normal distance from segment. | |
58 cpFloat dn = cpvdot(seg.tn, circ.tc) - cpvdot(seg.ta, seg.tn); | |
59 cpFloat dist = cpfabs(dn) - rsum; | |
60 if(dist > 0.0f) return 0; | |
61 | |
62 // Calculate tangential distance along segment. | |
63 cpFloat dt = -cpvcross(seg.tn, circ.tc); | |
64 cpFloat dtMin = -cpvcross(seg.tn, seg.ta); | |
65 cpFloat dtMax = -cpvcross(seg.tn, seg.tb); | |
66 | |
67 // Decision tree to decide which feature of the segment to collide with. | |
68 if(dt < dtMin){ | |
69 if(dt < (dtMin - rsum)){ | |
70 return 0; | |
71 } else { | |
72 return circle2circleQuery(circ.tc, seg.ta, circ.r, seg.r, con); | |
73 } | |
74 } else { | |
75 if(dt < dtMax){ | |
76 cpVect n = (dn < 0.0f) ? seg.tn : cpvneg(seg.tn); | |
77 cpContactInit( | |
78 con, | |
79 cpvadd(circ.tc, cpvmult(n, circ.r + dist*0.5f)), | |
80 n, | |
81 dist, | |
82 0 | |
83 ); | |
84 return 1; | |
85 } else { | |
86 if(dt < (dtMax + rsum)) { | |
87 return circle2circleQuery(circ.tc, seg.tb, circ.r, seg.r, con); | |
88 } else { | |
89 return 0; | |
90 } | |
91 } | |
92 } | |
93 | |
94 return 1; | |
95 } | |
96 | |
97 // Helper function for working with contact buffers | |
98 // This used to malloc/realloc memory on the fly but was repurposed. | |
99 static cpContact * | |
100 nextContactPoint(cpContact *arr, int *numPtr) | |
101 { | |
23 | 102 int index = *numPtr; |
4 | 103 |
23 | 104 if(index < CP_MAX_CONTACTS_PER_ARBITER){ |
105 (*numPtr) = index + 1; | |
106 return &arr[index]; | |
107 } else { | |
108 return &arr[CP_MAX_CONTACTS_PER_ARBITER - 1]; | |
109 } | |
4 | 110 } |
111 | |
112 // Find the minimum separating axis for the give poly and axis list. | |
113 static int | |
114 findMSA(const cpPolyShape *poly, const cpPolyShapeAxis *axes, const int num, cpFloat *min_out) | |
115 { | |
116 int min_index = 0; | |
117 cpFloat min = cpPolyShapeValueOnAxis(poly, axes.n, axes.d); | |
118 if(min > 0.0f) return -1; | |
119 | |
120 for(int i=1; i<num; i++){ | |
121 cpFloat dist = cpPolyShapeValueOnAxis(poly, axes[i].n, axes[i].d); | |
122 if(dist > 0.0f) { | |
123 return -1; | |
124 } else if(dist > min){ | |
125 min = dist; | |
126 min_index = i; | |
127 } | |
128 } | |
129 | |
130 (*min_out) = min; | |
131 return min_index; | |
132 } | |
133 | |
134 // Add contacts for probably penetrating vertexes. | |
135 // This handles the degenerate case where an overlap was detected, but no vertexes fall inside | |
136 // the opposing polygon. (like a star of david) | |
137 static int | |
138 findVertsFallback(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist) | |
139 { | |
140 int num = 0; | |
141 | |
142 for(int i=0; i<poly1.numVerts; i++){ | |
143 cpVect v = poly1.tVerts[i]; | |
144 if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n))) | |
145 cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1.shape.hashid, cast(const cpHashValue)i)); | |
146 } | |
147 | |
148 for(int i=0; i<poly2.numVerts; i++){ | |
149 cpVect v = poly2.tVerts[i]; | |
150 if(cpPolyShapeContainsVertPartial(poly1, v, n)) | |
151 cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2.shape.hashid, cast(const cpHashValue)i)); | |
152 } | |
153 | |
154 return num; | |
155 } | |
156 | |
157 // Add contacts for penetrating vertexes. | |
158 static int | |
159 findVerts(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist) | |
160 { | |
161 int num = 0; | |
162 | |
163 for(int i=0; i<poly1.numVerts; i++){ | |
164 cpVect v = poly1.tVerts[i]; | |
165 if(cpPolyShapeContainsVert(poly2, v)) | |
166 cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1.shape.hashid, cast(const cpHashValue)i)); | |
167 } | |
168 | |
169 for(int i=0; i<poly2.numVerts; i++){ | |
170 cpVect v = poly2.tVerts[i]; | |
171 if(cpPolyShapeContainsVert(poly1, v)) | |
172 cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2.shape.hashid, cast(const cpHashValue)i)); | |
173 } | |
174 | |
175 return (num ? num : findVertsFallback(arr, poly1, poly2, n, dist)); | |
176 } | |
177 | |
178 // Collide poly shapes together. | |
179 static int | |
180 poly2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) | |
181 { | |
182 cpPolyShape *poly1 = cast(cpPolyShape *)shape1; | |
183 cpPolyShape *poly2 = cast(cpPolyShape *)shape2; | |
184 | |
185 cpFloat min1; | |
186 int mini1 = findMSA(poly2, poly1.tAxes, poly1.numVerts, &min1); | |
187 if(mini1 == -1) return 0; | |
188 | |
189 cpFloat min2; | |
190 int mini2 = findMSA(poly1, poly2.tAxes, poly2.numVerts, &min2); | |
191 if(mini2 == -1) return 0; | |
192 | |
193 // There is overlap, find the penetrating verts | |
194 if(min1 > min2) | |
195 return findVerts(arr, poly1, poly2, poly1.tAxes[mini1].n, min1); | |
196 else | |
197 return findVerts(arr, poly1, poly2, cpvneg(poly2.tAxes[mini2].n), min2); | |
198 } | |
199 | |
200 // Like cpPolyValueOnAxis(), but for segments. | |
201 static cpFloat | |
202 segValueOnAxis(const cpSegmentShape *seg, const cpVect n, const cpFloat d) | |
203 { | |
204 cpFloat a = cpvdot(n, seg.ta) - seg.r; | |
205 cpFloat b = cpvdot(n, seg.tb) - seg.r; | |
206 return cpfmin(a, b) - d; | |
207 } | |
208 | |
209 // Identify vertexes that have penetrated the segment. | |
210 static void | |
211 findPointsBehindSeg(cpContact *arr, int *num, const cpSegmentShape *seg, const cpPolyShape *poly, const cpFloat pDist, const cpFloat coef) | |
212 { | |
213 cpFloat dta = cpvcross(seg.tn, seg.ta); | |
214 cpFloat dtb = cpvcross(seg.tn, seg.tb); | |
215 cpVect n = cpvmult(seg.tn, coef); | |
216 | |
217 for(int i=0; i<poly.numVerts; i++){ | |
218 cpVect v = poly.tVerts[i]; | |
219 if(cpvdot(v, n) < cpvdot(seg.tn, seg.ta)*coef + seg.r){ | |
220 cpFloat dt = cpvcross(seg.tn, v); | |
221 if(dta >= dt && dt >= dtb){ | |
222 cpContactInit(nextContactPoint(arr, num), v, n, pDist, CP_HASH_PAIR(poly.shape.hashid, cast(const cpHashValue)i)); | |
223 } | |
224 } | |
225 } | |
226 } | |
227 | |
228 // This one is complicated and gross. Just don't go there... | |
229 // TODO: Comment me! | |
230 static int | |
231 seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr) | |
232 { | |
233 cpSegmentShape *seg = cast(cpSegmentShape *)shape1; | |
234 cpPolyShape *poly = cast(cpPolyShape *)shape2; | |
235 cpPolyShapeAxis *axes = poly.tAxes; | |
236 | |
237 cpFloat segD = cpvdot(seg.tn, seg.ta); | |
238 cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg.tn, segD) - seg.r; | |
239 cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg.tn), -segD) - seg.r; | |
240 if(minNeg > 0.0f || minNorm > 0.0f) return 0; | |
241 | |
242 int mini = 0; | |
243 cpFloat poly_min = segValueOnAxis(seg, axes.n, axes.d); | |
244 if(poly_min > 0.0f) return 0; | |
245 for(int i=0; i<poly.numVerts; i++){ | |
246 cpFloat dist = segValueOnAxis(seg, axes[i].n, axes[i].d); | |
247 if(dist > 0.0f){ | |
248 return 0; | |
249 } else if(dist > poly_min){ | |
250 poly_min = dist; | |
251 mini = i; | |
252 } | |
253 } | |
254 | |
255 int num = 0; | |
256 | |
257 cpVect poly_n = cpvneg(axes[mini].n); | |
258 | |
259 cpVect va = cpvadd(seg.ta, cpvmult(poly_n, seg.r)); | |
260 cpVect vb = cpvadd(seg.tb, cpvmult(poly_n, seg.r)); | |
261 if(cpPolyShapeContainsVert(poly, va)) | |
262 cpContactInit(nextContactPoint(arr, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg.shape.hashid, cast(cpHashValue)0)); | |
263 if(cpPolyShapeContainsVert(poly, vb)) | |
264 cpContactInit(nextContactPoint(arr, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg.shape.hashid, cast(cpHashValue)1)); | |
265 | |
266 // Floating point precision problems here. | |
267 // This will have to do for now. | |
268 poly_min -= cp_collision_slop; | |
269 if(minNorm >= poly_min || minNeg >= poly_min) { | |
270 if(minNorm > minNeg) | |
271 findPointsBehindSeg(arr, &num, seg, poly, minNorm, 1.0f); | |
272 else | |
273 findPointsBehindSeg(arr, &num, seg, poly, minNeg, -1.0f); | |
274 } | |
275 | |
276 // If no other collision points are found, try colliding endpoints. | |
277 if(num == 0){ | |
278 cpVect poly_a = poly.tVerts[mini]; | |
279 cpVect poly_b = poly.tVerts[(mini + 1)%poly.numVerts]; | |
280 | |
281 if(circle2circleQuery(seg.ta, poly_a, seg.r, 0.0f, arr)) | |
282 return 1; | |
283 | |
284 if(circle2circleQuery(seg.tb, poly_a, seg.r, 0.0f, arr)) | |
285 return 1; | |
286 | |
287 if(circle2circleQuery(seg.ta, poly_b, seg.r, 0.0f, arr)) | |
288 return 1; | |
289 | |
290 if(circle2circleQuery(seg.tb, poly_b, seg.r, 0.0f, arr)) | |
291 return 1; | |
292 } | |
293 | |
294 return num; | |
295 } | |
296 | |
297 // This one is less gross, but still gross. | |
298 // TODO: Comment me! | |
299 static int | |
300 circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con) | |
301 { | |
302 cpCircleShape *circ = cast(cpCircleShape *)shape1; | |
303 cpPolyShape *poly = cast(cpPolyShape *)shape2; | |
304 cpPolyShapeAxis *axes = poly.tAxes; | |
305 | |
306 int mini = 0; | |
307 cpFloat min = cpvdot(axes.n, circ.tc) - axes.d - circ.r; | |
308 for(int i=0; i<poly.numVerts; i++){ | |
309 cpFloat dist = cpvdot(axes[i].n, circ.tc) - axes[i].d - circ.r; | |
310 if(dist > 0.0f){ | |
311 return 0; | |
312 } else if(dist > min) { | |
313 min = dist; | |
314 mini = i; | |
315 } | |
316 } | |
317 | |
318 cpVect n = axes[mini].n; | |
319 cpVect a = poly.tVerts[mini]; | |
320 cpVect b = poly.tVerts[(mini + 1)%poly.numVerts]; | |
321 cpFloat dta = cpvcross(n, a); | |
322 cpFloat dtb = cpvcross(n, b); | |
323 cpFloat dt = cpvcross(n, circ.tc); | |
324 | |
325 if(dt < dtb){ | |
326 return circle2circleQuery(circ.tc, b, circ.r, 0.0f, con); | |
327 } else if(dt < dta) { | |
328 cpContactInit( | |
329 con, | |
330 cpvsub(circ.tc, cpvmult(n, circ.r + min/2.0f)), | |
331 cpvneg(n), | |
332 min, | |
333 0 | |
334 ); | |
335 | |
336 return 1; | |
337 } else { | |
338 return circle2circleQuery(circ.tc, a, circ.r, 0.0f, con); | |
339 } | |
340 } | |
341 | |
342 static const collisionFunc builtinCollisionFuncs[9] = [ | |
343 &circle2circle, | |
344 null, | |
345 null, | |
346 &circle2segment, | |
347 null, | |
348 null, | |
349 &circle2poly, | |
350 &seg2poly, | |
351 &poly2poly, | |
352 ]; | |
353 | |
28
4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
Extrawurst
parents:
23
diff
changeset
|
354 __gshared collisionFunc[cpShapeType.CP_NUM_SHAPES * cpShapeType.CP_NUM_SHAPES] colfuncs = builtinCollisionFuncs; |
4 | 355 |
356 static void | |
357 addColFunc(const cpShapeType a, const cpShapeType b, collisionFunc func) | |
358 { | |
359 colfuncs[a + b*cpShapeType.CP_NUM_SHAPES] = func; | |
360 } | |
361 | |
362 // Initializes the array of collision functions. | |
363 // Called by cpInitChipmunk(). | |
364 void cpInitCollisionFuncs() | |
365 { | |
366 addColFunc(cpShapeType.CP_CIRCLE_SHAPE, cpShapeType.CP_CIRCLE_SHAPE, &circle2circle); | |
367 addColFunc(cpShapeType.CP_CIRCLE_SHAPE, cpShapeType.CP_SEGMENT_SHAPE, &circle2segment); | |
368 addColFunc(cpShapeType.CP_SEGMENT_SHAPE, cpShapeType.CP_POLY_SHAPE, &seg2poly); | |
369 addColFunc(cpShapeType.CP_CIRCLE_SHAPE, cpShapeType.CP_POLY_SHAPE, &circle2poly); | |
370 addColFunc(cpShapeType.CP_POLY_SHAPE, cpShapeType.CP_POLY_SHAPE, &poly2poly); | |
371 } | |
372 | |
373 | |
374 int | |
375 cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr) | |
376 { | |
377 // Their shape types must be in order. | |
378 assert(a.klass.type <= b.klass.type, "Collision shapes passed to cpCollideShapes() are not sorted."); | |
379 | |
380 collisionFunc cfunc = colfuncs[a.klass.type + b.klass.type*cpShapeType.CP_NUM_SHAPES]; | |
381 return (cfunc) ? cfunc(a, b, arr) : 0; | |
382 } |