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