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