chipmunk 4.1.0
Sign up to get free protection for your applications and to get access to all the features.
- data/Rakefile +40 -0
- data/ext/chipmunk/chipmunk.c +69 -0
- data/ext/chipmunk/chipmunk.h +91 -0
- data/ext/chipmunk/cpArbiter.c +263 -0
- data/ext/chipmunk/cpArbiter.h +85 -0
- data/ext/chipmunk/cpArray.c +114 -0
- data/ext/chipmunk/cpArray.h +45 -0
- data/ext/chipmunk/cpBB.c +46 -0
- data/ext/chipmunk/cpBB.h +53 -0
- data/ext/chipmunk/cpBody.c +180 -0
- data/ext/chipmunk/cpBody.h +132 -0
- data/ext/chipmunk/cpCollision.c +390 -0
- data/ext/chipmunk/cpCollision.h +23 -0
- data/ext/chipmunk/cpHashSet.c +219 -0
- data/ext/chipmunk/cpHashSet.h +79 -0
- data/ext/chipmunk/cpJoint.c +553 -0
- data/ext/chipmunk/cpJoint.h +122 -0
- data/ext/chipmunk/cpPolyShape.c +139 -0
- data/ext/chipmunk/cpPolyShape.h +92 -0
- data/ext/chipmunk/cpShape.c +244 -0
- data/ext/chipmunk/cpShape.h +141 -0
- data/ext/chipmunk/cpSpace.c +530 -0
- data/ext/chipmunk/cpSpace.h +120 -0
- data/ext/chipmunk/cpSpaceHash.c +455 -0
- data/ext/chipmunk/cpSpaceHash.h +100 -0
- data/ext/chipmunk/cpVect.c +63 -0
- data/ext/chipmunk/cpVect.h +106 -0
- data/ext/chipmunk/extconf.rb +4 -0
- data/ext/chipmunk/prime.h +68 -0
- data/ext/chipmunk/rb_chipmunk.c +109 -0
- data/ext/chipmunk/rb_chipmunk.h +77 -0
- data/ext/chipmunk/rb_cpBB.c +154 -0
- data/ext/chipmunk/rb_cpBody.c +239 -0
- data/ext/chipmunk/rb_cpJoint.c +136 -0
- data/ext/chipmunk/rb_cpShape.c +292 -0
- data/ext/chipmunk/rb_cpSpace.c +330 -0
- data/ext/chipmunk/rb_cpVect.c +250 -0
- data/lib/chipmunk.rb +15 -0
- metadata +92 -0
@@ -0,0 +1,530 @@
|
|
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 <stdlib.h>
|
23
|
+
#include <stdio.h>
|
24
|
+
#include <math.h>
|
25
|
+
#include <assert.h>
|
26
|
+
|
27
|
+
#include "chipmunk.h"
|
28
|
+
|
29
|
+
int cp_contact_persistence = 3;
|
30
|
+
|
31
|
+
// Equal function for contactSet.
|
32
|
+
static int
|
33
|
+
contactSetEql(void *ptr, void *elt)
|
34
|
+
{
|
35
|
+
cpShape **shapes = (cpShape **)ptr;
|
36
|
+
cpShape *a = shapes[0];
|
37
|
+
cpShape *b = shapes[1];
|
38
|
+
|
39
|
+
cpArbiter *arb = (cpArbiter *)elt;
|
40
|
+
|
41
|
+
return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b));
|
42
|
+
}
|
43
|
+
|
44
|
+
// Transformation function for contactSet.
|
45
|
+
static void *
|
46
|
+
contactSetTrans(void *ptr, void *data)
|
47
|
+
{
|
48
|
+
cpShape **shapes = (cpShape **)ptr;
|
49
|
+
cpShape *a = shapes[0];
|
50
|
+
cpShape *b = shapes[1];
|
51
|
+
|
52
|
+
cpSpace *space = (cpSpace *)data;
|
53
|
+
|
54
|
+
return cpArbiterNew(a, b, space->stamp);
|
55
|
+
}
|
56
|
+
|
57
|
+
// Collision pair function wrapper struct.
|
58
|
+
typedef struct collFuncData {
|
59
|
+
cpCollFunc func;
|
60
|
+
void *data;
|
61
|
+
} collFuncData;
|
62
|
+
|
63
|
+
// Equals function for collFuncSet.
|
64
|
+
static int
|
65
|
+
collFuncSetEql(void *ptr, void *elt)
|
66
|
+
{
|
67
|
+
unsigned int *ids = (unsigned int *)ptr;
|
68
|
+
unsigned int a = ids[0];
|
69
|
+
unsigned int b = ids[1];
|
70
|
+
|
71
|
+
cpCollPairFunc *pair = (cpCollPairFunc *)elt;
|
72
|
+
|
73
|
+
return ((a == pair->a && b == pair->b) || (b == pair->a && a == pair->b));
|
74
|
+
}
|
75
|
+
|
76
|
+
// Transformation function for collFuncSet.
|
77
|
+
static void *
|
78
|
+
collFuncSetTrans(void *ptr, void *data)
|
79
|
+
{
|
80
|
+
unsigned int *ids = (unsigned int *)ptr;
|
81
|
+
collFuncData *funcData = (collFuncData *)data;
|
82
|
+
|
83
|
+
cpCollPairFunc *pair = (cpCollPairFunc *)malloc(sizeof(cpCollPairFunc));
|
84
|
+
pair->a = ids[0];
|
85
|
+
pair->b = ids[1];
|
86
|
+
pair->func = funcData->func;
|
87
|
+
pair->data = funcData->data;
|
88
|
+
|
89
|
+
return pair;
|
90
|
+
}
|
91
|
+
|
92
|
+
// Default collision pair function.
|
93
|
+
static int
|
94
|
+
alwaysCollide(cpShape *a, cpShape *b, cpContact *arr, int numCon, cpFloat normal_coef, void *data)
|
95
|
+
{
|
96
|
+
return 1;
|
97
|
+
}
|
98
|
+
|
99
|
+
// BBfunc callback for the spatial hash.
|
100
|
+
static cpBB
|
101
|
+
bbfunc(void *ptr)
|
102
|
+
{
|
103
|
+
cpShape *shape = (cpShape *)ptr;
|
104
|
+
return shape->bb;
|
105
|
+
}
|
106
|
+
|
107
|
+
// Iterator functions for destructors.
|
108
|
+
static void freeWrap(void *ptr, void *unused){ free( ptr);}
|
109
|
+
static void shapeFreeWrap(void *ptr, void *unused){ cpShapeFree((cpShape *) ptr);}
|
110
|
+
static void arbiterFreeWrap(void *ptr, void *unused){ cpArbiterFree((cpArbiter *)ptr);}
|
111
|
+
static void bodyFreeWrap(void *ptr, void *unused){ cpBodyFree((cpBody *) ptr);}
|
112
|
+
static void jointFreeWrap(void *ptr, void *unused){ cpJointFree((cpJoint *) ptr);}
|
113
|
+
|
114
|
+
cpSpace*
|
115
|
+
cpSpaceAlloc(void)
|
116
|
+
{
|
117
|
+
return (cpSpace *)calloc(1, sizeof(cpSpace));
|
118
|
+
}
|
119
|
+
|
120
|
+
#define DEFAULT_DIM_SIZE 100.0f
|
121
|
+
#define DEFAULT_COUNT 1000
|
122
|
+
#define DEFAULT_ITERATIONS 10
|
123
|
+
#define DEFAULT_ELASTIC_ITERATIONS 0
|
124
|
+
|
125
|
+
cpSpace*
|
126
|
+
cpSpaceInit(cpSpace *space)
|
127
|
+
{
|
128
|
+
space->iterations = DEFAULT_ITERATIONS;
|
129
|
+
space->elasticIterations = DEFAULT_ELASTIC_ITERATIONS;
|
130
|
+
// space->sleepTicks = 300;
|
131
|
+
|
132
|
+
space->gravity = cpvzero;
|
133
|
+
space->damping = 1.0f;
|
134
|
+
|
135
|
+
space->stamp = 0;
|
136
|
+
|
137
|
+
space->staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, &bbfunc);
|
138
|
+
space->activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, &bbfunc);
|
139
|
+
|
140
|
+
space->bodies = cpArrayNew(0);
|
141
|
+
space->arbiters = cpArrayNew(0);
|
142
|
+
space->contactSet = cpHashSetNew(0, contactSetEql, contactSetTrans);
|
143
|
+
|
144
|
+
space->joints = cpArrayNew(0);
|
145
|
+
|
146
|
+
cpCollPairFunc pairFunc = {0, 0, alwaysCollide, NULL};
|
147
|
+
space->defaultPairFunc = pairFunc;
|
148
|
+
space->collFuncSet = cpHashSetNew(0, collFuncSetEql, collFuncSetTrans);
|
149
|
+
space->collFuncSet->default_value = &space->defaultPairFunc;
|
150
|
+
|
151
|
+
return space;
|
152
|
+
}
|
153
|
+
|
154
|
+
cpSpace*
|
155
|
+
cpSpaceNew(void)
|
156
|
+
{
|
157
|
+
return cpSpaceInit(cpSpaceAlloc());
|
158
|
+
}
|
159
|
+
|
160
|
+
void
|
161
|
+
cpSpaceDestroy(cpSpace *space)
|
162
|
+
{
|
163
|
+
cpSpaceHashFree(space->staticShapes);
|
164
|
+
cpSpaceHashFree(space->activeShapes);
|
165
|
+
|
166
|
+
cpArrayFree(space->bodies);
|
167
|
+
|
168
|
+
cpArrayFree(space->joints);
|
169
|
+
|
170
|
+
if(space->contactSet)
|
171
|
+
cpHashSetEach(space->contactSet, &arbiterFreeWrap, NULL);
|
172
|
+
cpHashSetFree(space->contactSet);
|
173
|
+
cpArrayFree(space->arbiters);
|
174
|
+
|
175
|
+
if(space->collFuncSet)
|
176
|
+
cpHashSetEach(space->collFuncSet, &freeWrap, NULL);
|
177
|
+
cpHashSetFree(space->collFuncSet);
|
178
|
+
}
|
179
|
+
|
180
|
+
void
|
181
|
+
cpSpaceFree(cpSpace *space)
|
182
|
+
{
|
183
|
+
if(space) cpSpaceDestroy(space);
|
184
|
+
free(space);
|
185
|
+
}
|
186
|
+
|
187
|
+
void
|
188
|
+
cpSpaceFreeChildren(cpSpace *space)
|
189
|
+
{
|
190
|
+
cpSpaceHashEach(space->staticShapes, &shapeFreeWrap, NULL);
|
191
|
+
cpSpaceHashEach(space->activeShapes, &shapeFreeWrap, NULL);
|
192
|
+
cpArrayEach(space->bodies, &bodyFreeWrap, NULL);
|
193
|
+
cpArrayEach(space->joints, &jointFreeWrap, NULL);
|
194
|
+
}
|
195
|
+
|
196
|
+
void
|
197
|
+
cpSpaceAddCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b,
|
198
|
+
cpCollFunc func, void *data)
|
199
|
+
{
|
200
|
+
unsigned int ids[] = {a, b};
|
201
|
+
unsigned int hash = CP_HASH_PAIR(a, b);
|
202
|
+
// Remove any old function so the new one will get added.
|
203
|
+
cpSpaceRemoveCollisionPairFunc(space, a, b);
|
204
|
+
|
205
|
+
collFuncData funcData = {func, data};
|
206
|
+
cpHashSetInsert(space->collFuncSet, hash, ids, &funcData);
|
207
|
+
}
|
208
|
+
|
209
|
+
void
|
210
|
+
cpSpaceRemoveCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b)
|
211
|
+
{
|
212
|
+
unsigned int ids[] = {a, b};
|
213
|
+
unsigned int hash = CP_HASH_PAIR(a, b);
|
214
|
+
cpCollPairFunc *old_pair = (cpCollPairFunc *)cpHashSetRemove(space->collFuncSet, hash, ids);
|
215
|
+
free(old_pair);
|
216
|
+
}
|
217
|
+
|
218
|
+
void
|
219
|
+
cpSpaceSetDefaultCollisionPairFunc(cpSpace *space, cpCollFunc func, void *data)
|
220
|
+
{
|
221
|
+
cpCollPairFunc pairFunc = {0, 0, (func ? func : alwaysCollide), (func ? data : NULL)};
|
222
|
+
space->defaultPairFunc = pairFunc;
|
223
|
+
}
|
224
|
+
|
225
|
+
void
|
226
|
+
cpSpaceAddShape(cpSpace *space, cpShape *shape)
|
227
|
+
{
|
228
|
+
cpSpaceHashInsert(space->activeShapes, shape, shape->id, shape->bb);
|
229
|
+
}
|
230
|
+
|
231
|
+
void
|
232
|
+
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
|
233
|
+
{
|
234
|
+
cpShapeCacheBB(shape);
|
235
|
+
cpSpaceHashInsert(space->staticShapes, shape, shape->id, shape->bb);
|
236
|
+
}
|
237
|
+
|
238
|
+
void
|
239
|
+
cpSpaceAddBody(cpSpace *space, cpBody *body)
|
240
|
+
{
|
241
|
+
cpArrayPush(space->bodies, body);
|
242
|
+
}
|
243
|
+
|
244
|
+
void
|
245
|
+
cpSpaceAddJoint(cpSpace *space, cpJoint *joint)
|
246
|
+
{
|
247
|
+
cpArrayPush(space->joints, joint);
|
248
|
+
}
|
249
|
+
|
250
|
+
void
|
251
|
+
cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
|
252
|
+
{
|
253
|
+
cpSpaceHashRemove(space->activeShapes, shape, shape->id);
|
254
|
+
}
|
255
|
+
|
256
|
+
void
|
257
|
+
cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
|
258
|
+
{
|
259
|
+
cpSpaceHashRemove(space->staticShapes, shape, shape->id);
|
260
|
+
}
|
261
|
+
|
262
|
+
void
|
263
|
+
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
|
264
|
+
{
|
265
|
+
cpArrayDeleteObj(space->bodies, body);
|
266
|
+
}
|
267
|
+
|
268
|
+
void
|
269
|
+
cpSpaceRemoveJoint(cpSpace *space, cpJoint *joint)
|
270
|
+
{
|
271
|
+
cpArrayDeleteObj(space->joints, joint);
|
272
|
+
}
|
273
|
+
|
274
|
+
void
|
275
|
+
cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data)
|
276
|
+
{
|
277
|
+
cpArray *bodies = space->bodies;
|
278
|
+
|
279
|
+
for(int i=0; i<bodies->num; i++)
|
280
|
+
func((cpBody *)bodies->arr[i], data);
|
281
|
+
}
|
282
|
+
|
283
|
+
// Iterator function used for updating shape BBoxes.
|
284
|
+
static void
|
285
|
+
updateBBCache(void *ptr, void *unused)
|
286
|
+
{
|
287
|
+
cpShape *shape = (cpShape *)ptr;
|
288
|
+
cpShapeCacheBB(shape);
|
289
|
+
}
|
290
|
+
|
291
|
+
void
|
292
|
+
cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count)
|
293
|
+
{
|
294
|
+
cpSpaceHashResize(space->staticShapes, dim, count);
|
295
|
+
cpSpaceHashRehash(space->staticShapes);
|
296
|
+
}
|
297
|
+
|
298
|
+
void
|
299
|
+
cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count)
|
300
|
+
{
|
301
|
+
cpSpaceHashResize(space->activeShapes, dim, count);
|
302
|
+
}
|
303
|
+
|
304
|
+
void
|
305
|
+
cpSpaceRehashStatic(cpSpace *space)
|
306
|
+
{
|
307
|
+
cpSpaceHashEach(space->staticShapes, &updateBBCache, NULL);
|
308
|
+
cpSpaceHashRehash(space->staticShapes);
|
309
|
+
}
|
310
|
+
|
311
|
+
typedef struct pointQueryFuncPair {
|
312
|
+
cpSpacePointQueryFunc func;
|
313
|
+
void *data;
|
314
|
+
} pointQueryFuncPair;
|
315
|
+
|
316
|
+
static int
|
317
|
+
pointQueryHelper(void *point, void *obj, void *data)
|
318
|
+
{
|
319
|
+
cpShape *shape = (cpShape *)obj;
|
320
|
+
pointQueryFuncPair *pair = (pointQueryFuncPair *)data;
|
321
|
+
|
322
|
+
if(cpShapePointQuery(shape, *((cpVect *)point)))
|
323
|
+
pair->func(shape, pair->data);
|
324
|
+
|
325
|
+
return 1; // return value needed for historical reasons (value is ignored)
|
326
|
+
}
|
327
|
+
|
328
|
+
static void
|
329
|
+
pointQuery(cpSpaceHash *hash, cpVect point, cpSpacePointQueryFunc func, void *data)
|
330
|
+
{
|
331
|
+
pointQueryFuncPair pair = {func, data};
|
332
|
+
cpSpaceHashPointQuery(hash, point, pointQueryHelper, &pair);
|
333
|
+
}
|
334
|
+
|
335
|
+
void
|
336
|
+
cpSpaceShapePointQuery(cpSpace *space, cpVect point, cpSpacePointQueryFunc func, void *data)
|
337
|
+
{
|
338
|
+
pointQuery(space->activeShapes, point, func, data);
|
339
|
+
}
|
340
|
+
|
341
|
+
void
|
342
|
+
cpSpaceStaticShapePointQuery(cpSpace *space, cpVect point, cpSpacePointQueryFunc func, void *data)
|
343
|
+
{
|
344
|
+
pointQuery(space->staticShapes, point, func, data);
|
345
|
+
}
|
346
|
+
|
347
|
+
static inline int
|
348
|
+
queryReject(cpShape *a, cpShape *b)
|
349
|
+
{
|
350
|
+
return
|
351
|
+
// BBoxes must overlap
|
352
|
+
!cpBBintersects(a->bb, b->bb)
|
353
|
+
// Don't collide shapes attached to the same body.
|
354
|
+
|| a->body == b->body
|
355
|
+
// Don't collide objects in the same non-zero group
|
356
|
+
|| (a->group && b->group && a->group == b->group)
|
357
|
+
// Don't collide objects that don't share at least on layer.
|
358
|
+
|| !(a->layers & b->layers);
|
359
|
+
}
|
360
|
+
|
361
|
+
// Callback from the spatial hash.
|
362
|
+
// TODO: Refactor this into separate functions?
|
363
|
+
static int
|
364
|
+
queryFunc(void *p1, void *p2, void *data)
|
365
|
+
{
|
366
|
+
// Cast the generic pointers from the spatial hash back to usefull types
|
367
|
+
cpShape *a = (cpShape *)p1;
|
368
|
+
cpShape *b = (cpShape *)p2;
|
369
|
+
cpSpace *space = (cpSpace *)data;
|
370
|
+
|
371
|
+
// Reject any of the simple cases
|
372
|
+
if(queryReject(a,b)) return 0;
|
373
|
+
|
374
|
+
// Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
|
375
|
+
if(a->klass->type > b->klass->type){
|
376
|
+
cpShape *temp = a;
|
377
|
+
a = b;
|
378
|
+
b = temp;
|
379
|
+
}
|
380
|
+
|
381
|
+
// Find the collision pair function for the shapes.
|
382
|
+
unsigned int ids[] = {a->collision_type, b->collision_type};
|
383
|
+
unsigned int hash = CP_HASH_PAIR(a->collision_type, b->collision_type);
|
384
|
+
cpCollPairFunc *pairFunc = (cpCollPairFunc *)cpHashSetFind(space->collFuncSet, hash, ids);
|
385
|
+
if(!pairFunc->func) return 0; // A NULL pair function means don't collide at all.
|
386
|
+
|
387
|
+
// Narrow-phase collision detection.
|
388
|
+
cpContact *contacts = NULL;
|
389
|
+
int numContacts = cpCollideShapes(a, b, &contacts);
|
390
|
+
if(!numContacts) return 0; // Shapes are not colliding.
|
391
|
+
|
392
|
+
// The collision pair function requires objects to be ordered by their collision types.
|
393
|
+
cpShape *pair_a = a;
|
394
|
+
cpShape *pair_b = b;
|
395
|
+
cpFloat normal_coef = 1.0f;
|
396
|
+
|
397
|
+
// Swap them if necessary.
|
398
|
+
if(pair_a->collision_type != pairFunc->a){
|
399
|
+
cpShape *temp = pair_a;
|
400
|
+
pair_a = pair_b;
|
401
|
+
pair_b = temp;
|
402
|
+
normal_coef = -1.0f;
|
403
|
+
}
|
404
|
+
|
405
|
+
if(pairFunc->func(pair_a, pair_b, contacts, numContacts, normal_coef, pairFunc->data)){
|
406
|
+
// The collision pair function OKed the collision. Record the contact information.
|
407
|
+
|
408
|
+
// Get an arbiter from space->contactSet for the two shapes.
|
409
|
+
// This is where the persistant contact magic comes from.
|
410
|
+
cpShape *shape_pair[] = {a, b};
|
411
|
+
cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, CP_HASH_PAIR(a, b), shape_pair, space);
|
412
|
+
|
413
|
+
// Timestamp the arbiter.
|
414
|
+
arb->stamp = space->stamp;
|
415
|
+
arb->a = a; arb->b = b; // TODO: Investigate why this is still necessary?
|
416
|
+
// Inject the new contact points into the arbiter.
|
417
|
+
cpArbiterInject(arb, contacts, numContacts);
|
418
|
+
|
419
|
+
// Add the arbiter to the list of active arbiters.
|
420
|
+
cpArrayPush(space->arbiters, arb);
|
421
|
+
|
422
|
+
return numContacts;
|
423
|
+
} else {
|
424
|
+
// The collision pair function rejected the collision.
|
425
|
+
|
426
|
+
free(contacts);
|
427
|
+
return 0;
|
428
|
+
}
|
429
|
+
}
|
430
|
+
|
431
|
+
// Iterator for active/static hash collisions.
|
432
|
+
static void
|
433
|
+
active2staticIter(void *ptr, void *data)
|
434
|
+
{
|
435
|
+
cpShape *shape = (cpShape *)ptr;
|
436
|
+
cpSpace *space = (cpSpace *)data;
|
437
|
+
cpSpaceHashQuery(space->staticShapes, shape, shape->bb, &queryFunc, space);
|
438
|
+
}
|
439
|
+
|
440
|
+
// Hashset reject func to throw away old arbiters.
|
441
|
+
static int
|
442
|
+
contactSetReject(void *ptr, void *data)
|
443
|
+
{
|
444
|
+
cpArbiter *arb = (cpArbiter *)ptr;
|
445
|
+
cpSpace *space = (cpSpace *)data;
|
446
|
+
|
447
|
+
if((space->stamp - arb->stamp) > cp_contact_persistence){
|
448
|
+
cpArbiterFree(arb);
|
449
|
+
return 0;
|
450
|
+
}
|
451
|
+
|
452
|
+
return 1;
|
453
|
+
}
|
454
|
+
|
455
|
+
void
|
456
|
+
cpSpaceStep(cpSpace *space, cpFloat dt)
|
457
|
+
{
|
458
|
+
if(!dt) return; // prevents div by zero.
|
459
|
+
cpFloat dt_inv = 1.0f/dt;
|
460
|
+
|
461
|
+
cpArray *bodies = space->bodies;
|
462
|
+
cpArray *arbiters = space->arbiters;
|
463
|
+
cpArray *joints = space->joints;
|
464
|
+
|
465
|
+
// Empty the arbiter list.
|
466
|
+
cpHashSetReject(space->contactSet, &contactSetReject, space);
|
467
|
+
space->arbiters->num = 0;
|
468
|
+
|
469
|
+
// Integrate positions.
|
470
|
+
for(int i=0; i<bodies->num; i++){
|
471
|
+
cpBody *body = (cpBody *)bodies->arr[i];
|
472
|
+
body->position_func(body, dt);
|
473
|
+
}
|
474
|
+
|
475
|
+
// Pre-cache BBoxes and shape data.
|
476
|
+
cpSpaceHashEach(space->activeShapes, &updateBBCache, NULL);
|
477
|
+
|
478
|
+
// Collide!
|
479
|
+
cpSpaceHashEach(space->activeShapes, &active2staticIter, space);
|
480
|
+
cpSpaceHashQueryRehash(space->activeShapes, &queryFunc, space);
|
481
|
+
|
482
|
+
// Prestep the arbiters.
|
483
|
+
for(int i=0; i<arbiters->num; i++)
|
484
|
+
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv);
|
485
|
+
|
486
|
+
// Prestep the joints.
|
487
|
+
for(int i=0; i<joints->num; i++){
|
488
|
+
cpJoint *joint = (cpJoint *)joints->arr[i];
|
489
|
+
joint->klass->preStep(joint, dt_inv);
|
490
|
+
}
|
491
|
+
|
492
|
+
for(int i=0; i<space->elasticIterations; i++){
|
493
|
+
for(int j=0; j<arbiters->num; j++)
|
494
|
+
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 1.0f);
|
495
|
+
|
496
|
+
for(int j=0; j<joints->num; j++){
|
497
|
+
cpJoint *joint = (cpJoint *)joints->arr[j];
|
498
|
+
joint->klass->applyImpulse(joint);
|
499
|
+
}
|
500
|
+
}
|
501
|
+
|
502
|
+
// Integrate velocities.
|
503
|
+
cpFloat damping = pow(1.0f/space->damping, -dt);
|
504
|
+
for(int i=0; i<bodies->num; i++){
|
505
|
+
cpBody *body = (cpBody *)bodies->arr[i];
|
506
|
+
body->velocity_func(body, space->gravity, damping, dt);
|
507
|
+
}
|
508
|
+
|
509
|
+
for(int i=0; i<arbiters->num; i++)
|
510
|
+
cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]);
|
511
|
+
|
512
|
+
// Run the impulse solver.
|
513
|
+
for(int i=0; i<space->iterations; i++){
|
514
|
+
for(int j=0; j<arbiters->num; j++)
|
515
|
+
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 0.0f);
|
516
|
+
|
517
|
+
for(int j=0; j<joints->num; j++){
|
518
|
+
cpJoint *joint = (cpJoint *)joints->arr[j];
|
519
|
+
joint->klass->applyImpulse(joint);
|
520
|
+
}
|
521
|
+
}
|
522
|
+
|
523
|
+
// cpFloat dvsq = cpvdot(space->gravity, space->gravity);
|
524
|
+
// dvsq *= dt*dt * space->damping*space->damping;
|
525
|
+
// for(int i=0; i<bodies->num; i++)
|
526
|
+
// cpBodyMarkLowEnergy(bodies->arr[i], dvsq, space->sleepTicks);
|
527
|
+
|
528
|
+
// Increment the stamp.
|
529
|
+
space->stamp++;
|
530
|
+
}
|
@@ -0,0 +1,120 @@
|
|
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
|
+
// Number of frames that contact information should persist.
|
23
|
+
extern int cp_contact_persistence;
|
24
|
+
|
25
|
+
// User collision pair function.
|
26
|
+
typedef int (*cpCollFunc)(cpShape *a, cpShape *b, cpContact *contacts, int numContacts, cpFloat normal_coef, void *data);
|
27
|
+
|
28
|
+
// Structure for holding collision pair function information.
|
29
|
+
// Used internally.
|
30
|
+
typedef struct cpCollPairFunc {
|
31
|
+
unsigned int a;
|
32
|
+
unsigned int b;
|
33
|
+
cpCollFunc func;
|
34
|
+
void *data;
|
35
|
+
} cpCollPairFunc;
|
36
|
+
|
37
|
+
typedef struct cpSpace{
|
38
|
+
// *** User definable fields
|
39
|
+
|
40
|
+
// Number of iterations to use in the impulse solver to solve contacts.
|
41
|
+
int iterations;
|
42
|
+
|
43
|
+
// Number of iterations to use in the impulse solver to solve elastic collisions.
|
44
|
+
int elasticIterations;
|
45
|
+
|
46
|
+
// Default gravity to supply when integrating rigid body motions.
|
47
|
+
cpVect gravity;
|
48
|
+
|
49
|
+
// Default damping to supply when integrating rigid body motions.
|
50
|
+
cpFloat damping;
|
51
|
+
|
52
|
+
// *** Internally Used Fields
|
53
|
+
|
54
|
+
// Time stamp. Is incremented on every call to cpSpaceStep().
|
55
|
+
int stamp;
|
56
|
+
|
57
|
+
// The static and active shape spatial hashes.
|
58
|
+
cpSpaceHash *staticShapes;
|
59
|
+
cpSpaceHash *activeShapes;
|
60
|
+
|
61
|
+
// List of bodies in the system.
|
62
|
+
cpArray *bodies;
|
63
|
+
// List of active arbiters for the impulse solver.
|
64
|
+
cpArray *arbiters;
|
65
|
+
// Persistant contact set.
|
66
|
+
cpHashSet *contactSet;
|
67
|
+
|
68
|
+
// List of joints in the system.
|
69
|
+
cpArray *joints;
|
70
|
+
|
71
|
+
// Set of collisionpair functions.
|
72
|
+
cpHashSet *collFuncSet;
|
73
|
+
// Default collision pair function.
|
74
|
+
cpCollPairFunc defaultPairFunc;
|
75
|
+
} cpSpace;
|
76
|
+
|
77
|
+
// Basic allocation/destruction functions.
|
78
|
+
cpSpace* cpSpaceAlloc(void);
|
79
|
+
cpSpace* cpSpaceInit(cpSpace *space);
|
80
|
+
cpSpace* cpSpaceNew(void);
|
81
|
+
|
82
|
+
void cpSpaceDestroy(cpSpace *space);
|
83
|
+
void cpSpaceFree(cpSpace *space);
|
84
|
+
|
85
|
+
// Convenience function. Frees all referenced entities. (bodies, shapes and joints)
|
86
|
+
void cpSpaceFreeChildren(cpSpace *space);
|
87
|
+
|
88
|
+
// Collision pair function management functions.
|
89
|
+
void cpSpaceAddCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b,
|
90
|
+
cpCollFunc func, void *data);
|
91
|
+
void cpSpaceRemoveCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b);
|
92
|
+
void cpSpaceSetDefaultCollisionPairFunc(cpSpace *space, cpCollFunc func, void *data);
|
93
|
+
|
94
|
+
// Add and remove entities from the system.
|
95
|
+
void cpSpaceAddShape(cpSpace *space, cpShape *shape);
|
96
|
+
void cpSpaceAddStaticShape(cpSpace *space, cpShape *shape);
|
97
|
+
void cpSpaceAddBody(cpSpace *space, cpBody *body);
|
98
|
+
void cpSpaceAddJoint(cpSpace *space, cpJoint *joint);
|
99
|
+
|
100
|
+
void cpSpaceRemoveShape(cpSpace *space, cpShape *shape);
|
101
|
+
void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape);
|
102
|
+
void cpSpaceRemoveBody(cpSpace *space, cpBody *body);
|
103
|
+
void cpSpaceRemoveJoint(cpSpace *space, cpJoint *joint);
|
104
|
+
|
105
|
+
// Point query callback function
|
106
|
+
typedef void (*cpSpacePointQueryFunc)(cpShape *shape, void *data);
|
107
|
+
void cpSpaceShapePointQuery(cpSpace *space, cpVect point, cpSpacePointQueryFunc func, void *data);
|
108
|
+
void cpSpaceStaticShapePointQuery(cpSpace *space, cpVect point, cpSpacePointQueryFunc func, void *data);
|
109
|
+
|
110
|
+
// Iterator function for iterating the bodies in a space.
|
111
|
+
typedef void (*cpSpaceBodyIterator)(cpBody *body, void *data);
|
112
|
+
void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data);
|
113
|
+
|
114
|
+
// Spatial hash management functions.
|
115
|
+
void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count);
|
116
|
+
void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count);
|
117
|
+
void cpSpaceRehashStatic(cpSpace *space);
|
118
|
+
|
119
|
+
// Update the space.
|
120
|
+
void cpSpaceStep(cpSpace *space, cpFloat dt);
|