Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/cpArbiter.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 c03a41d47b60
comparison
equal deleted inserted replaced
3:81145a61c2fe 4:7ebbd4d05553
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 }