|
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.");
|
|
|
45
|
|
|
46 cpBody* _body = root;
|
|
|
47 cpBody* next;
|
|
|
48 do {
|
|
|
49 next = _body.node.next;
|
|
|
50
|
|
|
51 cpComponentNode node = {null, null, 0, 0.0f};
|
|
|
52 _body.node = node;
|
|
|
53 cpArrayPush(space.bodies, _body);
|
|
|
54
|
|
|
55 for(cpShape *shape=_body.shapesList; shape; shape=shape.next){
|
|
|
56 cpSpaceHashRemove(space.staticShapes, shape, shape.hashid);
|
|
|
57 cpSpaceHashInsert(space.activeShapes, shape, shape.hashid, shape.bb);
|
|
|
58 }
|
|
|
59 } while((_body = next) != root);
|
|
|
60
|
|
|
61 cpArrayDeleteObj(space.sleepingComponents, root);
|
|
|
62 }
|
|
|
63
|
|
|
64 void
|
|
|
65 cpBodyActivate(cpBody *_body)
|
|
|
66 {
|
|
|
67 // Reset the idle time even if it's not in a currently sleeping component
|
|
|
68 // Like a body resting on or jointed to a rogue body.
|
|
|
69 _body.node.idleTime = 0.0f;
|
|
|
70 componentActivate(componentNodeRoot(_body));
|
|
|
71 }
|
|
|
72
|
|
|
73 static void
|
|
|
74 mergeBodies(cpSpace *space, cpArray *components, cpArray *rogueBodies, cpBody *a, cpBody *b)
|
|
|
75 {
|
|
|
76 // Don't merge with the static body
|
|
|
77 if(cpBodyIsStatic(a) || cpBodyIsStatic(b)) return;
|
|
|
78
|
|
|
79 cpBody *a_root = componentNodeRoot(a);
|
|
|
80 cpBody *b_root = componentNodeRoot(b);
|
|
|
81
|
|
|
82 cpBool a_sleep = cpBodyIsSleeping(a_root);
|
|
|
83 cpBool b_sleep = cpBodyIsSleeping(b_root);
|
|
|
84
|
|
|
85 if(a_sleep && b_sleep){
|
|
|
86 return;
|
|
|
87 } else if(a_sleep || b_sleep){
|
|
|
88 componentActivate(a_root);
|
|
|
89 componentActivate(b_root);
|
|
|
90 }
|
|
|
91
|
|
|
92 // Add any rogue bodies (bodies not added to the space)
|
|
|
93 if(!a.space) cpArrayPush(rogueBodies, a);
|
|
|
94 if(!b.space) cpArrayPush(rogueBodies, b);
|
|
|
95
|
|
|
96 componentNodeMerge(a_root, b_root);
|
|
|
97 }
|
|
|
98
|
|
|
99 static cpBool
|
|
|
100 componentActive(cpBody *root, cpFloat threshold)
|
|
|
101 {
|
|
|
102 cpBody *_body = root;
|
|
|
103 cpBody *next;
|
|
|
104 do {
|
|
|
105 next = _body.node.next;
|
|
|
106 if(cpBodyIsRogue(_body) || _body.node.idleTime < threshold) return cpTrue;
|
|
|
107 } while((_body = next) != root);
|
|
|
108
|
|
|
109 return cpFalse;
|
|
|
110 }
|
|
|
111
|
|
|
112 static void
|
|
|
113 addToComponent(cpBody *_body, cpArray *components)
|
|
|
114 {
|
|
|
115 // Check that the body is not already added to the component list
|
|
|
116 if(_body.node.next) return;
|
|
|
117 cpBody *root = componentNodeRoot(_body);
|
|
|
118
|
|
|
119 cpBody *next = root.node.next;
|
|
|
120 if(!next){
|
|
|
121 // If the root isn't part of a list yet, then it hasn't been
|
|
|
122 // added to the components list. Do that now.
|
|
|
123 cpArrayPush(components, root);
|
|
|
124 // Start the list
|
|
|
125 _body.node.next = root;
|
|
|
126 root.node.next = _body;
|
|
|
127 } else if(root != _body) {
|
|
|
128 // Splice in body after the root.
|
|
|
129 _body.node.next = next;
|
|
|
130 root.node.next = _body;
|
|
|
131 }
|
|
|
132 }
|
|
|
133
|
|
|
134 // TODO this function needs more commenting.
|
|
|
135 void
|
|
|
136 cpSpaceProcessComponents(cpSpace *space, cpFloat dt)
|
|
|
137 {
|
|
|
138 cpArray *bodies = space.bodies;
|
|
|
139 cpArray *newBodies = cpArrayNew(bodies.num);
|
|
|
140 cpArray *rogueBodies = cpArrayNew(16);
|
|
|
141 cpArray *arbiters = space.arbiters;
|
|
|
142 cpArray *constraints = space.constraints;
|
|
|
143 cpArray *components = cpArrayNew(bodies.num/8);
|
|
|
144
|
|
|
145 cpFloat dv = space.idleSpeedThreshold;
|
|
|
146 cpFloat dvsq = (dv ? dv*dv : cpvdot(space.gravity, space.gravity)*dt*dt);
|
|
|
147 // update idling
|
|
|
148 for(int i=0; i<bodies.num; i++){
|
|
|
149 cpBody *_body = cast(cpBody*)bodies.arr[i];
|
|
|
150
|
|
|
151 cpFloat thresh = (dvsq ? _body.m*dvsq : 0.0f);
|
|
|
152 _body.node.idleTime = (cpBodyKineticEnergy(_body) > thresh ? 0.0f : _body.node.idleTime + dt);
|
|
|
153 }
|
|
|
154
|
|
|
155 // iterate graph edges and build forests
|
|
|
156 for(int i=0; i<arbiters.num; i++){
|
|
|
157 cpArbiter *arb = cast(cpArbiter*)arbiters.arr[i];
|
|
|
158 mergeBodies(space, components, rogueBodies, arb.private_a._body, arb.private_b._body);
|
|
|
159 }
|
|
|
160 for(int j=0; j<constraints.num; j++){
|
|
|
161 cpConstraint *constraint = cast(cpConstraint *)constraints.arr[j];
|
|
|
162 mergeBodies(space, components, rogueBodies, constraint.a, constraint.b);
|
|
|
163 }
|
|
|
164
|
|
|
165 // iterate bodies and add them to their components
|
|
|
166 for(int i=0; i<bodies.num; i++)
|
|
|
167 addToComponent(cast(cpBody*)bodies.arr[i], components);
|
|
|
168 for(int i=0; i<rogueBodies.num; i++)
|
|
|
169 addToComponent(cast(cpBody*)rogueBodies.arr[i], components);
|
|
|
170
|
|
|
171 // iterate components, copy or deactivate
|
|
|
172 for(int i=0; i<components.num; i++){
|
|
|
173 cpBody *root = cast(cpBody*)components.arr[i];
|
|
|
174 if(componentActive(root, space.sleepTimeThreshold)){
|
|
|
175 cpBody *_body = root;
|
|
|
176 cpBody *next;
|
|
|
177 do {
|
|
|
178 next = _body.node.next;
|
|
|
179
|
|
|
180 if(!cpBodyIsRogue(_body)) cpArrayPush(newBodies, _body);
|
|
|
181 _body.node.next = null;
|
|
|
182 _body.node.parent = null;
|
|
|
183 _body.node.rank = 0;
|
|
|
184 } while((_body = next) != root);
|
|
|
185 } else {
|
|
|
186 cpBody *_body = root;
|
|
|
187 cpBody *next;
|
|
|
188 do {
|
|
|
189 next = _body.node.next;
|
|
|
190
|
|
|
191 for(cpShape *shape = _body.shapesList; shape; shape = shape.next){
|
|
|
192 cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
|
|
|
193 cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
|
|
|
194 }
|
|
|
195 } while((_body = next) != root);
|
|
|
196
|
|
|
197 cpArrayPush(space.sleepingComponents, root);
|
|
|
198 }
|
|
|
199 }
|
|
|
200
|
|
|
201 space.bodies = newBodies;
|
|
|
202 cpArrayFree(bodies);
|
|
|
203 cpArrayFree(rogueBodies);
|
|
|
204 cpArrayFree(components);
|
|
|
205 }
|
|
|
206
|
|
|
207 void
|
|
|
208 cpSpaceSleepBody(cpSpace *space, cpBody *_body){
|
|
|
209 cpComponentNode node = {null, _body, 0, 0.0f};
|
|
|
210 _body.node = node;
|
|
|
211
|
|
|
212 for(cpShape *shape = _body.shapesList; shape; shape = shape.next){
|
|
|
213 cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
|
|
|
214
|
|
|
215 cpShapeCacheBB(shape);
|
|
|
216 cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
|
|
|
217 }
|
|
|
218
|
|
|
219 cpArrayPush(space.sleepingComponents, _body);
|
|
|
220 cpArrayDeleteObj(space.bodies, _body);
|
|
|
221 }
|
