physics library version update
[spacedolphin:spacedolphin.git] / lib / chipmunk / src / cpSpaceStep.c
1 /* Copyright (c) 2007 Scott Lembcke
2  * 
3  * Permission is hereby granted, free of charge, to any person obtaining a copy
4  * of this software and associated documentation files (the "Software"), to deal
5  * in the Software without restriction, including without limitation the rights
6  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7  * copies of the Software, and to permit persons to whom the Software is
8  * furnished to do so, subject to the following conditions:
9  * 
10  * The above copyright notice and this permission notice shall be included in
11  * all copies or substantial portions of the Software.
12  * 
13  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19  * SOFTWARE.
20  */
21  
22 #include "chipmunk_private.h"
23
24 //MARK: Post Step Callback Functions
25
26 cpPostStepCallback *
27 cpSpaceGetPostStepCallback(cpSpace *space, void *key)
28 {
29         cpArray *arr = space->postStepCallbacks;
30         for(int i=0; i<arr->num; i++){
31                 cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i];
32                 if(callback && callback->key == key) return callback;
33         }
34         
35         return NULL;
36 }
37
38 static void PostStepDoNothing(cpSpace *space, void *obj, void *data){}
39
40 cpBool
41 cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, void *data)
42 {
43         cpAssertWarn(space->locked,
44                 "Adding a post-step callback when the space is not locked is unnecessary. "
45                 "Post-step callbacks will not called until the end of the next call to cpSpaceStep() or the next query.");
46         
47         if(!cpSpaceGetPostStepCallback(space, key)){
48                 cpPostStepCallback *callback = (cpPostStepCallback *)cpcalloc(1, sizeof(cpPostStepCallback));
49                 callback->func = (func ? func : PostStepDoNothing);
50                 callback->key = key;
51                 callback->data = data;
52                 
53                 cpArrayPush(space->postStepCallbacks, callback);
54                 return cpTrue;
55         } else {
56                 return cpFalse;
57         }
58 }
59
60 //MARK: Locking Functions
61
62 void
63 cpSpaceLock(cpSpace *space)
64 {
65         space->locked++;
66 }
67
68 void
69 cpSpaceUnlock(cpSpace *space, cpBool runPostStep)
70 {
71         space->locked--;
72         cpAssertHard(space->locked >= 0, "Internal Error: Space lock underflow.");
73         
74         if(space->locked == 0){
75                 cpArray *waking = space->rousedBodies;
76                 
77                 for(int i=0, count=waking->num; i<count; i++){
78                         cpSpaceActivateBody(space, (cpBody *)waking->arr[i]);
79                         waking->arr[i] = NULL;
80                 }
81                 
82                 waking->num = 0;
83                 
84                 if(space->locked == 0 && runPostStep && !space->skipPostStep){
85                         space->skipPostStep = cpTrue;
86                         
87                         cpArray *arr = space->postStepCallbacks;
88                         for(int i=0; i<arr->num; i++){
89                                 cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i];
90                                 cpPostStepFunc func = callback->func;
91                                 
92                                 // Mark the func as NULL in case calling it calls cpSpaceRunPostStepCallbacks() again.
93                                 // TODO need more tests around this case I think.
94                                 callback->func = NULL;
95                                 if(func) func(space, callback->key, callback->data);
96                                 
97                                 arr->arr[i] = NULL;
98                                 cpfree(callback);
99                         }
100                         
101                         arr->num = 0;
102                         space->skipPostStep = cpFalse;
103                 }
104         }
105 }
106
107 //MARK: Contact Buffer Functions
108
109 struct cpContactBufferHeader {
110         cpTimestamp stamp;
111         cpContactBufferHeader *next;
112         unsigned int numContacts;
113 };
114
115 #define CP_CONTACTS_BUFFER_SIZE ((CP_BUFFER_BYTES - sizeof(cpContactBufferHeader))/sizeof(cpContact))
116 typedef struct cpContactBuffer {
117         cpContactBufferHeader header;
118         cpContact contacts[CP_CONTACTS_BUFFER_SIZE];
119 } cpContactBuffer;
120
121 static cpContactBufferHeader *
122 cpSpaceAllocContactBuffer(cpSpace *space)
123 {
124         cpContactBuffer *buffer = (cpContactBuffer *)cpcalloc(1, sizeof(cpContactBuffer));
125         cpArrayPush(space->allocatedBuffers, buffer);
126         return (cpContactBufferHeader *)buffer;
127 }
128
129 static cpContactBufferHeader *
130 cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpContactBufferHeader *splice)
131 {
132         header->stamp = stamp;
133         header->next = (splice ? splice->next : header);
134         header->numContacts = 0;
135         
136         return header;
137 }
138
139 void
140 cpSpacePushFreshContactBuffer(cpSpace *space)
141 {
142         cpTimestamp stamp = space->stamp;
143         
144         cpContactBufferHeader *head = space->contactBuffersHead;
145         
146         if(!head){
147                 // No buffers have been allocated, make one
148                 space->contactBuffersHead = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, NULL);
149         } else if(stamp - head->next->stamp > space->collisionPersistence){
150                 // The tail buffer is available, rotate the ring
151         cpContactBufferHeader *tail = head->next;
152                 space->contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail);
153         } else {
154                 // Allocate a new buffer and push it into the ring
155                 cpContactBufferHeader *buffer = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, head);
156                 space->contactBuffersHead = head->next = buffer;
157         }
158 }
159
160
161 cpContact *
162 cpContactBufferGetArray(cpSpace *space)
163 {
164         if(space->contactBuffersHead->numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){
165                 // contact buffer could overflow on the next collision, push a fresh one.
166                 cpSpacePushFreshContactBuffer(space);
167         }
168         
169         cpContactBufferHeader *head = space->contactBuffersHead;
170         return ((cpContactBuffer *)head)->contacts + head->numContacts;
171 }
172
173 void
174 cpSpacePushContacts(cpSpace *space, int count)
175 {
176         cpAssertHard(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal Error: Contact buffer overflow!");
177         space->contactBuffersHead->numContacts += count;
178 }
179
180 static void
181 cpSpacePopContacts(cpSpace *space, int count){
182         space->contactBuffersHead->numContacts -= count;
183 }
184
185 //MARK: Collision Detection Functions
186
187 static void *
188 cpSpaceArbiterSetTrans(cpShape **shapes, cpSpace *space)
189 {
190         if(space->pooledArbiters->num == 0){
191                 // arbiter pool is exhausted, make more
192                 int count = CP_BUFFER_BYTES/sizeof(cpArbiter);
193                 cpAssertHard(count, "Internal Error: Buffer size too small.");
194                 
195                 cpArbiter *buffer = (cpArbiter *)cpcalloc(1, CP_BUFFER_BYTES);
196                 cpArrayPush(space->allocatedBuffers, buffer);
197                 
198                 for(int i=0; i<count; i++) cpArrayPush(space->pooledArbiters, buffer + i);
199         }
200         
201         return cpArbiterInit((cpArbiter *)cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
202 }
203
204 static inline cpBool
205 queryReject(cpShape *a, cpShape *b)
206 {
207         return (
208                 // BBoxes must overlap
209                 !cpBBIntersects(a->bb, b->bb)
210                 // Don't collide shapes attached to the same body.
211                 || a->body == b->body
212                 // Don't collide objects in the same non-zero group
213                 || (a->group && a->group == b->group)
214                 // Don't collide objects that don't share at least on layer.
215                 || !(a->layers & b->layers)
216                 // Don't collide infinite mass objects
217                 || (a->body->m == INFINITY && b->body->m == INFINITY)
218         );
219 }
220
221 // Callback from the spatial hash.
222 void
223 cpSpaceCollideShapes(cpShape *a, cpShape *b, cpSpace *space)
224 {
225         // Reject any of the simple cases
226         if(queryReject(a,b)) return;
227         
228         cpCollisionHandler *handler = cpSpaceLookupHandler(space, a->collision_type, b->collision_type);
229         
230         cpBool sensor = a->sensor || b->sensor;
231         if(sensor && handler == &cpDefaultCollisionHandler) return;
232         
233         // Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
234         if(a->klass->type > b->klass->type){
235                 cpShape *temp = a;
236                 a = b;
237                 b = temp;
238         }
239         
240         // Narrow-phase collision detection.
241         cpContact *contacts = cpContactBufferGetArray(space);
242         int numContacts = cpCollideShapes(a, b, contacts);
243         if(!numContacts) return; // Shapes are not colliding.
244         cpSpacePushContacts(space, numContacts);
245         
246         // Get an arbiter from space->arbiterSet for the two shapes.
247         // This is where the persistant contact magic comes from.
248         cpShape *shape_pair[] = {a, b};
249         cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b);
250         cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, space, (cpHashSetTransFunc)cpSpaceArbiterSetTrans);
251         cpArbiterUpdate(arb, contacts, numContacts, handler, a, b);
252         
253         // Call the begin function first if it's the first step
254         if(arb->state == cpArbiterStateFirstColl && !handler->begin(arb, space, handler->data)){
255                 cpArbiterIgnore(arb); // permanently ignore the collision until separation
256         }
257         
258         if(
259                 // Ignore the arbiter if it has been flagged
260                 (arb->state != cpArbiterStateIgnore) && 
261                 // Call preSolve
262                 handler->preSolve(arb, space, handler->data) &&
263                 // Process, but don't add collisions for sensors.
264                 !sensor
265         ){
266                 cpArrayPush(space->arbiters, arb);
267         } else {
268                 cpSpacePopContacts(space, numContacts);
269                 
270                 arb->contacts = NULL;
271                 arb->numContacts = 0;
272                 
273                 // Normally arbiters are set as used after calling the post-solve callback.
274                 // However, post-solve callbacks are not called for sensors or arbiters rejected from pre-solve.
275                 if(arb->state != cpArbiterStateIgnore) arb->state = cpArbiterStateNormal;
276         }
277         
278         // Time stamp the arbiter so we know it was used recently.
279         arb->stamp = space->stamp;
280 }
281
282 // Hashset filter func to throw away old arbiters.
283 cpBool
284 cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space)
285 {
286         cpTimestamp ticks = space->stamp - arb->stamp;
287         
288         cpBody *a = arb->body_a, *b = arb->body_b;
289         
290         // TODO should make an arbiter state for this so it doesn't require filtering arbiters for dangling body pointers on body removal.
291         // Preserve arbiters on sensors and rejected arbiters for sleeping objects.
292         // This prevents errant separate callbacks from happenening.
293         if(
294                 (cpBodyIsStatic(a) || cpBodyIsSleeping(a)) &&
295                 (cpBodyIsStatic(b) || cpBodyIsSleeping(b))
296         ){
297                 return cpTrue;
298         }
299         
300         // Arbiter was used last frame, but not this one
301         if(ticks >= 1 && arb->state != cpArbiterStateCached){
302                 arb->state = cpArbiterStateCached;
303                 cpArbiterCallSeparate(arb, space);
304         }
305         
306         if(ticks >= space->collisionPersistence){
307                 arb->contacts = NULL;
308                 arb->numContacts = 0;
309                 
310                 cpArrayPush(space->pooledArbiters, arb);
311                 return cpFalse;
312         }
313         
314         return cpTrue;
315 }
316
317 //MARK: All Important cpSpaceStep() Function
318
319 void
320 cpShapeUpdateFunc(cpShape *shape, void *unused)
321 {
322         cpBody *body = shape->body;
323         cpShapeUpdate(shape, body->p, body->rot);
324 }
325
326 void
327 cpSpaceStep(cpSpace *space, cpFloat dt)
328 {
329         // don't step if the timestep is 0!
330         if(dt == 0.0f) return;
331         
332         space->stamp++;
333         
334         cpFloat prev_dt = space->curr_dt;
335         space->curr_dt = dt;
336                 
337         cpArray *bodies = space->bodies;
338         cpArray *constraints = space->constraints;
339         cpArray *arbiters = space->arbiters;
340         
341         // Reset and empty the arbiter lists.
342         for(int i=0; i<arbiters->num; i++){
343                 cpArbiter *arb = (cpArbiter *)arbiters->arr[i];
344                 arb->state = cpArbiterStateNormal;
345                 
346                 // If both bodies are awake, unthread the arbiter from the contact graph.
347                 if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)){
348                         cpArbiterUnthread(arb);
349                 }
350         }
351         arbiters->num = 0;
352
353         cpSpaceLock(space); {
354                 // Integrate positions
355                 for(int i=0; i<bodies->num; i++){
356                         cpBody *body = (cpBody *)bodies->arr[i];
357                         body->position_func(body, dt);
358                 }
359                 
360                 // Find colliding pairs.
361                 cpSpacePushFreshContactBuffer(space);
362                 cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL);
363                 cpSpatialIndexReindexQuery(space->activeShapes, (cpSpatialIndexQueryFunc)cpSpaceCollideShapes, space);
364         } cpSpaceUnlock(space, cpFalse);
365         
366         // Rebuild the contact graph (and detect sleeping components if sleeping is enabled)
367         cpSpaceProcessComponents(space, dt);
368         
369         cpSpaceLock(space); {
370                 // Clear out old cached arbiters and call separate callbacks
371                 cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space);
372
373                 // Prestep the arbiters and constraints.
374                 cpFloat slop = space->collisionSlop;
375                 cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt);
376                 for(int i=0; i<arbiters->num; i++){
377                         cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef);
378                 }
379
380                 for(int i=0; i<constraints->num; i++){
381                         cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
382                         
383                         cpConstraintPreSolveFunc preSolve = constraint->preSolve;
384                         if(preSolve) preSolve(constraint, space);
385                         
386                         constraint->klass->preStep(constraint, dt);
387                 }
388         
389                 // Integrate velocities.
390                 cpFloat damping = cpfpow(space->damping, dt);
391                 cpVect gravity = space->gravity;
392                 for(int i=0; i<bodies->num; i++){
393                         cpBody *body = (cpBody *)bodies->arr[i];
394                         body->velocity_func(body, gravity, damping, dt);
395                 }
396                 
397                 // Apply cached impulses
398                 cpFloat dt_coef = (prev_dt == 0.0f ? 0.0f : dt/prev_dt);
399                 for(int i=0; i<arbiters->num; i++){
400                         cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef);
401                 }
402                 
403                 for(int i=0; i<constraints->num; i++){
404                         cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
405                         constraint->klass->applyCachedImpulse(constraint, dt_coef);
406                 }
407                 
408                 // Run the impulse solver.
409                 for(int i=0; i<space->iterations; i++){
410                         for(int j=0; j<arbiters->num; j++){
411                                 cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j]);
412                         }
413                                 
414                         for(int j=0; j<constraints->num; j++){
415                                 cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
416                                 constraint->klass->applyImpulse(constraint, dt);
417                         }
418                 }
419                 
420                 // Run the constraint post-solve callbacks
421                 for(int i=0; i<constraints->num; i++){
422                         cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
423                         
424                         cpConstraintPostSolveFunc postSolve = constraint->postSolve;
425                         if(postSolve) postSolve(constraint, space);
426                 }
427                 
428                 // run the post-solve callbacks
429                 for(int i=0; i<arbiters->num; i++){
430                         cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
431                         
432                         cpCollisionHandler *handler = arb->handler;
433                         handler->postSolve(arb, space, handler->data);
434                 }
435         } cpSpaceUnlock(space, cpTrue);
436 }