|
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
|
|
|
354 static collisionFunc[cpShapeType.CP_NUM_SHAPES * cpShapeType.CP_NUM_SHAPES] colfuncs = builtinCollisionFuncs;
|
|
|
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 }
|
