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