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