|
4
|
1
|
|
|
2 // written in the D programming language
|
|
|
3
|
|
|
4 module chipmunkd.cpSpaceComponent;
|
|
|
5
|
|
|
6 import chipmunkd.chipmunk;
|
|
|
7
|
|
|
8 // Chipmunk uses a data structure called a disjoint set forest.
|
|
|
9 // My attempts to find a way to splice circularly linked lists in
|
|
|
10 // constant time failed, and so I found this neat data structure instead.
|
|
|
11
|
|
|
12 static cpBody *
|
|
|
13 componentNodeRoot(cpBody *_body)
|
|
|
14 {
|
|
|
15 cpBody *parent = _body.node.parent;
|
|
|
16
|
|
|
17 if(parent){
|
|
|
18 // path compression, attaches this node directly to the root
|
|
|
19 return (_body.node.parent = componentNodeRoot(parent));
|
|
|
20 } else {
|
|
|
21 return _body;
|
|
|
22 }
|
|
|
23 }
|
|
|
24
|
|
|
25 static void
|
|
|
26 componentNodeMerge(cpBody *a_root, cpBody *b_root)
|
|
|
27 {
|
|
|
28 if(a_root.node.rank < b_root.node.rank){
|
|
|
29 a_root.node.parent = b_root;
|
|
|
30 } else if(a_root.node.rank > b_root.node.rank){
|
|
|
31 b_root.node.parent = a_root;
|
|
|
32 } else if(a_root != b_root){
|
|
|
33 b_root.node.parent = a_root;
|
|
|
34 a_root.node.rank++;
|
|
|
35 }
|
|
|
36 }
|
|
|
37
|
|
29
|
38 void
|
|
|
39 cpSpaceActivateBody(cpSpace *space, cpBody *_body)
|
|
|
40 {
|
|
|
41 if(space.locked){
|
|
|
42 // cpSpaceActivateBody() is called again once the space is unlocked
|
|
|
43 cpArrayPush(space.rousedBodies, _body);
|
|
|
44 } else {
|
|
|
45 cpArrayPush(space.bodies, _body);
|
|
|
46 for(cpShape *shape=_body.shapesList; shape; shape=shape.next){
|
|
|
47 cpSpaceHashRemove(space.staticShapes, shape, shape.hashid);
|
|
|
48 cpSpaceHashInsert(space.activeShapes, shape, shape.hashid, shape.bb);
|
|
|
49 }
|
|
|
50 }
|
|
|
51 }
|
|
|
52
|
|
4
|
53 static void
|
|
|
54 componentActivate(cpBody *root)
|
|
|
55 {
|
|
|
56 if(!cpBodyIsSleeping(root)) return;
|
|
|
57
|
|
|
58 cpSpace *space = root.space;
|
|
|
59 assert(space, "Trying to activate a body that was never added to a space.");
|
|
29
|
60
|
|
|
61 cpBody *_body = root, next;
|
|
|
62 do {
|
|
|
63 next = _body.node.next;
|
|
|
64
|
|
|
65 cpComponentNode node = {null, null, 0, 0.0f};
|
|
|
66 _body.node = node;
|
|
|
67
|
|
|
68 cpSpaceActivateBody(space, _body);
|
|
|
69
|
|
4
|
70 } while((_body = next) != root);
|
|
|
71
|
|
|
72 cpArrayDeleteObj(space.sleepingComponents, root);
|
|
|
73 }
|
|
|
74
|
|
|
75 void
|
|
|
76 cpBodyActivate(cpBody *_body)
|
|
|
77 {
|
|
30
|
78 //NOTE: dmd compiler bug when using -inline
|
|
|
79 //componentActivate(componentNodeRoot(_body));
|
|
29
|
80 auto foo = componentNodeRoot(_body);
|
|
|
81 componentActivate(foo);
|
|
4
|
82 }
|
|
|
83
|
|
|
84 static void
|
|
|
85 mergeBodies(cpSpace *space, cpArray *components, cpArray *rogueBodies, cpBody *a, cpBody *b)
|
|
|
86 {
|
|
29
|
87 // Ignore connections to static bodies
|
|
4
|
88 if(cpBodyIsStatic(a) || cpBodyIsStatic(b)) return;
|
|
|
89
|
|
|
90 cpBody *a_root = componentNodeRoot(a);
|
|
|
91 cpBody *b_root = componentNodeRoot(b);
|
|
|
92
|
|
|
93 cpBool a_sleep = cpBodyIsSleeping(a_root);
|
|
|
94 cpBool b_sleep = cpBodyIsSleeping(b_root);
|
|
|
95
|
|
|
96 if(a_sleep && b_sleep){
|
|
|
97 return;
|
|
|
98 } else if(a_sleep || b_sleep){
|
|
|
99 componentActivate(a_root);
|
|
|
100 componentActivate(b_root);
|
|
|
101 }
|
|
|
102
|
|
29
|
103 // Add any rogue bodies found to the list and reset the idle time of anything they touch.
|
|
|
104 if(cpBodyIsRogue(a)){ cpArrayPush(rogueBodies, a); b.node.idleTime = 0.0f; }
|
|
|
105 if(cpBodyIsRogue(b)){ cpArrayPush(rogueBodies, b); a.node.idleTime = 0.0f; }
|
|
4
|
106
|
|
|
107 componentNodeMerge(a_root, b_root);
|
|
|
108 }
|
|
|
109
|
|
|
110 static cpBool
|
|
|
111 componentActive(cpBody *root, cpFloat threshold)
|
|
|
112 {
|
|
|
113 cpBody *_body = root;
|
|
|
114 cpBody *next;
|
|
|
115 do {
|
|
|
116 next = _body.node.next;
|
|
23
|
117 if(_body.node.idleTime < threshold) return cpTrue;
|
|
4
|
118 } while((_body = next) != root);
|
|
|
119
|
|
|
120 return cpFalse;
|
|
|
121 }
|
|
|
122
|
|
|
123 static void
|
|
|
124 addToComponent(cpBody *_body, cpArray *components)
|
|
|
125 {
|
|
|
126 // Check that the body is not already added to the component list
|
|
|
127 if(_body.node.next) return;
|
|
|
128 cpBody *root = componentNodeRoot(_body);
|
|
|
129
|
|
|
130 cpBody *next = root.node.next;
|
|
|
131 if(!next){
|
|
|
132 // If the root isn't part of a list yet, then it hasn't been
|
|
|
133 // added to the components list. Do that now.
|
|
|
134 cpArrayPush(components, root);
|
|
|
135 // Start the list
|
|
|
136 _body.node.next = root;
|
|
|
137 root.node.next = _body;
|
|
|
138 } else if(root != _body) {
|
|
|
139 // Splice in body after the root.
|
|
|
140 _body.node.next = next;
|
|
|
141 root.node.next = _body;
|
|
|
142 }
|
|
|
143 }
|
|
|
144
|
|
|
145 // TODO this function needs more commenting.
|
|
|
146 void
|
|
|
147 cpSpaceProcessComponents(cpSpace *space, cpFloat dt)
|
|
|
148 {
|
|
|
149 cpArray *bodies = space.bodies;
|
|
|
150 cpArray *newBodies = cpArrayNew(bodies.num);
|
|
|
151 cpArray *rogueBodies = cpArrayNew(16);
|
|
|
152 cpArray *arbiters = space.arbiters;
|
|
|
153 cpArray *constraints = space.constraints;
|
|
29
|
154 cpArray *components = cpArrayNew(space.sleepingComponents.num);
|
|
4
|
155
|
|
|
156 cpFloat dv = space.idleSpeedThreshold;
|
|
|
157 cpFloat dvsq = (dv ? dv*dv : cpvdot(space.gravity, space.gravity)*dt*dt);
|
|
29
|
158
|
|
4
|
159 // update idling
|
|
|
160 for(int i=0; i<bodies.num; i++){
|
|
|
161 cpBody *_body = cast(cpBody*)bodies.arr[i];
|
|
|
162
|
|
|
163 cpFloat thresh = (dvsq ? _body.m*dvsq : 0.0f);
|
|
|
164 _body.node.idleTime = (cpBodyKineticEnergy(_body) > thresh ? 0.0f : _body.node.idleTime + dt);
|
|
|
165 }
|
|
|
166
|
|
|
167 // iterate graph edges and build forests
|
|
|
168 for(int i=0; i<arbiters.num; i++){
|
|
|
169 cpArbiter *arb = cast(cpArbiter*)arbiters.arr[i];
|
|
23
|
170 mergeBodies(space, components, rogueBodies, arb.a._body, arb.b._body);
|
|
4
|
171 }
|
|
|
172 for(int j=0; j<constraints.num; j++){
|
|
|
173 cpConstraint *constraint = cast(cpConstraint *)constraints.arr[j];
|
|
|
174 mergeBodies(space, components, rogueBodies, constraint.a, constraint.b);
|
|
|
175 }
|
|
|
176
|
|
|
177 // iterate bodies and add them to their components
|
|
29
|
178 for(int i=0; i<bodies.num; i++) addToComponent(cast(cpBody*)bodies.arr[i], components);
|
|
|
179 for(int i=0; i<rogueBodies.num; i++) addToComponent(cast(cpBody*)rogueBodies.arr[i], components);
|
|
4
|
180
|
|
|
181 // iterate components, copy or deactivate
|
|
|
182 for(int i=0; i<components.num; i++){
|
|
|
183 cpBody *root = cast(cpBody*)components.arr[i];
|
|
|
184 if(componentActive(root, space.sleepTimeThreshold)){
|
|
|
185 cpBody *_body = root;
|
|
|
186 cpBody *next;
|
|
|
187 do {
|
|
|
188 next = _body.node.next;
|
|
|
189
|
|
|
190 if(!cpBodyIsRogue(_body)) cpArrayPush(newBodies, _body);
|
|
29
|
191 cpComponentNode node = {null, null, 0, _body.node.idleTime};
|
|
|
192 _body.node = node;
|
|
4
|
193 } while((_body = next) != root);
|
|
|
194 } else {
|
|
|
195 cpBody *_body = root;
|
|
|
196 cpBody *next;
|
|
|
197 do {
|
|
|
198 next = _body.node.next;
|
|
|
199
|
|
|
200 for(cpShape *shape = _body.shapesList; shape; shape = shape.next){
|
|
|
201 cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
|
|
|
202 cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
|
|
|
203 }
|
|
|
204 } while((_body = next) != root);
|
|
|
205
|
|
|
206 cpArrayPush(space.sleepingComponents, root);
|
|
|
207 }
|
|
|
208 }
|
|
|
209
|
|
|
210 space.bodies = newBodies;
|
|
|
211 cpArrayFree(bodies);
|
|
|
212 cpArrayFree(rogueBodies);
|
|
|
213 cpArrayFree(components);
|
|
|
214 }
|
|
|
215
|
|
|
216 void
|
|
23
|
217 cpBodySleep(cpBody *_body)
|
|
|
218 {
|
|
|
219 cpBodySleepWithGroup(_body, null);
|
|
|
220 }
|
|
|
221
|
|
|
222 void
|
|
|
223 cpBodySleepWithGroup(cpBody *_body, cpBody *group){
|
|
|
224 assert(!cpBodyIsStatic(_body) && !cpBodyIsRogue(_body), "Rogue and static bodies cannot be put to sleep.");
|
|
|
225
|
|
|
226 cpSpace *space = _body.space;
|
|
|
227 assert(space, "Cannot put a body to sleep that has not been added to a space.");
|
|
|
228 assert(!space.locked, "Bodies can not be put to sleep during a query or a call to cpSpaceSte(). Put these calls into a post-step callback.");
|
|
|
229 assert(!group || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier.");
|
|
|
230
|
|
|
231 if(cpBodyIsSleeping(_body)) return;
|
|
4
|
232
|
|
|
233 for(cpShape *shape = _body.shapesList; shape; shape = shape.next){
|
|
29
|
234 cpShapeCacheBB(shape);
|
|
4
|
235 cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
|
|
|
236 cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
|
|
|
237 }
|
|
|
238
|
|
23
|
239 if(group){
|
|
|
240 cpBody *root = componentNodeRoot(group);
|
|
|
241
|
|
|
242 cpComponentNode node = {root, root.node.next, 0, 0.0f};
|
|
|
243 _body.node = node;
|
|
|
244 root.node.next = _body;
|
|
|
245 } else {
|
|
|
246 cpComponentNode node = {null, _body, 0, 0.0f};
|
|
|
247 _body.node = node;
|
|
|
248
|
|
|
249 cpArrayPush(space.sleepingComponents, _body);
|
|
|
250 }
|
|
|
251
|
|
4
|
252 cpArrayDeleteObj(space.bodies, _body);
|
|
|
253 }
|
|
23
|
254
|
|
|
255 static void
|
|
|
256 activateTouchingHelper(cpShape *shape, cpContactPointSet *points, cpArray **bodies){
|
|
29
|
257 cpBodyActivate(shape._body);
|
|
23
|
258 }
|
|
|
259
|
|
|
260 void
|
|
|
261 cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape){
|
|
|
262 cpArray *bodies = null;
|
|
|
263 cpSpaceShapeQuery(space, shape, cast(cpSpaceShapeQueryFunc)&activateTouchingHelper, &bodies);
|
|
|
264 }
|
