|
4
|
1
|
|
|
2 // written in the D programming language
|
|
|
3
|
|
|
4 module chipmunkd.cpSpaceComponent;
|
|
|
5
|
|
|
6 import chipmunkd.chipmunk;
|
|
|
7 import chipmunkd.chipmunk_types_h;
|
|
|
8 import chipmunkd.cpVect,chipmunkd.cpVect_h;
|
|
|
9 import chipmunkd.cpBody;
|
|
|
10
|
|
|
11 // Chipmunk uses a data structure called a disjoint set forest.
|
|
|
12 // My attempts to find a way to splice circularly linked lists in
|
|
|
13 // constant time failed, and so I found this neat data structure instead.
|
|
|
14
|
|
|
15 static cpBody *
|
|
|
16 componentNodeRoot(cpBody *_body)
|
|
|
17 {
|
|
|
18 cpBody *parent = _body.node.parent;
|
|
|
19
|
|
|
20 if(parent){
|
|
|
21 // path compression, attaches this node directly to the root
|
|
|
22 return (_body.node.parent = componentNodeRoot(parent));
|
|
|
23 } else {
|
|
|
24 return _body;
|
|
|
25 }
|
|
|
26 }
|
|
|
27
|
|
|
28 static void
|
|
|
29 componentNodeMerge(cpBody *a_root, cpBody *b_root)
|
|
|
30 {
|
|
|
31 if(a_root.node.rank < b_root.node.rank){
|
|
|
32 a_root.node.parent = b_root;
|
|
|
33 } else if(a_root.node.rank > b_root.node.rank){
|
|
|
34 b_root.node.parent = a_root;
|
|
|
35 } else if(a_root != b_root){
|
|
|
36 b_root.node.parent = a_root;
|
|
|
37 a_root.node.rank++;
|
|
|
38 }
|
|
|
39 }
|
|
|
40
|
|
|
41 static void
|
|
|
42 componentActivate(cpBody *root)
|
|
|
43 {
|
|
|
44 if(!cpBodyIsSleeping(root)) return;
|
|
|
45
|
|
|
46 cpSpace *space = root.space;
|
|
|
47 assert(space, "Trying to activate a body that was never added to a space.");
|
|
|
48
|
|
|
49 cpBody* _body = root;
|
|
|
50 cpBody* next;
|
|
|
51 do {
|
|
|
52 next = _body.node.next;
|
|
|
53
|
|
|
54 cpComponentNode node = {null, null, 0, 0.0f};
|
|
|
55 _body.node = node;
|
|
|
56 cpArrayPush(space.bodies, _body);
|
|
|
57
|
|
|
58 for(cpShape *shape=_body.shapesList; shape; shape=shape.next){
|
|
|
59 cpSpaceHashRemove(space.staticShapes, shape, shape.hashid);
|
|
|
60 cpSpaceHashInsert(space.activeShapes, shape, shape.hashid, shape.bb);
|
|
|
61 }
|
|
|
62 } while((_body = next) != root);
|
|
|
63
|
|
|
64 cpArrayDeleteObj(space.sleepingComponents, root);
|
|
|
65 }
|
|
|
66
|
|
|
67 void
|
|
|
68 cpBodyActivate(cpBody *_body)
|
|
|
69 {
|
|
|
70 // Reset the idle time even if it's not in a currently sleeping component
|
|
|
71 // Like a body resting on or jointed to a rogue body.
|
|
|
72 _body.node.idleTime = 0.0f;
|
|
|
73 componentActivate(componentNodeRoot(_body));
|
|
|
74 }
|
|
|
75
|
|
|
76 static void
|
|
|
77 mergeBodies(cpSpace *space, cpArray *components, cpArray *rogueBodies, cpBody *a, cpBody *b)
|
|
|
78 {
|
|
|
79 // Don't merge with the static body
|
|
|
80 if(cpBodyIsStatic(a) || cpBodyIsStatic(b)) return;
|
|
|
81
|
|
|
82 cpBody *a_root = componentNodeRoot(a);
|
|
|
83 cpBody *b_root = componentNodeRoot(b);
|
|
|
84
|
|
|
85 cpBool a_sleep = cpBodyIsSleeping(a_root);
|
|
|
86 cpBool b_sleep = cpBodyIsSleeping(b_root);
|
|
|
87
|
|
|
88 if(a_sleep && b_sleep){
|
|
|
89 return;
|
|
|
90 } else if(a_sleep || b_sleep){
|
|
|
91 componentActivate(a_root);
|
|
|
92 componentActivate(b_root);
|
|
|
93 }
|
|
|
94
|
|
|
95 // Add any rogue bodies (bodies not added to the space)
|
|
|
96 if(!a.space) cpArrayPush(rogueBodies, a);
|
|
|
97 if(!b.space) cpArrayPush(rogueBodies, b);
|
|
|
98
|
|
|
99 componentNodeMerge(a_root, b_root);
|
|
|
100 }
|
|
|
101
|
|
|
102 static cpBool
|
|
|
103 componentActive(cpBody *root, cpFloat threshold)
|
|
|
104 {
|
|
|
105 cpBody *_body = root;
|
|
|
106 cpBody *next;
|
|
|
107 do {
|
|
|
108 next = _body.node.next;
|
|
|
109 if(cpBodyIsRogue(_body) || _body.node.idleTime < threshold) return cpTrue;
|
|
|
110 } while((_body = next) != root);
|
|
|
111
|
|
|
112 return cpFalse;
|
|
|
113 }
|
|
|
114
|
|
|
115 static void
|
|
|
116 addToComponent(cpBody *_body, cpArray *components)
|
|
|
117 {
|
|
|
118 // Check that the body is not already added to the component list
|
|
|
119 if(_body.node.next) return;
|
|
|
120 cpBody *root = componentNodeRoot(_body);
|
|
|
121
|
|
|
122 cpBody *next = root.node.next;
|
|
|
123 if(!next){
|
|
|
124 // If the root isn't part of a list yet, then it hasn't been
|
|
|
125 // added to the components list. Do that now.
|
|
|
126 cpArrayPush(components, root);
|
|
|
127 // Start the list
|
|
|
128 _body.node.next = root;
|
|
|
129 root.node.next = _body;
|
|
|
130 } else if(root != _body) {
|
|
|
131 // Splice in body after the root.
|
|
|
132 _body.node.next = next;
|
|
|
133 root.node.next = _body;
|
|
|
134 }
|
|
|
135 }
|
|
|
136
|
|
|
137 // TODO this function needs more commenting.
|
|
|
138 void
|
|
|
139 cpSpaceProcessComponents(cpSpace *space, cpFloat dt)
|
|
|
140 {
|
|
|
141 cpArray *bodies = space.bodies;
|
|
|
142 cpArray *newBodies = cpArrayNew(bodies.num);
|
|
|
143 cpArray *rogueBodies = cpArrayNew(16);
|
|
|
144 cpArray *arbiters = space.arbiters;
|
|
|
145 cpArray *constraints = space.constraints;
|
|
|
146 cpArray *components = cpArrayNew(bodies.num/8);
|
|
|
147
|
|
|
148 cpFloat dv = space.idleSpeedThreshold;
|
|
|
149 cpFloat dvsq = (dv ? dv*dv : cpvdot(space.gravity, space.gravity)*dt*dt);
|
|
|
150 // update idling
|
|
|
151 for(int i=0; i<bodies.num; i++){
|
|
|
152 cpBody *_body = cast(cpBody*)bodies.arr[i];
|
|
|
153
|
|
|
154 cpFloat thresh = (dvsq ? _body.m*dvsq : 0.0f);
|
|
|
155 _body.node.idleTime = (cpBodyKineticEnergy(_body) > thresh ? 0.0f : _body.node.idleTime + dt);
|
|
|
156 }
|
|
|
157
|
|
|
158 // iterate graph edges and build forests
|
|
|
159 for(int i=0; i<arbiters.num; i++){
|
|
|
160 cpArbiter *arb = cast(cpArbiter*)arbiters.arr[i];
|
|
|
161 mergeBodies(space, components, rogueBodies, arb.private_a._body, arb.private_b._body);
|
|
|
162 }
|
|
|
163 for(int j=0; j<constraints.num; j++){
|
|
|
164 cpConstraint *constraint = cast(cpConstraint *)constraints.arr[j];
|
|
|
165 mergeBodies(space, components, rogueBodies, constraint.a, constraint.b);
|
|
|
166 }
|
|
|
167
|
|
|
168 // iterate bodies and add them to their components
|
|
|
169 for(int i=0; i<bodies.num; i++)
|
|
|
170 addToComponent(cast(cpBody*)bodies.arr[i], components);
|
|
|
171 for(int i=0; i<rogueBodies.num; i++)
|
|
|
172 addToComponent(cast(cpBody*)rogueBodies.arr[i], components);
|
|
|
173
|
|
|
174 // iterate components, copy or deactivate
|
|
|
175 for(int i=0; i<components.num; i++){
|
|
|
176 cpBody *root = cast(cpBody*)components.arr[i];
|
|
|
177 if(componentActive(root, space.sleepTimeThreshold)){
|
|
|
178 cpBody *_body = root;
|
|
|
179 cpBody *next;
|
|
|
180 do {
|
|
|
181 next = _body.node.next;
|
|
|
182
|
|
|
183 if(!cpBodyIsRogue(_body)) cpArrayPush(newBodies, _body);
|
|
|
184 _body.node.next = null;
|
|
|
185 _body.node.parent = null;
|
|
|
186 _body.node.rank = 0;
|
|
|
187 } while((_body = next) != root);
|
|
|
188 } else {
|
|
|
189 cpBody *_body = root;
|
|
|
190 cpBody *next;
|
|
|
191 do {
|
|
|
192 next = _body.node.next;
|
|
|
193
|
|
|
194 for(cpShape *shape = _body.shapesList; shape; shape = shape.next){
|
|
|
195 cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
|
|
|
196 cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
|
|
|
197 }
|
|
|
198 } while((_body = next) != root);
|
|
|
199
|
|
|
200 cpArrayPush(space.sleepingComponents, root);
|
|
|
201 }
|
|
|
202 }
|
|
|
203
|
|
|
204 space.bodies = newBodies;
|
|
|
205 cpArrayFree(bodies);
|
|
|
206 cpArrayFree(rogueBodies);
|
|
|
207 cpArrayFree(components);
|
|
|
208 }
|
|
|
209
|
|
|
210 void
|
|
|
211 cpSpaceSleepBody(cpSpace *space, cpBody *_body){
|
|
|
212 cpComponentNode node = {null, _body, 0, 0.0f};
|
|
|
213 _body.node = node;
|
|
|
214
|
|
|
215 for(cpShape *shape = _body.shapesList; shape; shape = shape.next){
|
|
|
216 cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
|
|
|
217
|
|
|
218 cpShapeCacheBB(shape);
|
|
|
219 cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
|
|
|
220 }
|
|
|
221
|
|
|
222 cpArrayPush(space.sleepingComponents, _body);
|
|
|
223 cpArrayDeleteObj(space.bodies, _body);
|
|
|
224 }
|
