Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/cpCollision.d @ 4:7ebbd4d05553

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
initial commit
author Extrawurst
date Thu, 02 Dec 2010 02:11:26 +0100
parents
children df4ebc8add66
comparison
equal deleted inserted replaced
3:81145a61c2fe 4:7ebbd4d05553
1
2 // written in the D programming language
3
4 module chipmunkd.cpCollision;
5
6 import chipmunkd.chipmunk_types_h;
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 {
102 int num = *numPtr;
103
104 if(num < CP_MAX_CONTACTS_PER_ARBITER)
105 (*numPtr) = num + 1;
106
107 return &arr[num];
108 }
109
110 // Find the minimum separating axis for the give poly and axis list.
111 static int
112 findMSA(const cpPolyShape *poly, const cpPolyShapeAxis *axes, const int num, cpFloat *min_out)
113 {
114 int min_index = 0;
115 cpFloat min = cpPolyShapeValueOnAxis(poly, axes.n, axes.d);
116 if(min > 0.0f) return -1;
117
118 for(int i=1; i<num; i++){
119 cpFloat dist = cpPolyShapeValueOnAxis(poly, axes[i].n, axes[i].d);
120 if(dist > 0.0f) {
121 return -1;
122 } else if(dist > min){
123 min = dist;
124 min_index = i;
125 }
126 }
127
128 (*min_out) = min;
129 return min_index;
130 }
131
132 // Add contacts for probably penetrating vertexes.
133 // This handles the degenerate case where an overlap was detected, but no vertexes fall inside
134 // the opposing polygon. (like a star of david)
135 static int
136 findVertsFallback(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist)
137 {
138 int num = 0;
139
140 for(int i=0; i<poly1.numVerts; i++){
141 cpVect v = poly1.tVerts[i];
142 if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n)))
143 cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1.shape.hashid, cast(const cpHashValue)i));
144 }
145
146 for(int i=0; i<poly2.numVerts; i++){
147 cpVect v = poly2.tVerts[i];
148 if(cpPolyShapeContainsVertPartial(poly1, v, n))
149 cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2.shape.hashid, cast(const cpHashValue)i));
150 }
151
152 return num;
153 }
154
155 // Add contacts for penetrating vertexes.
156 static int
157 findVerts(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist)
158 {
159 int num = 0;
160
161 for(int i=0; i<poly1.numVerts; i++){
162 cpVect v = poly1.tVerts[i];
163 if(cpPolyShapeContainsVert(poly2, v))
164 cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1.shape.hashid, cast(const cpHashValue)i));
165 }
166
167 for(int i=0; i<poly2.numVerts; i++){
168 cpVect v = poly2.tVerts[i];
169 if(cpPolyShapeContainsVert(poly1, v))
170 cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2.shape.hashid, cast(const cpHashValue)i));
171 }
172
173 return (num ? num : findVertsFallback(arr, poly1, poly2, n, dist));
174 }
175
176 // Collide poly shapes together.
177 static int
178 poly2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
179 {
180 cpPolyShape *poly1 = cast(cpPolyShape *)shape1;
181 cpPolyShape *poly2 = cast(cpPolyShape *)shape2;
182
183 cpFloat min1;
184 int mini1 = findMSA(poly2, poly1.tAxes, poly1.numVerts, &min1);
185 if(mini1 == -1) return 0;
186
187 cpFloat min2;
188 int mini2 = findMSA(poly1, poly2.tAxes, poly2.numVerts, &min2);
189 if(mini2 == -1) return 0;
190
191 // There is overlap, find the penetrating verts
192 if(min1 > min2)
193 return findVerts(arr, poly1, poly2, poly1.tAxes[mini1].n, min1);
194 else
195 return findVerts(arr, poly1, poly2, cpvneg(poly2.tAxes[mini2].n), min2);
196 }
197
198 // Like cpPolyValueOnAxis(), but for segments.
199 static cpFloat
200 segValueOnAxis(const cpSegmentShape *seg, const cpVect n, const cpFloat d)
201 {
202 cpFloat a = cpvdot(n, seg.ta) - seg.r;
203 cpFloat b = cpvdot(n, seg.tb) - seg.r;
204 return cpfmin(a, b) - d;
205 }
206
207 // Identify vertexes that have penetrated the segment.
208 static void
209 findPointsBehindSeg(cpContact *arr, int *num, const cpSegmentShape *seg, const cpPolyShape *poly, const cpFloat pDist, const cpFloat coef)
210 {
211 cpFloat dta = cpvcross(seg.tn, seg.ta);
212 cpFloat dtb = cpvcross(seg.tn, seg.tb);
213 cpVect n = cpvmult(seg.tn, coef);
214
215 for(int i=0; i<poly.numVerts; i++){
216 cpVect v = poly.tVerts[i];
217 if(cpvdot(v, n) < cpvdot(seg.tn, seg.ta)*coef + seg.r){
218 cpFloat dt = cpvcross(seg.tn, v);
219 if(dta >= dt && dt >= dtb){
220 cpContactInit(nextContactPoint(arr, num), v, n, pDist, CP_HASH_PAIR(poly.shape.hashid, cast(const cpHashValue)i));
221 }
222 }
223 }
224 }
225
226 // This one is complicated and gross. Just don't go there...
227 // TODO: Comment me!
228 static int
229 seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
230 {
231 cpSegmentShape *seg = cast(cpSegmentShape *)shape1;
232 cpPolyShape *poly = cast(cpPolyShape *)shape2;
233 cpPolyShapeAxis *axes = poly.tAxes;
234
235 cpFloat segD = cpvdot(seg.tn, seg.ta);
236 cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg.tn, segD) - seg.r;
237 cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg.tn), -segD) - seg.r;
238 if(minNeg > 0.0f || minNorm > 0.0f) return 0;
239
240 int mini = 0;
241 cpFloat poly_min = segValueOnAxis(seg, axes.n, axes.d);
242 if(poly_min > 0.0f) return 0;
243 for(int i=0; i<poly.numVerts; i++){
244 cpFloat dist = segValueOnAxis(seg, axes[i].n, axes[i].d);
245 if(dist > 0.0f){
246 return 0;
247 } else if(dist > poly_min){
248 poly_min = dist;
249 mini = i;
250 }
251 }
252
253 int num = 0;
254
255 cpVect poly_n = cpvneg(axes[mini].n);
256
257 cpVect va = cpvadd(seg.ta, cpvmult(poly_n, seg.r));
258 cpVect vb = cpvadd(seg.tb, cpvmult(poly_n, seg.r));
259 if(cpPolyShapeContainsVert(poly, va))
260 cpContactInit(nextContactPoint(arr, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg.shape.hashid, cast(cpHashValue)0));
261 if(cpPolyShapeContainsVert(poly, vb))
262 cpContactInit(nextContactPoint(arr, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg.shape.hashid, cast(cpHashValue)1));
263
264 // Floating point precision problems here.
265 // This will have to do for now.
266 poly_min -= cp_collision_slop;
267 if(minNorm >= poly_min || minNeg >= poly_min) {
268 if(minNorm > minNeg)
269 findPointsBehindSeg(arr, &num, seg, poly, minNorm, 1.0f);
270 else
271 findPointsBehindSeg(arr, &num, seg, poly, minNeg, -1.0f);
272 }
273
274 // If no other collision points are found, try colliding endpoints.
275 if(num == 0){
276 cpVect poly_a = poly.tVerts[mini];
277 cpVect poly_b = poly.tVerts[(mini + 1)%poly.numVerts];
278
279 if(circle2circleQuery(seg.ta, poly_a, seg.r, 0.0f, arr))
280 return 1;
281
282 if(circle2circleQuery(seg.tb, poly_a, seg.r, 0.0f, arr))
283 return 1;
284
285 if(circle2circleQuery(seg.ta, poly_b, seg.r, 0.0f, arr))
286 return 1;
287
288 if(circle2circleQuery(seg.tb, poly_b, seg.r, 0.0f, arr))
289 return 1;
290 }
291
292 return num;
293 }
294
295 // This one is less gross, but still gross.
296 // TODO: Comment me!
297 static int
298 circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con)
299 {
300 cpCircleShape *circ = cast(cpCircleShape *)shape1;
301 cpPolyShape *poly = cast(cpPolyShape *)shape2;
302 cpPolyShapeAxis *axes = poly.tAxes;
303
304 int mini = 0;
305 cpFloat min = cpvdot(axes.n, circ.tc) - axes.d - circ.r;
306 for(int i=0; i<poly.numVerts; i++){
307 cpFloat dist = cpvdot(axes[i].n, circ.tc) - axes[i].d - circ.r;
308 if(dist > 0.0f){
309 return 0;
310 } else if(dist > min) {
311 min = dist;
312 mini = i;
313 }
314 }
315
316 cpVect n = axes[mini].n;
317 cpVect a = poly.tVerts[mini];
318 cpVect b = poly.tVerts[(mini + 1)%poly.numVerts];
319 cpFloat dta = cpvcross(n, a);
320 cpFloat dtb = cpvcross(n, b);
321 cpFloat dt = cpvcross(n, circ.tc);
322
323 if(dt < dtb){
324 return circle2circleQuery(circ.tc, b, circ.r, 0.0f, con);
325 } else if(dt < dta) {
326 cpContactInit(
327 con,
328 cpvsub(circ.tc, cpvmult(n, circ.r + min/2.0f)),
329 cpvneg(n),
330 min,
331 0
332 );
333
334 return 1;
335 } else {
336 return circle2circleQuery(circ.tc, a, circ.r, 0.0f, con);
337 }
338 }
339
340 static const collisionFunc builtinCollisionFuncs[9] = [
341 &circle2circle,
342 null,
343 null,
344 &circle2segment,
345 null,
346 null,
347 &circle2poly,
348 &seg2poly,
349 &poly2poly,
350 ];
351
352 static collisionFunc[cpShapeType.CP_NUM_SHAPES * cpShapeType.CP_NUM_SHAPES] colfuncs = builtinCollisionFuncs;
353
354 static void
355 addColFunc(const cpShapeType a, const cpShapeType b, collisionFunc func)
356 {
357 colfuncs[a + b*cpShapeType.CP_NUM_SHAPES] = func;
358 }
359
360 // Initializes the array of collision functions.
361 // Called by cpInitChipmunk().
362 void cpInitCollisionFuncs()
363 {
364 addColFunc(cpShapeType.CP_CIRCLE_SHAPE, cpShapeType.CP_CIRCLE_SHAPE, &circle2circle);
365 addColFunc(cpShapeType.CP_CIRCLE_SHAPE, cpShapeType.CP_SEGMENT_SHAPE, &circle2segment);
366 addColFunc(cpShapeType.CP_SEGMENT_SHAPE, cpShapeType.CP_POLY_SHAPE, &seg2poly);
367 addColFunc(cpShapeType.CP_CIRCLE_SHAPE, cpShapeType.CP_POLY_SHAPE, &circle2poly);
368 addColFunc(cpShapeType.CP_POLY_SHAPE, cpShapeType.CP_POLY_SHAPE, &poly2poly);
369 }
370
371
372 int
373 cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr)
374 {
375 // Their shape types must be in order.
376 assert(a.klass.type <= b.klass.type, "Collision shapes passed to cpCollideShapes() are not sorted.");
377
378 collisionFunc cfunc = colfuncs[a.klass.type + b.klass.type*cpShapeType.CP_NUM_SHAPES];
379 return (cfunc) ? cfunc(a, b, arr) : 0;
380 }