4
|
1
|
|
2 // written in the D programming language
|
|
3
|
|
4 module chipmunkd.cpArbiter;
|
|
5
|
|
6 import chipmunkd.chipmunk;
|
|
7 import chipmunkd.constraints.util;
|
|
8
|
|
9 // Determines how fast penetrations resolve themselves.
|
|
10 //extern cpFloat cp_bias_coef;
|
|
11 // Amount of allowed penetration. Used to reduce vibrating contacts.
|
|
12 //extern cpFloat cp_collision_slop;
|
|
13
|
|
14 // Data structure for contact points.
|
|
15 struct cpContact {
|
|
16 // Contact point and normal.
|
|
17 cpVect p, n;
|
|
18 // Penetration distance.
|
|
19 cpFloat dist;
|
|
20
|
|
21 // Calculated by cpArbiterPreStep().
|
|
22 cpVect r1, r2;
|
|
23 cpFloat nMass, tMass, bounce;
|
|
24
|
|
25 // Persistant contact information.
|
|
26 cpFloat jnAcc, jtAcc, jBias;
|
|
27 cpFloat bias;
|
|
28
|
|
29 // Hash value used to (mostly) uniquely identify a contact.
|
|
30 cpHashValue hash;
|
|
31 }
|
|
32
|
|
33 //// Contacts are always allocated in groups.
|
|
34 //cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash);
|
|
35 //
|
|
36 //// Sum the contact impulses. (Can be used after cpSpaceStep() returns)
|
|
37 //cpVect cpContactsSumImpulses(cpContact *contacts, int numContacts);
|
|
38 //cpVect cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts);
|
|
39
|
23
|
40 enum CP_MAX_CONTACTS_PER_ARBITER = 6;
|
|
41
|
4
|
42 enum cpArbiterState {
|
|
43 cpArbiterStateNormal,
|
|
44 cpArbiterStateFirstColl,
|
|
45 cpArbiterStateIgnore,
|
|
46 cpArbiterStateSleep,
|
|
47 cpArbiterStateCached,
|
|
48 }
|
|
49
|
|
50 // Data structure for tracking collisions between shapes.
|
|
51 struct cpArbiter {
|
|
52 // Information on the contact points between the objects.
|
|
53 int numContacts;
|
|
54 cpContact *contacts;
|
|
55
|
|
56 // The two shapes and bodies involved in the collision.
|
|
57 // These variables are NOT in the order defined by the collision handler.
|
|
58 // Using CP_ARBITER_GET_SHAPES and CP_ARBITER_GET_BODIES will save you from
|
|
59 // many headaches
|
23
|
60 cpShape* a;
|
|
61 cpShape* b;
|
4
|
62
|
|
63 // Calculated before calling the pre-solve collision handler
|
|
64 // Override them with custom values if you want specialized behavior
|
|
65 cpFloat e;
|
|
66 cpFloat u;
|
|
67 // Used for surface_v calculations, implementation may change
|
|
68 cpVect surface_vr;
|
|
69
|
|
70 // Time stamp of the arbiter. (from cpSpace)
|
|
71 cpTimestamp stamp;
|
|
72
|
|
73 cpCollisionHandler *handler;
|
|
74
|
|
75 // Are the shapes swapped in relation to the collision handler?
|
|
76 cpBool swappedColl;
|
|
77 cpArbiterState state;
|
|
78 }
|
|
79
|
|
80 //// Arbiters are allocated in large buffers by the space and don't require a destroy function
|
|
81 //cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b);
|
|
82 //
|
|
83 //// These functions are all intended to be used internally.
|
|
84 //// Inject new contact points into the arbiter while preserving contact history.
|
|
85 //void cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, struct cpCollisionHandler *handler, cpShape *a, cpShape *b);
|
|
86 //// Precalculate values used by the solver.
|
|
87 //void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv);
|
|
88 //void cpArbiterApplyCachedImpulse(cpArbiter *arb);
|
|
89 //// Run an iteration of the solver on the arbiter.
|
|
90 //void cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef);
|
|
91 //
|
|
92 //// Arbiter Helper Functions
|
|
93 //cpVect cpArbiterTotalImpulse(cpArbiter *arb);
|
|
94 //cpVect cpArbiterTotalImpulseWithFriction(cpArbiter *arb);
|
|
95 //void cpArbiterIgnore(cpArbiter *arb);
|
|
96
|
|
97
|
|
98 static void
|
|
99 cpArbiterGetShapes(/+const+/ cpArbiter *arb, cpShape **a, cpShape **b)
|
|
100 {
|
|
101 if(arb.swappedColl){
|
23
|
102 (*a) = arb.b, (*b) = arb.a;
|
4
|
103 } else {
|
23
|
104 (*a) = arb.a, (*b) = arb.b;
|
4
|
105 }
|
|
106 }
|
13
|
107
|
4
|
108 template CP_ARBITER_GET_SHAPES(string arb,string a,string b)
|
|
109 {
|
|
110 enum CP_ARBITER_GET_SHAPES = "cpShape* "~a~", "~b~";"~
|
|
111 "cpArbiterGetShapes("~arb~", &"~a~", &"~b~");";
|
|
112 }
|
|
113
|
|
114 static void
|
|
115 cpArbiterGetBodies(/+const+/ cpArbiter *arb, cpBody **a, cpBody **b)
|
|
116 {
|
|
117 //CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b);
|
|
118 cpShape *shape_a, shape_b; cpArbiterGetShapes(arb, &shape_a, &shape_b);
|
|
119 (*a) = shape_a._body;
|
|
120 (*b) = shape_b._body;
|
|
121 }
|
|
122 //#define CP_ARBITER_GET_BODIES(arb, a, b) cpBody *a, *b; cpArbiterGetBodies(arb, &a, &b);
|
|
123
|
|
124 static cpBool
|
|
125 cpArbiterIsFirstContact(const cpArbiter *arb)
|
|
126 {
|
|
127 return arb.state == cpArbiterState.cpArbiterStateFirstColl;
|
|
128 }
|
|
129
|
23
|
130 static int
|
|
131 cpArbiterGetCount(const cpArbiter *arb)
|
|
132 {
|
|
133 return arb.numContacts;
|
|
134 }
|
|
135
|
4
|
136 static cpVect
|
|
137 cpArbiterGetNormal(const cpArbiter *arb, int i)
|
|
138 {
|
|
139 cpVect n = arb.contacts[i].n;
|
|
140 return arb.swappedColl ? cpvneg(n) : n;
|
|
141 }
|
|
142
|
|
143 static cpVect
|
|
144 cpArbiterGetPoint(const cpArbiter *arb, int i)
|
|
145 {
|
|
146 return arb.contacts[i].p;
|
|
147 }
|
|
148
|
23
|
149
|
|
150 static cpFloat
|
|
151 cpArbiteGetDepth(const cpArbiter *arb, int i)
|
|
152 {
|
|
153 return arb.contacts[i].dist;
|
|
154 }
|
|
155
|
|
156 struct cpContactPointSet {
|
|
157 int count;
|
|
158
|
|
159 struct TPoint
|
|
160 {
|
|
161 cpVect point, normal;
|
|
162 cpFloat dist = 0;
|
|
163 }
|
|
164
|
|
165 TPoint[CP_MAX_CONTACTS_PER_ARBITER] points;
|
|
166 }
|
|
167
|
|
168 static cpContactPointSet
|
|
169 cpArbiterGetContactPointSet(const cpArbiter *arb)
|
|
170 {
|
|
171 cpContactPointSet set;
|
|
172 set.count = cpArbiterGetCount(arb);
|
|
173
|
|
174 int i;
|
|
175 for(i=0; i<set.count; i++){
|
|
176 set.points[i].point = arb.contacts[i].p;
|
|
177 set.points[i].normal = arb.contacts[i].p;
|
|
178 set.points[i].dist = arb.contacts[i].dist;
|
|
179 }
|
|
180
|
|
181 return set;
|
|
182 }
|
|
183
|
|
184 // cpArbiter.c --------------------------
|
|
185
|
4
|
186 cpFloat cp_bias_coef = 0.1f;
|
|
187 cpFloat cp_collision_slop = 0.1f;
|
|
188
|
|
189 cpContact*
|
|
190 cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash)
|
|
191 {
|
|
192 con.p = p;
|
|
193 con.n = n;
|
|
194 con.dist = dist;
|
|
195
|
|
196 con.jnAcc = 0.0f;
|
|
197 con.jtAcc = 0.0f;
|
|
198 con.jBias = 0.0f;
|
|
199
|
|
200 con.hash = hash;
|
|
201
|
|
202 return con;
|
|
203 }
|
|
204
|
|
205 cpVect
|
|
206 cpArbiterTotalImpulse(cpArbiter *arb)
|
|
207 {
|
|
208 cpContact *contacts = arb.contacts;
|
|
209 cpVect sum = cpvzero;
|
|
210
|
|
211 for(int i=0, count=arb.numContacts; i<count; i++){
|
|
212 cpContact *con = &contacts[i];
|
|
213 sum = cpvadd(sum, cpvmult(con.n, con.jnAcc));
|
|
214 }
|
|
215
|
|
216 return sum;
|
|
217 }
|
|
218
|
|
219 cpVect
|
|
220 cpArbiterTotalImpulseWithFriction(cpArbiter *arb)
|
|
221 {
|
|
222 cpContact *contacts = arb.contacts;
|
|
223 cpVect sum = cpvzero;
|
|
224
|
|
225 for(int i=0, count=arb.numContacts; i<count; i++){
|
|
226 cpContact *con = &contacts[i];
|
|
227 sum = cpvadd(sum, cpvrotate(con.n, cpv(con.jnAcc, con.jtAcc)));
|
|
228 }
|
|
229
|
|
230 return sum;
|
|
231 }
|
|
232
|
|
233 cpFloat
|
|
234 cpContactsEstimateCrushingImpulse(cpContact *contacts, int numContacts)
|
|
235 {
|
|
236 cpFloat fsum = 0.0f;
|
|
237 cpVect vsum = cpvzero;
|
|
238
|
|
239 for(int i=0; i<numContacts; i++){
|
|
240 cpContact *con = &contacts[i];
|
|
241 cpVect j = cpvrotate(con.n, cpv(con.jnAcc, con.jtAcc));
|
|
242
|
|
243 fsum += cpvlength(j);
|
|
244 vsum = cpvadd(vsum, j);
|
|
245 }
|
|
246
|
|
247 cpFloat vmag = cpvlength(vsum);
|
|
248 return (1.0f - vmag/fsum);
|
|
249 }
|
|
250
|
|
251 void
|
|
252 cpArbiterIgnore(cpArbiter *arb)
|
|
253 {
|
|
254 arb.state = cpArbiterState.cpArbiterStateIgnore;
|
|
255 }
|
|
256
|
|
257 cpArbiter*
|
|
258 cpArbiterAlloc()
|
|
259 {
|
|
260 return cast(cpArbiter *)cpcalloc(1, cpArbiter.sizeof);
|
|
261 }
|
|
262
|
|
263 cpArbiter*
|
|
264 cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b)
|
|
265 {
|
23
|
266 arb.handler = null;
|
|
267 arb.swappedColl = cpFalse;
|
|
268
|
|
269 arb.e = 0.0f;
|
|
270 arb.u = 0.0f;
|
|
271 arb.surface_vr = cpvzero;
|
|
272
|
4
|
273 arb.numContacts = 0;
|
|
274 arb.contacts = null;
|
|
275
|
23
|
276 arb.a = a;
|
|
277 arb.b = b;
|
4
|
278
|
|
279 arb.stamp = 0;
|
|
280 arb.state = cpArbiterState.cpArbiterStateFirstColl;
|
|
281
|
|
282 return arb;
|
|
283 }
|
|
284
|
|
285 cpArbiter*
|
|
286 cpArbiterNew(cpShape *a, cpShape *b)
|
|
287 {
|
|
288 return cpArbiterInit(cpArbiterAlloc(), a, b);
|
|
289 }
|
|
290
|
|
291 void
|
|
292 cpArbiterDestroy(cpArbiter *arb)
|
|
293 {
|
|
294 // if(arb.contacts) cpfree(arb.contacts);
|
|
295 }
|
|
296
|
|
297 void
|
|
298 cpArbiterFree(cpArbiter *arb)
|
|
299 {
|
|
300 if(arb){
|
|
301 cpArbiterDestroy(arb);
|
|
302 cpfree(arb);
|
|
303 }
|
|
304 }
|
|
305
|
|
306 void
|
|
307 cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisionHandler *handler, cpShape *a, cpShape *b)
|
|
308 {
|
|
309 // Arbiters without contact data may exist if a collision function rejected the collision.
|
|
310 if(arb.contacts){
|
|
311 // Iterate over the possible pairs to look for hash value matches.
|
|
312 for(int i=0; i<arb.numContacts; i++){
|
|
313 cpContact *old = &arb.contacts[i];
|
|
314
|
|
315 for(int j=0; j<numContacts; j++){
|
|
316 cpContact *new_contact = &contacts[j];
|
|
317
|
|
318 // This could trigger false positives, but is fairly unlikely nor serious if it does.
|
|
319 if(new_contact.hash == old.hash){
|
|
320 // Copy the persistant contact information.
|
|
321 new_contact.jnAcc = old.jnAcc;
|
|
322 new_contact.jtAcc = old.jtAcc;
|
|
323 }
|
|
324 }
|
|
325 }
|
|
326 }
|
|
327
|
|
328 arb.contacts = contacts;
|
|
329 arb.numContacts = numContacts;
|
|
330
|
|
331 arb.handler = handler;
|
|
332 arb.swappedColl = (a.collision_type != handler.a);
|
|
333
|
|
334 arb.e = a.e * b.e;
|
|
335 arb.u = a.u * b.u;
|
|
336 arb.surface_vr = cpvsub(a.surface_v, b.surface_v);
|
|
337
|
|
338 // For collisions between two similar primitive types, the order could have been swapped.
|
23
|
339 arb.a = a;
|
|
340 arb.b = b;
|
4
|
341
|
|
342 // mark it as new if it's been cached
|
|
343 if(arb.state == cpArbiterState.cpArbiterStateCached) arb.state = cpArbiterState.cpArbiterStateFirstColl;
|
|
344 }
|
|
345
|
|
346 void
|
|
347 cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
|
|
348 {
|
23
|
349 cpBody *a = arb.a._body;
|
|
350 cpBody *b = arb.b._body;
|
4
|
351
|
|
352 for(int i=0; i<arb.numContacts; i++){
|
|
353 cpContact *con = &arb.contacts[i];
|
|
354
|
|
355 // Calculate the offsets.
|
|
356 con.r1 = cpvsub(con.p, a.p);
|
|
357 con.r2 = cpvsub(con.p, b.p);
|
|
358
|
|
359 // Calculate the mass normal and mass tangent.
|
|
360 con.nMass = 1.0f/k_scalar(a, b, con.r1, con.r2, con.n);
|
|
361 con.tMass = 1.0f/k_scalar(a, b, con.r1, con.r2, cpvperp(con.n));
|
|
362
|
|
363 // Calculate the target bias velocity.
|
|
364 con.bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con.dist + cp_collision_slop);
|
|
365 con.jBias = 0.0f;
|
|
366
|
|
367 // Calculate the target bounce velocity.
|
|
368 con.bounce = normal_relative_velocity(a, b, con.r1, con.r2, con.n)*arb.e;//cpvdot(con.n, cpvsub(v2, v1))*e;
|
|
369 }
|
|
370 }
|
|
371
|
|
372 void
|
|
373 cpArbiterApplyCachedImpulse(cpArbiter *arb)
|
|
374 {
|
23
|
375 cpShape *shapea = arb.a;
|
|
376 cpShape *shapeb = arb.b;
|
4
|
377
|
|
378 arb.u = shapea.u * shapeb.u;
|
|
379 arb.surface_vr = cpvsub(shapeb.surface_v, shapea.surface_v);
|
|
380
|
|
381 cpBody *a = shapea._body;
|
|
382 cpBody *b = shapeb._body;
|
|
383
|
|
384 for(int i=0; i<arb.numContacts; i++){
|
|
385 cpContact *con = &arb.contacts[i];
|
|
386 apply_impulses(a, b, con.r1, con.r2, cpvrotate(con.n, cpv(con.jnAcc, con.jtAcc)));
|
|
387 }
|
|
388 }
|
|
389
|
|
390 void
|
|
391 cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef)
|
|
392 {
|
23
|
393 cpBody *a = arb.a._body;
|
|
394 cpBody *b = arb.b._body;
|
4
|
395
|
|
396 for(int i=0; i<arb.numContacts; i++){
|
|
397 cpContact *con = &arb.contacts[i];
|
|
398 cpVect n = con.n;
|
|
399 cpVect r1 = con.r1;
|
|
400 cpVect r2 = con.r2;
|
|
401
|
|
402 // Calculate the relative bias velocities.
|
|
403 cpVect vb1 = cpvadd(a.v_bias, cpvmult(cpvperp(r1), a.w_bias));
|
|
404 cpVect vb2 = cpvadd(b.v_bias, cpvmult(cpvperp(r2), b.w_bias));
|
|
405 cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
|
|
406
|
|
407 // Calculate and clamp the bias impulse.
|
|
408 cpFloat jbn = (con.bias - vbn)*con.nMass;
|
|
409 cpFloat jbnOld = con.jBias;
|
|
410 con.jBias = cpfmax(jbnOld + jbn, 0.0f);
|
|
411 jbn = con.jBias - jbnOld;
|
|
412
|
|
413 // Apply the bias impulse.
|
|
414 apply_bias_impulses(a, b, r1, r2, cpvmult(n, jbn));
|
|
415
|
|
416 // Calculate the relative velocity.
|
|
417 cpVect vr = relative_velocity(a, b, r1, r2);
|
|
418 cpFloat vrn = cpvdot(vr, n);
|
|
419
|
|
420 // Calculate and clamp the normal impulse.
|
|
421 cpFloat jn = -(con.bounce*eCoef + vrn)*con.nMass;
|
|
422 cpFloat jnOld = con.jnAcc;
|
|
423 con.jnAcc = cpfmax(jnOld + jn, 0.0f);
|
|
424 jn = con.jnAcc - jnOld;
|
|
425
|
|
426 // Calculate the relative tangent velocity.
|
|
427 cpFloat vrt = cpvdot(cpvadd(vr, arb.surface_vr), cpvperp(n));
|
|
428
|
|
429 // Calculate and clamp the friction impulse.
|
|
430 cpFloat jtMax = arb.u*con.jnAcc;
|
|
431 cpFloat jt = -vrt*con.tMass;
|
|
432 cpFloat jtOld = con.jtAcc;
|
|
433 con.jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);
|
|
434 jt = con.jtAcc - jtOld;
|
|
435
|
|
436 // Apply the final impulse.
|
|
437 apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(jn, jt)));
|
|
438 }
|
|
439 }
|