|
4
|
1
|
|
|
2 // written in the D programming language
|
|
|
3
|
|
|
4 module chipmunkd.cpSpace;
|
|
|
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 // Number of frames that contact information should persist.
|
|
|
21 //extern cpTimestamp cp_contact_persistence;
|
|
|
22
|
|
|
23 // User collision handler function types.
|
|
|
24 alias cpBool function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionBeginFunc;
|
|
|
25 alias cpBool function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionPreSolveFunc;
|
|
|
26 alias void function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionPostSolveFunc;
|
|
|
27 alias void function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionSeparateFunc;
|
|
|
28
|
|
|
29 // Structure for holding collision pair function information.
|
|
|
30 // Used internally.
|
|
|
31 struct cpCollisionHandler {
|
|
|
32 cpCollisionType a;
|
|
|
33 cpCollisionType b;
|
|
|
34 cpCollisionBeginFunc begin;
|
|
|
35 cpCollisionPreSolveFunc preSolve;
|
|
|
36 cpCollisionPostSolveFunc postSolve;
|
|
|
37 cpCollisionSeparateFunc separate;
|
|
|
38 void *data;
|
|
|
39 }
|
|
|
40
|
|
|
41 enum CP_MAX_CONTACTS_PER_ARBITER = 6;
|
|
|
42 struct cpContactBufferHeader {
|
|
|
43 cpTimestamp stamp;
|
|
|
44 cpContactBufferHeader *next;
|
|
|
45 uint numContacts;
|
|
|
46 }
|
|
|
47
|
|
|
48 struct cpSpace{
|
|
|
49 // *** User definable fields
|
|
|
50
|
|
|
51 // Number of iterations to use in the impulse solver to solve contacts.
|
|
|
52 int iterations;
|
|
|
53
|
|
|
54 // Number of iterations to use in the impulse solver to solve elastic collisions.
|
|
|
55 int elasticIterations;
|
|
|
56
|
|
|
57 // Default gravity to supply when integrating rigid body motions.
|
|
|
58 cpVect gravity;
|
|
|
59
|
|
|
60 // Default damping to supply when integrating rigid body motions.
|
|
|
61 cpFloat damping;
|
|
|
62
|
|
|
63 // Speed threshold for a body to be considered idle.
|
|
|
64 // The default value of 0 means to let the space guess a good threshold based on gravity.
|
|
|
65 cpFloat idleSpeedThreshold;
|
|
|
66
|
|
|
67 // Time a group of bodies must remain idle in order to fall asleep
|
|
|
68 // The default value of INFINITY disables the sleeping algorithm.
|
|
|
69 cpFloat sleepTimeThreshold;
|
|
|
70
|
|
|
71 // *** Internally Used Fields
|
|
|
72
|
|
|
73 // When the space is locked, you should not add or remove objects;
|
|
|
74 cpBool locked;
|
|
|
75
|
|
|
76 // Time stamp. Is incremented on every call to cpSpaceStep().
|
|
|
77 cpTimestamp stamp;
|
|
|
78
|
|
|
79 // The static and active shape spatial hashes.
|
|
|
80 cpSpaceHash* staticShapes;
|
|
|
81 cpSpaceHash* activeShapes;
|
|
|
82
|
|
|
83 // List of bodies in the system.
|
|
|
84 cpArray *bodies;
|
|
|
85
|
|
|
86 // List of groups of sleeping bodies.
|
|
|
87 cpArray *sleepingComponents;
|
|
|
88
|
|
|
89 // List of active arbiters for the impulse solver.
|
|
|
90 cpArray* arbiters, pooledArbiters;
|
|
|
91
|
|
|
92 // Linked list ring of contact buffers.
|
|
|
93 // Head is the newest buffer, and each buffer points to a newer buffer.
|
|
|
94 // Head wraps around and points to the oldest (tail) buffer.
|
|
|
95 cpContactBufferHeader* contactBuffersHead, _contactBuffersTail;
|
|
|
96
|
|
|
97 // List of buffers to be free()ed when destroying the space.
|
|
|
98 cpArray *allocatedBuffers;
|
|
|
99
|
|
|
100 // Persistant contact set.
|
|
|
101 cpHashSet *contactSet;
|
|
|
102
|
|
|
103 // List of constraints in the system.
|
|
|
104 cpArray *constraints;
|
|
|
105
|
|
|
106 // Set of collisionpair functions.
|
|
|
107 cpHashSet *collFuncSet;
|
|
|
108 // Default collision handler.
|
|
|
109 cpCollisionHandler defaultHandler;
|
|
|
110
|
|
|
111 cpHashSet *postStepCallbacks;
|
|
|
112
|
|
|
113 cpBody staticBody;
|
|
|
114 }
|
|
|
115
|
|
|
116 //// Basic allocation/destruction functions.
|
|
|
117 //cpSpace* cpSpaceAlloc(void);
|
|
|
118 //cpSpace* cpSpaceInit(cpSpace *space);
|
|
|
119 //cpSpace* cpSpaceNew(void);
|
|
|
120 //
|
|
|
121 //void cpSpaceDestroy(cpSpace *space);
|
|
|
122 //void cpSpaceFree(cpSpace *space);
|
|
|
123 //
|
|
|
124 //// Convenience function. Frees all referenced entities. (bodies, shapes and constraints)
|
|
|
125 //void cpSpaceFreeChildren(cpSpace *space);
|
|
|
126 //
|
|
|
127 //// Collision handler management functions.
|
|
|
128 //void cpSpaceSetDefaultCollisionHandler(
|
|
|
129 // cpSpace *space,
|
|
|
130 // cpCollisionBeginFunc begin,
|
|
|
131 // cpCollisionPreSolveFunc preSolve,
|
|
|
132 // cpCollisionPostSolveFunc postSolve,
|
|
|
133 // cpCollisionSeparateFunc separate,
|
|
|
134 // void *data
|
|
|
135 //);
|
|
|
136 //void cpSpaceAddCollisionHandler(
|
|
|
137 // cpSpace *space,
|
|
|
138 // cpCollisionType a, cpCollisionType b,
|
|
|
139 // cpCollisionBeginFunc begin,
|
|
|
140 // cpCollisionPreSolveFunc preSolve,
|
|
|
141 // cpCollisionPostSolveFunc postSolve,
|
|
|
142 // cpCollisionSeparateFunc separate,
|
|
|
143 // void *data
|
|
|
144 //);
|
|
|
145 //void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b);
|
|
|
146 //
|
|
|
147 //// Add and remove entities from the system.
|
|
|
148 //cpShape *cpSpaceAddShape(cpSpace *space, cpShape *shape);
|
|
|
149 //cpShape *cpSpaceAddStaticShape(cpSpace *space, cpShape *shape);
|
|
|
150 //cpBody *cpSpaceAddBody(cpSpace *space, cpBody *body);
|
|
|
151 //cpConstraint *cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint);
|
|
|
152 //
|
|
|
153 //void cpSpaceRemoveShape(cpSpace *space, cpShape *shape);
|
|
|
154 //void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape);
|
|
|
155 //void cpSpaceRemoveBody(cpSpace *space, cpBody *body);
|
|
|
156 //void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint);
|
|
|
157 //
|
|
|
158 //// Post Step function definition
|
|
|
159 alias void function(cpSpace *space, void *obj, void *data) cpPostStepFunc;
|
|
|
160 //// Register a post step function to be called after cpSpaceStep() has finished.
|
|
|
161 //// obj is used a key, you can only register one callback per unique value for obj
|
|
|
162 //void cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data);
|
|
|
163 //
|
|
|
164 //// Point query callback function
|
|
|
165 alias void function(cpShape *shape, void *data) cpSpacePointQueryFunc;
|
|
|
166 //void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data);
|
|
|
167 //cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group);
|
|
|
168 //
|
|
|
169 //// Segment query callback function
|
|
|
170 alias void function(cpShape *shape, cpFloat t, cpVect n, void *data)cpSpaceSegmentQueryFunc;
|
|
|
171 //void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data);
|
|
|
172 //cpShape *cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out);
|
|
|
173 //
|
|
|
174 //// BB query callback function
|
|
|
175 alias void function(cpShape *shape, void *data)cpSpaceBBQueryFunc;
|
|
|
176 //void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data);
|
|
|
177 //
|
|
|
178 //
|
|
|
179 //// Iterator function for iterating the bodies in a space.
|
|
|
180 alias void function(cpBody *_body, void *data)cpSpaceBodyIterator;
|
|
|
181 //void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data);
|
|
|
182 //
|
|
|
183 //// Spatial hash management functions.
|
|
|
184 //void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count);
|
|
|
185 //void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count);
|
|
|
186 //void cpSpaceRehashStatic(cpSpace *space);
|
|
|
187 //
|
|
|
188 //void cpSpaceRehashShape(cpSpace *space, cpShape *shape);
|
|
|
189 //
|
|
|
190 //// Update the space.
|
|
|
191 //void cpSpaceStep(cpSpace *space, cpFloat dt);
|
|
|
192
|
|
|
193
|
|
|
194 cpTimestamp cp_contact_persistence = 3;
|
|
|
195
|
|
|
196 // Equal function for contactSet.
|
|
|
197 static cpBool
|
|
|
198 contactSetEql(cpShape **shapes, cpArbiter *arb)
|
|
|
199 {
|
|
|
200 cpShape *a = shapes[0];
|
|
|
201 cpShape *b = shapes[1];
|
|
|
202
|
|
|
203 return ((a == arb.private_a && b == arb.private_b) || (b == arb.private_a && a == arb.private_b));
|
|
|
204 }
|
|
|
205
|
|
|
206 // Transformation function for contactSet.
|
|
|
207 static void *
|
|
|
208 contactSetTrans(cpShape **shapes, cpSpace *space)
|
|
|
209 {
|
|
|
210 if(space.pooledArbiters.num == 0){
|
|
|
211 // arbiter pool is exhausted, make more
|
|
|
212 int count = CP_BUFFER_BYTES/cpArbiter.sizeof;
|
|
|
213 assert(count, "Buffer size too small.");
|
|
|
214
|
|
|
215 cpArbiter *buffer = cast(cpArbiter *)cpmalloc(CP_BUFFER_BYTES);
|
|
|
216 cpArrayPush(space.allocatedBuffers, buffer);
|
|
|
217
|
|
|
218 for(int i=0; i<count; i++) cpArrayPush(space.pooledArbiters, buffer + i);
|
|
|
219 }
|
|
|
220
|
|
|
221 return cpArbiterInit(cast(cpArbiter *) cpArrayPop(space.pooledArbiters), shapes[0], shapes[1]);
|
|
|
222 }
|
|
|
223
|
|
|
224 // Equals function for collFuncSet.
|
|
|
225 static cpBool
|
|
|
226 collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair)
|
|
|
227 {
|
|
|
228 return ((check.a == pair.a && check.b == pair.b) || (check.b == pair.a && check.a == pair.b));
|
|
|
229 }
|
|
|
230
|
|
|
231 // Transformation function for collFuncSet.
|
|
|
232 static void *
|
|
|
233 collFuncSetTrans(cpCollisionHandler *handler, void *unused)
|
|
|
234 {
|
|
|
235 cpCollisionHandler *copy = cast(cpCollisionHandler *)cpmalloc(cpCollisionHandler.sizeof);
|
|
|
236 (*copy) = (*handler);
|
|
|
237
|
|
|
238 return copy;
|
|
|
239 }
|
|
|
240
|
|
|
241 // Default collision functions.
|
|
|
242 static cpBool alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;}
|
|
|
243 static void nothing(cpArbiter *arb, cpSpace *space, void *data){}
|
|
|
244
|
|
|
245 // BBfunc callback for the spatial hash.
|
|
|
246 static cpBB shapeBBFunc(cpShape *shape){return shape.bb;}
|
|
|
247
|
|
|
248 // Iterator functions for destructors.
|
|
|
249 static void freeWrap(void *ptr, void *unused){ cpfree(ptr);}
|
|
|
250 static void shapeFreeWrap(cpShape *ptr, void *unused){ cpShapeFree(ptr);}
|
|
|
251 static void bodyFreeWrap(cpBody *ptr, void *unused){ cpBodyFree(ptr);}
|
|
|
252 static void constraintFreeWrap(cpConstraint *ptr, void *unused){cpConstraintFree(ptr);}
|
|
|
253
|
|
|
254 cpSpace *
|
|
|
255 cpSpaceAlloc()
|
|
|
256 {
|
|
|
257 return cast(cpSpace *)cpcalloc(1, cpSpace.sizeof);
|
|
|
258 }
|
|
|
259
|
|
|
260 enum DEFAULT_DIM_SIZE = 100.0f;
|
|
|
261 enum DEFAULT_COUNT = 1000;
|
|
|
262 enum DEFAULT_ITERATIONS = 10;
|
|
|
263 enum DEFAULT_ELASTIC_ITERATIONS = 0;
|
|
|
264
|
|
|
265 cpCollisionHandler defaultHandler = {0, 0, &alwaysCollide, &alwaysCollide, ¬hing, ¬hing, null};
|
|
|
266
|
|
|
267 cpSpace*
|
|
|
268 cpSpaceInit(cpSpace *space)
|
|
|
269 {
|
|
|
270 space.iterations = DEFAULT_ITERATIONS;
|
|
|
271 space.elasticIterations = DEFAULT_ELASTIC_ITERATIONS;
|
|
|
272 // space.sleepTicks = 300;
|
|
|
273
|
|
|
274 space.gravity = cpvzero;
|
|
|
275 space.damping = 1.0f;
|
|
|
276
|
|
|
277 space.locked = 0;
|
|
|
278 space.stamp = 0;
|
|
|
279
|
|
|
280 space.staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, cast(cpSpaceHashBBFunc)&shapeBBFunc);
|
|
|
281 space.activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, cast(cpSpaceHashBBFunc)&shapeBBFunc);
|
|
|
282
|
|
|
283 space.allocatedBuffers = cpArrayNew(0);
|
|
|
284
|
|
|
285 space.bodies = cpArrayNew(0);
|
|
|
286 space.sleepingComponents = cpArrayNew(0);
|
|
|
287 space.sleepTimeThreshold = INFINITY;
|
|
|
288 space.idleSpeedThreshold = 0.0f;
|
|
|
289
|
|
|
290 space.arbiters = cpArrayNew(0);
|
|
|
291 space.pooledArbiters = cpArrayNew(0);
|
|
|
292
|
|
|
293 space.contactBuffersHead = null;
|
|
|
294 space.contactSet = cpHashSetNew(0, cast(cpHashSetEqlFunc)&contactSetEql, cast(cpHashSetTransFunc)&contactSetTrans);
|
|
|
295
|
|
|
296 space.constraints = cpArrayNew(0);
|
|
|
297
|
|
|
298 space.defaultHandler = defaultHandler;
|
|
|
299 space.collFuncSet = cpHashSetNew(0, cast(cpHashSetEqlFunc)&collFuncSetEql, cast(cpHashSetTransFunc)&collFuncSetTrans);
|
|
|
300 space.collFuncSet.default_value = &space.defaultHandler;
|
|
|
301
|
|
|
302 space.postStepCallbacks = null;
|
|
|
303
|
|
|
304 cpBodyInit(&space.staticBody, INFINITY, INFINITY);
|
|
|
305 space.staticBody.space = space;
|
|
|
306
|
|
|
307 return space;
|
|
|
308 }
|
|
|
309
|
|
|
310 cpSpace*
|
|
|
311 cpSpaceNew()
|
|
|
312 {
|
|
|
313 return cpSpaceInit(cpSpaceAlloc());
|
|
|
314 }
|
|
|
315
|
|
|
316 void
|
|
|
317 cpSpaceDestroy(cpSpace *space)
|
|
|
318 {
|
|
|
319 cpSpaceHashFree(space.staticShapes);
|
|
|
320 cpSpaceHashFree(space.activeShapes);
|
|
|
321
|
|
|
322 cpArrayFree(space.bodies);
|
|
|
323 cpArrayFree(space.sleepingComponents);
|
|
|
324
|
|
|
325 cpArrayFree(space.constraints);
|
|
|
326
|
|
|
327 cpHashSetFree(space.contactSet);
|
|
|
328
|
|
|
329 cpArrayFree(space.arbiters);
|
|
|
330 cpArrayFree(space.pooledArbiters);
|
|
|
331
|
|
|
332 if(space.allocatedBuffers){
|
|
|
333 cpArrayEach(space.allocatedBuffers, &freeWrap, null);
|
|
|
334 cpArrayFree(space.allocatedBuffers);
|
|
|
335 }
|
|
|
336
|
|
|
337 if(space.postStepCallbacks){
|
|
|
338 cpHashSetEach(space.postStepCallbacks, &freeWrap, null);
|
|
|
339 cpHashSetFree(space.postStepCallbacks);
|
|
|
340 }
|
|
|
341
|
|
|
342 if(space.collFuncSet){
|
|
|
343 cpHashSetEach(space.collFuncSet, &freeWrap, null);
|
|
|
344 cpHashSetFree(space.collFuncSet);
|
|
|
345 }
|
|
|
346 }
|
|
|
347
|
|
|
348 void
|
|
|
349 cpSpaceFree(cpSpace *space)
|
|
|
350 {
|
|
|
351 if(space){
|
|
|
352 cpSpaceDestroy(space);
|
|
|
353 cpfree(space);
|
|
|
354 }
|
|
|
355 }
|
|
|
356
|
|
|
357 void
|
|
|
358 cpSpaceFreeChildren(cpSpace *space)
|
|
|
359 {
|
|
|
360 cpArray *components = space.sleepingComponents;
|
|
|
361 while(components.num) cpBodyActivate(cast(cpBody *)components.arr[0]);
|
|
|
362
|
|
|
363 cpSpaceHashEach(space.staticShapes, cast(cpSpaceHashIterator)&shapeFreeWrap, null);
|
|
|
364 cpSpaceHashEach(space.activeShapes, cast(cpSpaceHashIterator)&shapeFreeWrap, null);
|
|
|
365 cpArrayEach(space.bodies, cast(cpArrayIter)&bodyFreeWrap, null);
|
|
|
366 cpArrayEach(space.constraints, cast(cpArrayIter)&constraintFreeWrap, null);
|
|
|
367 }
|
|
|
368
|
|
|
369 void
|
|
|
370 cpSpaceAddCollisionHandler(
|
|
|
371 cpSpace *space,
|
|
|
372 cpCollisionType a, cpCollisionType b,
|
|
|
373 cpCollisionBeginFunc begin,
|
|
|
374 cpCollisionPreSolveFunc preSolve,
|
|
|
375 cpCollisionPostSolveFunc postSolve,
|
|
|
376 cpCollisionSeparateFunc separate,
|
|
|
377 void *data
|
|
|
378 ){
|
|
|
379 // Remove any old function so the new one will get added.
|
|
|
380 cpSpaceRemoveCollisionHandler(space, a, b);
|
|
|
381
|
|
|
382 cpCollisionHandler handler = {
|
|
|
383 a, b,
|
|
|
384 begin ? begin : &alwaysCollide,
|
|
|
385 preSolve ? preSolve : &alwaysCollide,
|
|
|
386 postSolve ? postSolve : ¬hing,
|
|
|
387 separate ? separate : ¬hing,
|
|
|
388 data
|
|
|
389 };
|
|
|
390
|
|
|
391 cpHashSetInsert(space.collFuncSet, CP_HASH_PAIR(a, b), &handler, null);
|
|
|
392 }
|
|
|
393
|
|
|
394 void
|
|
|
395 cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b)
|
|
|
396 {
|
|
|
397 struct tmp{cpCollisionType a, b;} tmp ids = {a, b};
|
|
|
398 cpCollisionHandler *old_handler = cast(cpCollisionHandler *) cpHashSetRemove(space.collFuncSet, CP_HASH_PAIR(a, b), &ids);
|
|
|
399 cpfree(old_handler);
|
|
|
400 }
|
|
|
401
|
|
|
402 void
|
|
|
403 cpSpaceSetDefaultCollisionHandler(
|
|
|
404 cpSpace *space,
|
|
|
405 cpCollisionBeginFunc begin,
|
|
|
406 cpCollisionPreSolveFunc preSolve,
|
|
|
407 cpCollisionPostSolveFunc postSolve,
|
|
|
408 cpCollisionSeparateFunc separate,
|
|
|
409 void *data
|
|
|
410 ){
|
|
|
411 cpCollisionHandler handler = {
|
|
|
412 0, 0,
|
|
|
413 begin ? begin : &alwaysCollide,
|
|
|
414 preSolve ? preSolve : &alwaysCollide,
|
|
|
415 postSolve ? postSolve : ¬hing,
|
|
|
416 separate ? separate : ¬hing,
|
|
|
417 data
|
|
|
418 };
|
|
|
419
|
|
|
420 space.defaultHandler = handler;
|
|
|
421 }
|
|
|
422
|
|
|
423 //#pragma mark Body, Shape, and Joint Management
|
|
|
424
|
|
|
425 void cpAssertSpaceUnlocked(cpSpace* _space){
|
|
|
426 assert(!_space.locked,
|
|
|
427 "This addition/removal cannot be done safely during a call to cpSpaceStep(). "
|
|
|
428 "Put these calls into a Post Step Callback."
|
|
|
429 );}
|
|
|
430
|
|
|
431 static void
|
|
|
432 cpBodyAddShape(cpBody *_body, cpShape *shape)
|
|
|
433 {
|
|
|
434 shape.next = shape._body.shapesList;
|
|
|
435 shape._body.shapesList = shape;
|
|
|
436 }
|
|
|
437
|
|
|
438 static void
|
|
|
439 cpBodyRemoveShape(cpBody *_body, cpShape *shape)
|
|
|
440 {
|
|
|
441 cpShape **prev_ptr = &_body.shapesList;
|
|
|
442 cpShape *node = _body.shapesList;
|
|
|
443
|
|
|
444 while(node && node != shape){
|
|
|
445 prev_ptr = &node.next;
|
|
|
446 node = node.next;
|
|
|
447 }
|
|
|
448
|
|
|
449 assert(node, "Attempted to remove a shape from a body it was never attached to.");
|
|
|
450 (*prev_ptr) = node.next;
|
|
|
451 }
|
|
|
452
|
|
|
453 cpShape *
|
|
|
454 cpSpaceAddShape(cpSpace *space, cpShape *shape)
|
|
|
455 {
|
|
|
456 cpBody *_body = shape._body;
|
|
|
457 if(!_body || _body == &space.staticBody) return cpSpaceAddStaticShape(space, shape);
|
|
|
458
|
|
|
459 assert(!cpHashSetFind(space.activeShapes.handleSet, shape.hashid, shape),
|
|
|
460 "Cannot add the same shape more than once.");
|
|
|
461 cpAssertSpaceUnlocked(space);
|
|
|
462
|
|
|
463 cpBodyActivate(_body);
|
|
|
464 cpBodyAddShape(_body, shape);
|
|
|
465
|
|
|
466 cpShapeCacheBB(shape);
|
|
|
467 cpSpaceHashInsert(space.activeShapes, shape, shape.hashid, shape.bb);
|
|
|
468
|
|
|
469 return shape;
|
|
|
470 }
|
|
|
471
|
|
|
472 static void
|
|
|
473 activateShapesTouchingShapeHelper(cpShape *shape, void *unused)
|
|
|
474 {
|
|
|
475 cpBodyActivate(shape._body);
|
|
|
476 }
|
|
|
477
|
|
|
478 static void
|
|
|
479 activateShapesTouchingShape(cpSpace *space, cpShape *shape)
|
|
|
480 {
|
|
|
481 // TODO this query should be more precise
|
|
|
482 // Use shape queries once they are written
|
|
|
483 cpSpaceBBQuery(space, shape.bb, shape.layers, shape.group, &activateShapesTouchingShapeHelper, null);
|
|
|
484 }
|
|
|
485
|
|
|
486 cpShape *
|
|
|
487 cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
|
|
|
488 {
|
|
|
489 assert(!cpHashSetFind(space.staticShapes.handleSet, shape.hashid, shape),
|
|
|
490 "Cannot add the same static shape more than once.");
|
|
|
491 cpAssertSpaceUnlocked(space);
|
|
|
492
|
|
|
493 if(!shape._body) shape._body = &space.staticBody;
|
|
|
494
|
|
|
495 cpShapeCacheBB(shape);
|
|
|
496 activateShapesTouchingShape(space, shape);
|
|
|
497 cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
|
|
|
498
|
|
|
499 return shape;
|
|
|
500 }
|
|
|
501
|
|
|
502 cpBody *
|
|
|
503 cpSpaceAddBody(cpSpace *space, cpBody *_body)
|
|
|
504 {
|
|
|
505 cpAssertWarn(_body.m != INFINITY, "Did you really mean to add an infinite mass body to the space?");
|
|
|
506 assert(!_body.space, "Cannot add a body to a more than one space or to the same space twice.");
|
|
|
507 // cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback
|
|
|
508
|
|
|
509 cpArrayPush(space.bodies, _body);
|
|
|
510 _body.space = space;
|
|
|
511
|
|
|
512 return _body;
|
|
|
513 }
|
|
|
514
|
|
|
515 cpConstraint *
|
|
|
516 cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
|
|
|
517 {
|
|
|
518 assert(!cpArrayContains(space.constraints, constraint), "Cannot add the same constraint more than once.");
|
|
|
519 // cpAssertSpaceUnlocked(space); This should be safe as long as its not from a constraint callback.
|
|
|
520
|
|
|
521 if(!constraint.a) constraint.a = &space.staticBody;
|
|
|
522 if(!constraint.b) constraint.b = &space.staticBody;
|
|
|
523
|
|
|
524 cpBodyActivate(constraint.a);
|
|
|
525 cpBodyActivate(constraint.b);
|
|
|
526 cpArrayPush(space.constraints, constraint);
|
|
|
527
|
|
|
528 return constraint;
|
|
|
529 }
|
|
|
530
|
|
|
531 struct removalContext {
|
|
|
532 cpSpace *space;
|
|
|
533 cpShape *shape;
|
|
|
534 }
|
|
|
535
|
|
|
536 // Hashset filter func to throw away old arbiters.
|
|
|
537 static cpBool
|
|
|
538 contactSetFilterRemovedShape(cpArbiter *arb, removalContext *context)
|
|
|
539 {
|
|
|
540 if(context.shape == arb.private_a || context.shape == arb.private_b){
|
|
|
541 arb.handler.separate(arb, context.space, arb.handler.data);
|
|
|
542 cpArrayPush(context.space.pooledArbiters, arb);
|
|
|
543 return cpFalse;
|
|
|
544 }
|
|
|
545
|
|
|
546 return cpTrue;
|
|
|
547 }
|
|
|
548
|
|
|
549 void
|
|
|
550 cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
|
|
|
551 {
|
|
|
552 cpBody *_body = shape._body;
|
|
|
553 if(cpBodyIsStatic(_body)){
|
|
|
554 cpSpaceRemoveStaticShape(space, shape);
|
|
|
555 return;
|
|
|
556 }
|
|
|
557
|
|
|
558 cpBodyActivate(_body);
|
|
|
559
|
|
|
560 cpAssertSpaceUnlocked(space);
|
|
|
561 cpAssertWarn(cpHashSetFind(space.activeShapes.handleSet, shape.hashid, shape) !is null,
|
|
|
562 "Cannot remove a shape that was never added to the space. (Removed twice maybe?)");
|
|
|
563
|
|
|
564 cpBodyRemoveShape(_body, shape);
|
|
|
565
|
|
|
566 removalContext context = {space, shape};
|
|
|
567 cpHashSetFilter(space.contactSet, cast(cpHashSetFilterFunc)&contactSetFilterRemovedShape, &context);
|
|
|
568 cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
|
|
|
569 }
|
|
|
570
|
|
|
571 void
|
|
|
572 cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
|
|
|
573 {
|
|
|
574 cpAssertWarn(cpHashSetFind(space.staticShapes.handleSet, shape.hashid, shape) !is null,
|
|
|
575 "Cannot remove a static or sleeping shape that was never added to the space. (Removed twice maybe?)");
|
|
|
576 cpAssertSpaceUnlocked(space);
|
|
|
577
|
|
|
578 removalContext context = {space, shape};
|
|
|
579 cpHashSetFilter(space.contactSet, cast(cpHashSetFilterFunc)&contactSetFilterRemovedShape, &context);
|
|
|
580 cpSpaceHashRemove(space.staticShapes, shape, shape.hashid);
|
|
|
581
|
|
|
582 activateShapesTouchingShape(space, shape);
|
|
|
583 }
|
|
|
584
|
|
|
585 void
|
|
|
586 cpSpaceRemoveBody(cpSpace *space, cpBody *_body)
|
|
|
587 {
|
|
|
588 cpAssertWarn(_body.space == space,
|
|
|
589 "Cannot remove a body that was never added to the space. (Removed twice maybe?)");
|
|
|
590 cpAssertSpaceUnlocked(space);
|
|
|
591
|
|
|
592 cpBodyActivate(_body);
|
|
|
593 cpArrayDeleteObj(space.bodies, _body);
|
|
|
594 _body.space = null;
|
|
|
595 }
|
|
|
596
|
|
|
597 void
|
|
|
598 cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
|
|
|
599 {
|
|
|
600 cpAssertWarn(cpArrayContains(space.constraints, constraint),
|
|
|
601 "Cannot remove a constraint that was never added to the space. (Removed twice maybe?)");
|
|
|
602 // cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback.
|
|
|
603
|
|
|
604 cpBodyActivate(constraint.a);
|
|
|
605 cpBodyActivate(constraint.b);
|
|
|
606 cpArrayDeleteObj(space.constraints, constraint);
|
|
|
607 }
|
|
|
608
|
|
|
609 //#pragma mark Spatial Hash Management
|
|
|
610
|
|
|
611 static void updateBBCache(cpShape *shape, void *unused){cpShapeCacheBB(shape);}
|
|
|
612
|
|
|
613 void
|
|
|
614 cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count)
|
|
|
615 {
|
|
|
616 cpSpaceHashResize(space.staticShapes, dim, count);
|
|
|
617 cpSpaceHashRehash(space.staticShapes);
|
|
|
618 }
|
|
|
619
|
|
|
620 void
|
|
|
621 cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count)
|
|
|
622 {
|
|
|
623 cpSpaceHashResize(space.activeShapes, dim, count);
|
|
|
624 }
|
|
|
625
|
|
|
626 void
|
|
|
627 cpSpaceRehashStatic(cpSpace *space)
|
|
|
628 {
|
|
|
629 cpSpaceHashEach(space.staticShapes, cast(cpSpaceHashIterator)&updateBBCache, null);
|
|
|
630 cpSpaceHashRehash(space.staticShapes);
|
|
|
631 }
|
|
|
632
|
|
|
633 void
|
|
|
634 cpSpaceRehashShape(cpSpace *space, cpShape *shape)
|
|
|
635 {
|
|
|
636 cpShapeCacheBB(shape);
|
|
|
637
|
|
|
638 // attempt to rehash the shape in both hashes
|
|
|
639 cpSpaceHashRehashObject(space.activeShapes, shape, shape.hashid);
|
|
|
640 cpSpaceHashRehashObject(space.staticShapes, shape, shape.hashid);
|
|
|
641 }
|
|
|
642
|
