|
4
|
1
|
|
|
2 // written in the D programming language
|
|
|
3
|
|
|
4 module chipmunkd.cpSpaceStep;
|
|
|
5
|
|
|
6 import chipmunkd.chipmunk;
|
|
|
7 //import chipmunkd.chipmunk_types_h;
|
|
|
8 //import chipmunkd.cpVect,chipmunkd.cpVect_h;
|
|
|
9 //import chipmunkd.cpHashSet;
|
|
|
10 //import chipmunkd.cpCollision;
|
|
|
11 //import chipmunkd.cpBody;
|
|
|
12 //import chipmunkd.cpArray;
|
|
|
13 //import chipmunkd.cpShape;
|
|
|
14 //import chipmunkd.cpBB;
|
|
|
15 //import chipmunkd.cpArbiter;
|
|
|
16 //import chipmunkd.cpSpaceHash;
|
|
|
17 //import chipmunkd.constraints.cpConstraint;
|
|
|
18 //import chipmunkd.cpSpaceQuery;
|
|
|
19 //
|
|
|
20 //#pragma mark Post Step Callback Functions
|
|
|
21
|
|
29
|
22 struct PostStepCallback {
|
|
4
|
23 cpPostStepFunc func;
|
|
|
24 void *obj;
|
|
|
25 void *data;
|
|
|
26 }
|
|
|
27
|
|
|
28 static cpBool
|
|
29
|
29 postStepFuncSetEql(PostStepCallback *a, PostStepCallback *b){
|
|
4
|
30 return a.obj == b.obj;
|
|
|
31 }
|
|
|
32
|
|
|
33 static void *
|
|
29
|
34 postStepFuncSetTrans(PostStepCallback *callback, void *ignored)
|
|
4
|
35 {
|
|
29
|
36 PostStepCallback *value = cast(PostStepCallback *)cpmalloc(PostStepCallback.sizeof);
|
|
4
|
37 (*value) = (*callback);
|
|
|
38
|
|
|
39 return value;
|
|
|
40 }
|
|
|
41
|
|
|
42 void
|
|
|
43 cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data)
|
|
|
44 {
|
|
|
45 if(!space.postStepCallbacks){
|
|
|
46 space.postStepCallbacks = cpHashSetNew(0, cast(cpHashSetEqlFunc)&postStepFuncSetEql, cast(cpHashSetTransFunc)&postStepFuncSetTrans);
|
|
|
47 }
|
|
|
48
|
|
29
|
49 PostStepCallback callback = {func, obj, data};
|
|
4
|
50 cpHashSetInsert(space.postStepCallbacks, cast(cpHashValue)cast(size_t)obj, &callback, null);
|
|
|
51 }
|
|
|
52
|
|
29
|
53 void *
|
|
|
54 cpSpaceGetPostStepData(cpSpace *space, void *obj)
|
|
|
55 {
|
|
|
56 if(space.postStepCallbacks){
|
|
|
57 PostStepCallback query = {null, obj, null};
|
|
|
58 PostStepCallback *callback = cast(PostStepCallback *)cpHashSetFind(space.postStepCallbacks, cast(cpHashValue)cast(size_t)obj, &query);
|
|
|
59 return (callback ? callback.data : null);
|
|
|
60 } else {
|
|
|
61 return null;
|
|
|
62 }
|
|
|
63 }
|
|
|
64
|
|
4
|
65 //#pragma mark Contact Buffer Functions
|
|
|
66
|
|
|
67 enum CP_CONTACTS_BUFFER_SIZE = ((CP_BUFFER_BYTES - cpContactBufferHeader.sizeof)/cpContact.sizeof);
|
|
|
68
|
|
|
69 struct cpContactBuffer {
|
|
|
70 cpContactBufferHeader header;
|
|
|
71 cpContact[CP_CONTACTS_BUFFER_SIZE] contacts;
|
|
|
72 }
|
|
|
73
|
|
|
74 static cpContactBufferHeader *
|
|
|
75 cpSpaceAllocContactBuffer(cpSpace *space)
|
|
|
76 {
|
|
|
77 cpContactBuffer *buffer = cast(cpContactBuffer *)cpmalloc(cpContactBuffer.sizeof);
|
|
|
78 cpArrayPush(space.allocatedBuffers, buffer);
|
|
|
79 return cast(cpContactBufferHeader *)buffer;
|
|
|
80 }
|
|
|
81
|
|
|
82 static cpContactBufferHeader *
|
|
|
83 cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpContactBufferHeader *splice)
|
|
|
84 {
|
|
|
85 header.stamp = stamp;
|
|
|
86 header.next = (splice ? splice.next : header);
|
|
|
87 header.numContacts = 0;
|
|
|
88
|
|
|
89 return header;
|
|
|
90 }
|
|
|
91
|
|
|
92 static void
|
|
|
93 cpSpacePushFreshContactBuffer(cpSpace *space)
|
|
|
94 {
|
|
|
95 cpTimestamp stamp = space.stamp;
|
|
|
96
|
|
|
97 cpContactBufferHeader *head = space.contactBuffersHead;
|
|
|
98
|
|
|
99 if(!head){
|
|
|
100 // No buffers have been allocated, make one
|
|
|
101 space.contactBuffersHead = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, null);
|
|
|
102 } else if(stamp - head.next.stamp > cp_contact_persistence){
|
|
|
103 // The tail buffer is available, rotate the ring
|
|
|
104 cpContactBufferHeader *tail = head.next;
|
|
|
105 space.contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail);
|
|
|
106 } else {
|
|
|
107 // Allocate a new buffer and push it into the ring
|
|
|
108 cpContactBufferHeader *buffer = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, head);
|
|
|
109 space.contactBuffersHead = head.next = buffer;
|
|
|
110 }
|
|
|
111 }
|
|
|
112
|
|
|
113
|
|
|
114 static cpContact *
|
|
|
115 cpContactBufferGetArray(cpSpace *space)
|
|
|
116 {
|
|
|
117 if(space.contactBuffersHead.numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){
|
|
|
118 // contact buffer could overflow on the next collision, push a fresh one.
|
|
|
119 cpSpacePushFreshContactBuffer(space);
|
|
|
120 }
|
|
|
121
|
|
|
122 cpContactBufferHeader *head = space.contactBuffersHead;
|
|
13
|
123 return &(cast(cpContactBuffer *)head).contacts[head.numContacts];
|
|
4
|
124 }
|
|
|
125
|
|
|
126 static void
|
|
|
127 cpSpacePushContacts(cpSpace *space, int count){
|
|
|
128 assert(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal error, too many contact point overflow!");
|
|
|
129 space.contactBuffersHead.numContacts += count;
|
|
|
130 }
|
|
|
131
|
|
|
132 static void
|
|
|
133 cpSpacePopContacts(cpSpace *space, int count){
|
|
|
134 space.contactBuffersHead.numContacts -= count;
|
|
|
135 }
|
|
|
136
|
|
|
137 //#pragma mark Collision Detection Functions
|
|
|
138
|
|
|
139 static cpBool
|
|
|
140 queryReject(cpShape *a, cpShape *b)
|
|
|
141 {
|
|
|
142 return
|
|
|
143 // BBoxes must overlap
|
|
|
144 !cpBBintersects(a.bb, b.bb)
|
|
|
145 // Don't collide shapes attached to the same body.
|
|
|
146 || a._body == b._body
|
|
|
147 // Don't collide objects in the same non-zero group
|
|
|
148 || (a.group && a.group == b.group)
|
|
|
149 // Don't collide objects that don't share at least on layer.
|
|
|
150 || !(a.layers & b.layers);
|
|
|
151 }
|
|
|
152
|
|
|
153 // Callback from the spatial hash.
|
|
|
154 static void
|
|
|
155 queryFunc(cpShape *a, cpShape *b, cpSpace *space)
|
|
|
156 {
|
|
|
157 // Reject any of the simple cases
|
|
|
158 if(queryReject(a,b)) return;
|
|
|
159
|
|
|
160 // Find the collision pair function for the shapes.
|
|
|
161 struct tmp{cpCollisionType a, b;} tmp ids = {a.collision_type, b.collision_type};
|
|
|
162 cpHashValue collHashID = CP_HASH_PAIR(a.collision_type, b.collision_type);
|
|
|
163 cpCollisionHandler *handler = cast(cpCollisionHandler *)cpHashSetFind(space.collFuncSet, collHashID, &ids);
|
|
|
164
|
|
|
165 cpBool sensor = a.sensor || b.sensor;
|
|
|
166 if(sensor && handler == &space.defaultHandler) return;
|
|
|
167
|
|
|
168 // Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
|
|
|
169 if(a.klass.type > b.klass.type){
|
|
|
170 cpShape *temp = a;
|
|
|
171 a = b;
|
|
|
172 b = temp;
|
|
|
173 }
|
|
|
174
|
|
|
175 // Narrow-phase collision detection.
|
|
|
176 cpContact *contacts = cpContactBufferGetArray(space);
|
|
|
177 int numContacts = cpCollideShapes(a, b, contacts);
|
|
|
178 if(!numContacts) return; // Shapes are not colliding.
|
|
|
179 cpSpacePushContacts(space, numContacts);
|
|
|
180
|
|
|
181 // Get an arbiter from space.contactSet for the two shapes.
|
|
|
182 // This is where the persistant contact magic comes from.
|
|
|
183 cpShape *shape_pair[] = [a, b];
|
|
|
184 cpHashValue arbHashID = CP_HASH_PAIR(cast(size_t)a, cast(size_t)b);
|
|
|
185 cpArbiter *arb = cast(cpArbiter *)cpHashSetInsert(space.contactSet, arbHashID, shape_pair.ptr, space);
|
|
|
186 cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); // retains the contacts array
|
|
|
187
|
|
|
188 // Call the begin function first if it's the first step
|
|
|
189 if(arb.state == cpArbiterState.cpArbiterStateFirstColl && !handler.begin(arb, space, handler.data)){
|
|
|
190 cpArbiterIgnore(arb); // permanently ignore the collision until separation
|
|
|
191 }
|
|
|
192
|
|
|
193 if(
|
|
|
194 // Ignore the arbiter if it has been flagged
|
|
|
195 (arb.state != cpArbiterState.cpArbiterStateIgnore) &&
|
|
|
196 // Call preSolve
|
|
|
197 handler.preSolve(arb, space, handler.data) &&
|
|
|
198 // Process, but don't add collisions for sensors.
|
|
|
199 !sensor
|
|
|
200 ){
|
|
|
201 cpArrayPush(space.arbiters, arb);
|
|
|
202 } else {
|
|
|
203 cpSpacePopContacts(space, numContacts);
|
|
|
204
|
|
|
205 arb.contacts = null;
|
|
|
206 arb.numContacts = 0;
|
|
|
207
|
|
|
208 // Normally arbiters are set as used after calling the post-step callback.
|
|
|
209 // However, post-step callbacks are not called for sensors or arbiters rejected from pre-solve.
|
|
|
210 if(arb.state != cpArbiterState.cpArbiterStateIgnore) arb.state = cpArbiterState.cpArbiterStateNormal;
|
|
|
211 }
|
|
|
212
|
|
|
213 // Time stamp the arbiter so we know it was used recently.
|
|
|
214 arb.stamp = space.stamp;
|
|
|
215 }
|
|
|
216
|
|
|
217 // Iterator for active/static hash collisions.
|
|
|
218 static void
|
|
|
219 active2staticIter(cpShape *shape, cpSpace *space)
|
|
|
220 {
|
|
|
221 cpSpaceHashQuery(space.staticShapes, shape, shape.bb, cast(cpSpaceHashQueryFunc)&queryFunc, space);
|
|
|
222 }
|
|
|
223
|
|
|
224 // Hashset filter func to throw away old arbiters.
|
|
|
225 static cpBool
|
|
|
226 contactSetFilter(cpArbiter *arb, cpSpace *space)
|
|
|
227 {
|
|
|
228 if(space.sleepTimeThreshold != INFINITY){
|
|
23
|
229 cpBody *a = arb.a._body;
|
|
|
230 cpBody *b = arb.b._body;
|
|
4
|
231
|
|
|
232 // both bodies are either static or sleeping
|
|
|
233 cpBool sleepingNow =
|
|
|
234 (cpBodyIsStatic(a) || cpBodyIsSleeping(a)) &&
|
|
|
235 (cpBodyIsStatic(b) || cpBodyIsSleeping(b));
|
|
|
236
|
|
|
237 if(sleepingNow){
|
|
|
238 arb.state = cpArbiterState.cpArbiterStateSleep;
|
|
|
239 return cpTrue;
|
|
|
240 } else if(arb.state == cpArbiterState.cpArbiterStateSleep){
|
|
|
241 // wake up the arbiter and continue as normal
|
|
|
242 arb.state = cpArbiterState.cpArbiterStateNormal;
|
|
|
243 // TODO is it possible that cpArbiterStateIgnore should be set here instead?
|
|
|
244 }
|
|
|
245 }
|
|
|
246
|
|
|
247 cpTimestamp ticks = space.stamp - arb.stamp;
|
|
|
248
|
|
|
249 // was used last frame, but not this one
|
|
|
250 if(ticks >= 1 && arb.state != cpArbiterState.cpArbiterStateCached){
|
|
|
251 arb.handler.separate(arb, space, arb.handler.data);
|
|
|
252 arb.state = cpArbiterState.cpArbiterStateCached;
|
|
|
253 }
|
|
|
254
|
|
|
255 if(ticks >= cp_contact_persistence){
|
|
|
256 arb.contacts = null;
|
|
|
257 arb.numContacts = 0;
|
|
|
258
|
|
|
259 cpArrayPush(space.pooledArbiters, arb);
|
|
|
260 return cpFalse;
|
|
|
261 }
|
|
|
262
|
|
|
263 return cpTrue;
|
|
|
264 }
|
|
|
265
|
|
|
266 // Hashset filter func to call and throw away post step callbacks.
|
|
|
267 static void
|
|
29
|
268 postStepCallbackSetIter(PostStepCallback *callback, cpSpace *space)
|
|
4
|
269 {
|
|
|
270 callback.func(space, callback.obj, callback.data);
|
|
|
271 cpfree(callback);
|
|
|
272 }
|
|
|
273
|
|
|
274 //#pragma mark All Important cpSpaceStep() Function
|
|
|
275
|
|
|
276 //void cpSpaceProcessComponents(cpSpace *space, cpFloat dt);
|
|
|
277
|
|
|
278 static void updateBBCache(cpShape *shape, void *unused){cpShapeCacheBB(shape);}
|
|
|
279
|
|
|
280 void
|
|
|
281 cpSpaceStep(cpSpace *space, cpFloat dt)
|
|
|
282 {
|
|
|
283 if(!dt) return; // don't step if the timestep is 0!
|
|
|
284 cpFloat dt_inv = 1.0f/dt;
|
|
|
285
|
|
|
286 cpArray *bodies = space.bodies;
|
|
|
287 cpArray *constraints = space.constraints;
|
|
|
288
|
|
|
289 // Empty the arbiter list.
|
|
|
290 space.arbiters.num = 0;
|
|
|
291
|
|
|
292 // Integrate positions.
|
|
|
293 for(int i=0; i<bodies.num; i++){
|
|
|
294 cpBody *_body = cast(cpBody *)bodies.arr[i];
|
|
|
295 _body.position_func(_body, dt);
|
|
|
296 }
|
|
|
297
|
|
|
298 // Pre-cache BBoxes and shape data.
|
|
|
299 cpSpaceHashEach(space.activeShapes, cast(cpSpaceHashIterator)&updateBBCache, null);
|
|
|
300
|
|
29
|
301 cpSpaceLock(space);
|
|
23
|
302
|
|
4
|
303 // Collide!
|
|
|
304 cpSpacePushFreshContactBuffer(space);
|
|
|
305 if(space.staticShapes.handleSet.entries)
|
|
|
306 cpSpaceHashEach(space.activeShapes, cast(cpSpaceHashIterator)&active2staticIter, space);
|
|
|
307 cpSpaceHashQueryRehash(space.activeShapes, cast(cpSpaceHashQueryFunc)&queryFunc, space);
|
|
|
308
|
|
29
|
309 cpSpaceUnlock(space);
|
|
23
|
310
|
|
4
|
311 // If body sleeping is enabled, do that now.
|
|
|
312 if(space.sleepTimeThreshold != INFINITY){
|
|
|
313 cpSpaceProcessComponents(space, dt);
|
|
|
314 bodies = space.bodies; // rebuilt by processContactComponents()
|
|
|
315 }
|
|
|
316
|
|
|
317 // Clear out old cached arbiters and dispatch untouch functions
|
|
|
318 cpHashSetFilter(space.contactSet, cast(cpHashSetFilterFunc)&contactSetFilter, space);
|
|
|
319
|
|
|
320 // Prestep the arbiters.
|
|
|
321 cpArray *arbiters = space.arbiters;
|
|
|
322 for(int i=0; i<arbiters.num; i++)
|
|
|
323 cpArbiterPreStep(cast(cpArbiter *)arbiters.arr[i], dt_inv);
|
|
|
324
|
|
|
325 // Prestep the constraints.
|
|
|
326 for(int i=0; i<constraints.num; i++){
|
|
|
327 cpConstraint *constraint = cast(cpConstraint *)constraints.arr[i];
|
|
|
328 constraint.klass.preStep(constraint, dt, dt_inv);
|
|
|
329 }
|
|
|
330
|
|
|
331 for(int i=0; i<space.elasticIterations; i++){
|
|
|
332 for(int j=0; j<arbiters.num; j++)
|
|
|
333 cpArbiterApplyImpulse(cast(cpArbiter *)arbiters.arr[j], 1.0f);
|
|
|
334
|
|
|
335 for(int j=0; j<constraints.num; j++){
|
|
|
336 cpConstraint *constraint = cast(cpConstraint *)constraints.arr[j];
|
|
|
337 constraint.klass.applyImpulse(constraint);
|
|
|
338 }
|
|
|
339 }
|
|
|
340
|
|
|
341 // Integrate velocities.
|
|
|
342 cpFloat damping = cpfpow(1.0f/space.damping, -dt);
|
|
|
343 for(int i=0; i<bodies.num; i++){
|
|
|
344 cpBody *_body = cast(cpBody *)bodies.arr[i];
|
|
|
345 _body.velocity_func(_body, space.gravity, damping, dt);
|
|
|
346 }
|
|
|
347
|
|
|
348 for(int i=0; i<arbiters.num; i++)
|
|
|
349 cpArbiterApplyCachedImpulse(cast(cpArbiter *)arbiters.arr[i]);
|
|
|
350
|
|
|
351 // run the old-style elastic solver if elastic iterations are disabled
|
|
|
352 cpFloat elasticCoef = (space.elasticIterations ? 0.0f : 1.0f);
|
|
|
353
|
|
|
354 // Run the impulse solver.
|
|
|
355 for(int i=0; i<space.iterations; i++){
|
|
|
356 for(int j=0; j<arbiters.num; j++)
|
|
|
357 cpArbiterApplyImpulse(cast(cpArbiter *)arbiters.arr[j], elasticCoef);
|
|
|
358
|
|
|
359 for(int j=0; j<constraints.num; j++){
|
|
|
360 cpConstraint *constraint = cast(cpConstraint *)constraints.arr[j];
|
|
|
361 constraint.klass.applyImpulse(constraint);
|
|
|
362 }
|
|
|
363 }
|
|
|
364
|
|
29
|
365 cpSpaceLock(space);
|
|
4
|
366
|
|
|
367 // run the post solve callbacks
|
|
|
368 for(int i=0; i<arbiters.num; i++){
|
|
|
369 cpArbiter *arb = cast(cpArbiter *) arbiters.arr[i];
|
|
|
370
|
|
|
371 cpCollisionHandler *handler = arb.handler;
|
|
|
372 handler.postSolve(arb, space, handler.data);
|
|
|
373
|
|
|
374 arb.state = cpArbiterState.cpArbiterStateNormal;
|
|
|
375 }
|
|
|
376
|
|
29
|
377 cpSpaceUnlock(space);
|
|
23
|
378
|
|
4
|
379 // Run the post step callbacks
|
|
|
380 // Loop because post step callbacks may create more post step callbacks
|
|
|
381 while(space.postStepCallbacks){
|
|
|
382 cpHashSet *callbacks = space.postStepCallbacks;
|
|
|
383 space.postStepCallbacks = null;
|
|
|
384
|
|
|
385 cpHashSetEach(callbacks, cast(cpHashSetIterFunc)&postStepCallbackSetIter, space);
|
|
|
386 cpHashSetFree(callbacks);
|
|
|
387 }
|
|
|
388
|
|
|
389 // Increment the stamp.
|
|
|
390 space.stamp++;
|
|
|
391 }
|
