Mercurial > projects > chipmunkd
annotate trunk/chipmunkd/cpBody.d @ 28:4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
author | Extrawurst |
---|---|
date | Mon, 13 Dec 2010 21:40:56 +0100 |
parents | 4ceef5833c8c |
children |
rev | line source |
---|---|
4 | 1 |
2 // written in the D programming language | |
3 | |
4 module chipmunkd.cpBody; | |
5 | |
6 import chipmunkd.chipmunk; | |
7 | |
8 alias void function(cpBody *_body, cpVect gravity, cpFloat damping, cpFloat dt)cpBodyVelocityFunc; | |
9 alias void function(cpBody *_body, cpFloat dt)cpBodyPositionFunc; | |
10 | |
11 //extern cpBodyVelocityFunc cpBodyUpdateVelocityDefault; | |
12 //extern cpBodyPositionFunc cpBodyUpdatePositionDefault; | |
13 | |
14 // Structure to hold information about the contact graph components | |
15 // when putting groups of objects to sleep. | |
16 // No interesting user accessible fields. | |
17 struct cpComponentNode { | |
18 cpBody *parent; | |
19 cpBody *next; | |
20 int rank; | |
21 cpFloat idleTime; | |
22 } | |
23 | |
24 struct cpBody{ | |
25 // *** Integration Functions. | |
26 | |
27 // Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity) | |
28 cpBodyVelocityFunc velocity_func; | |
29 | |
30 // Function that is called to integrate the body's position. (Defaults to cpBodyUpdatePosition) | |
31 cpBodyPositionFunc position_func; | |
32 | |
33 // *** Mass Properties | |
34 | |
35 // Mass and it's inverse. | |
36 // Always use cpBodySetMass() whenever changing the mass as these values must agree. | |
37 cpFloat m, m_inv; | |
38 | |
39 // Moment of inertia and it's inverse. | |
40 // Always use cpBodySetMoment() whenever changing the moment as these values must agree. | |
41 cpFloat i, i_inv; | |
42 | |
43 // *** Positional Properties | |
44 | |
45 // Linear components of motion (position, velocity, and force) | |
46 cpVect p, v, f; | |
47 | |
48 // Angular components of motion (angle, angular velocity, and torque) | |
49 // Always use cpBodySetAngle() to set the angle of the body as a and rot must agree. | |
50 cpFloat a, w, t; | |
51 | |
52 // Cached unit length vector representing the angle of the body. | |
53 // Used for fast vector rotation using cpvrotate(). | |
54 cpVect rot; | |
55 | |
56 // *** User Definable Fields | |
57 | |
58 // User defined data pointer. | |
59 cpDataPointer data; | |
60 | |
61 // *** Other Fields | |
62 | |
63 // Maximum velocities this body can move at after integrating velocity | |
64 cpFloat v_limit, w_limit; | |
65 | |
66 // *** Internally Used Fields | |
67 | |
68 // Velocity bias values used when solving penetrations and correcting constraints. | |
69 cpVect v_bias; | |
70 cpFloat w_bias; | |
71 | |
72 // Space this body has been added to | |
73 cpSpace* space; | |
74 | |
75 // Pointer to the shape list. | |
76 // Shapes form a linked list using cpShape.next when added to a space. | |
77 cpShape *shapesList; | |
78 | |
79 // Used by cpSpaceStep() to store contact graph information. | |
80 cpComponentNode node; | |
81 } | |
82 | |
83 //// Basic allocation/destruction functions | |
84 //cpBody *cpBodyAlloc(void); | |
85 //cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i); | |
86 //cpBody *cpBodyNew(cpFloat m, cpFloat i); | |
87 // | |
88 //void cpBodyDestroy(cpBody *body); | |
89 //void cpBodyFree(cpBody *body); | |
90 // | |
91 //// Wake up a sleeping or idle body. (defined in cpSpace.c) | |
92 //void cpBodyActivate(cpBody *body); | |
93 // | |
94 //// Force a body to sleep; | |
95 //void cpBodySleep(cpBody *body); | |
96 ////void cpBodySleepGroup(cpBody *body, ...); | |
97 | |
98 static cpBool | |
99 cpBodyIsSleeping(const cpBody *_body) | |
100 { | |
101 return (_body.node.next !is null); | |
102 } | |
103 | |
23 | 104 static cpBool |
105 cpBodyIsStatic(const cpBody *_body) | |
106 { | |
107 return _body.node.idleTime == INFINITY; | |
108 } | |
4 | 109 |
110 static cpBool | |
111 cpBodyIsRogue(const cpBody* _body) | |
112 { | |
113 return (_body.space is null); | |
114 } | |
115 | |
13 | 116 //TODO |
4 | 117 //#define CP_DefineBodyGetter(type, member, name) \ |
118 //static inline type cpBodyGet##name(const cpBody *body){return body.member;} | |
119 // | |
120 //#define CP_DefineBodySetter(type, member, name) \ | |
121 //static inline void \ | |
122 //cpBodySet##name(cpBody *body, const type value){ \ | |
123 // cpBodyActivate(body); \ | |
124 // body.member = value; \ | |
125 //} \ | |
126 | |
127 //#define CP_DefineBodyProperty(type, member, name) \ | |
128 //CP_DefineBodyGetter(type, member, name) \ | |
129 //CP_DefineBodySetter(type, member, name) | |
130 | |
14 | 131 template CP_DefineBodyGetter(string type,string member,string name) |
132 { | |
133 enum CP_DefineBodyGetter = | |
134 "static "~type~" BodyGet"~name~"(const cpBody *_body){"~ | |
135 "return _body."~member~";"~ | |
136 "}"; | |
137 } | |
138 | |
139 template CP_DefineBodySetter(string type,string member,string name) | |
140 { | |
141 enum CP_DefineBodySetter = | |
142 "static void BodySet"~name~"(cpBody *_body, const "~type~" value){"~ | |
143 "cpBodyActivate(_body);"~ | |
144 "_body."~member~" = value;"~ | |
145 "}"; | |
146 } | |
147 | |
148 template CP_DefineBodyProperty(string type,string member,string name) | |
149 { | |
150 enum CP_DefineBodyProperty = | |
151 CP_DefineBodyGetter!(type,member,name)~CP_DefineBodySetter!(type,member,name); | |
152 | |
153 } | |
4 | 154 |
155 //// Accessors for cpBody struct members | |
14 | 156 mixin(CP_DefineBodyGetter!("cpFloat","m","Mass")); |
4 | 157 //void cpBodySetMass(cpBody *body, cpFloat m); |
158 // | |
14 | 159 mixin(CP_DefineBodyGetter!("cpFloat","i","Moment")); |
4 | 160 //void cpBodySetMoment(cpBody *body, cpFloat i); |
161 // | |
162 // | |
14 | 163 |
164 mixin(CP_DefineBodyProperty!("cpVect","p","Pos")); | |
165 mixin(CP_DefineBodyProperty!("cpVect","v","Vel")); | |
166 mixin(CP_DefineBodyProperty!("cpVect","f","Force")); | |
167 mixin(CP_DefineBodyProperty!("cpFloat","a","Angle")); | |
168 mixin(CP_DefineBodyProperty!("cpFloat","w","AngVel")); | |
169 mixin(CP_DefineBodyProperty!("cpFloat","t","Torque")); | |
170 mixin(CP_DefineBodyProperty!("cpVect","rot","Rot")); | |
171 mixin(CP_DefineBodyProperty!("cpFloat","v_limit","VelLimit")); | |
172 mixin(CP_DefineBodyProperty!("cpFloat","w_limit","AngVelLimit")); | |
4 | 173 |
174 //// Modify the velocity of the body so that it will move to the specified absolute coordinates in the next timestep. | |
175 //// Intended for objects that are moved manually with a custom velocity integration function. | |
176 //void cpBodySlew(cpBody *body, cpVect pos, cpFloat dt); | |
177 // | |
178 //// Default Integration functions. | |
179 //void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); | |
180 //void cpBodyUpdatePosition(cpBody *body, cpFloat dt); | |
181 | |
182 // Convert body local to world coordinates | |
183 static cpVect | |
184 cpBodyLocal2World(const cpBody *_body, const cpVect v) | |
185 { | |
186 return cpvadd(_body.p, cpvrotate(v, _body.rot)); | |
187 } | |
188 | |
189 // Convert world to body local coordinates | |
190 static cpVect | |
191 cpBodyWorld2Local(const cpBody *_body, const cpVect v) | |
192 { | |
193 return cpvunrotate(cpvsub(v, _body.p), _body.rot); | |
194 } | |
195 | |
196 // Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates). | |
197 static void | |
198 cpBodyApplyImpulse(cpBody *_body, const cpVect j, const cpVect r) | |
199 { | |
200 _body.v = cpvadd(_body.v, cpvmult(j, _body.m_inv)); | |
201 _body.w += _body.i_inv*cpvcross(r, j); | |
202 } | |
203 | |
204 //// Zero the forces on a body. | |
205 //void cpBodyResetForces(cpBody *body); | |
206 //// Apply a force (in world coordinates) to a body at a point relative to the center of gravity (also in world coordinates). | |
207 //void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r); | |
208 | |
209 static cpFloat | |
210 cpBodyKineticEnergy(const cpBody *_body) | |
211 { | |
212 // Need to do some fudging to avoid NaNs | |
213 cpFloat vsq = cpvdot(_body.v, _body.v); | |
214 cpFloat wsq = _body.w*_body.w; | |
215 return (vsq ? vsq*_body.m : 0.0f) + (wsq ? wsq*_body.i : 0.0f); | |
216 } | |
217 | |
218 //// Apply a damped spring force between two bodies. | |
219 //// Warning: Large damping values can be unstable. Use a cpDampedSpring constraint for this instead. | |
220 //void cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt); | |
221 | |
222 // initialized in cpInitChipmunk() | |
28
4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
Extrawurst
parents:
23
diff
changeset
|
223 __gshared cpBody cpStaticBodySingleton; |
4 | 224 |
225 cpBody* | |
226 cpBodyAlloc() | |
227 { | |
228 return cast(cpBody *)cpmalloc(cpBody.sizeof); | |
229 } | |
230 | |
28
4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
Extrawurst
parents:
23
diff
changeset
|
231 __gshared cpBodyVelocityFunc cpBodyUpdateVelocityDefault = &cpBodyUpdateVelocity; |
4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
Extrawurst
parents:
23
diff
changeset
|
232 __gshared cpBodyPositionFunc cpBodyUpdatePositionDefault = &cpBodyUpdatePosition; |
4 | 233 |
234 cpBody* | |
235 cpBodyInit(cpBody *_body, cpFloat m, cpFloat i) | |
236 { | |
237 _body.velocity_func = cpBodyUpdateVelocityDefault; | |
238 _body.position_func = cpBodyUpdatePositionDefault; | |
239 | |
240 cpBodySetMass(_body, m); | |
241 cpBodySetMoment(_body, i); | |
242 | |
243 _body.p = cpvzero; | |
244 _body.v = cpvzero; | |
245 _body.f = cpvzero; | |
246 | |
247 cpBodySetAngle(_body, 0.0f); | |
248 _body.w = 0.0f; | |
249 _body.t = 0.0f; | |
250 | |
251 _body.v_bias = cpvzero; | |
252 _body.w_bias = 0.0f; | |
253 | |
254 _body.data = null; | |
255 _body.v_limit = INFINITY; | |
256 _body.w_limit = INFINITY; | |
257 | |
258 _body.space = null; | |
259 _body.shapesList = null; | |
260 | |
261 cpComponentNode node = {null, null, 0, 0.0f}; | |
262 _body.node = node; | |
263 | |
264 return _body; | |
265 } | |
266 | |
267 cpBody* | |
268 cpBodyNew(cpFloat m, cpFloat i) | |
269 { | |
270 return cpBodyInit(cpBodyAlloc(), m, i); | |
271 } | |
272 | |
23 | 273 cpBody * |
274 cpBodyInitStatic(cpBody *_body) | |
275 { | |
276 cpBodyInit(_body, cast(cpFloat)INFINITY, cast(cpFloat)INFINITY); | |
277 _body.node.idleTime = cast(cpFloat)INFINITY; | |
278 | |
279 return _body; | |
280 } | |
281 | |
282 cpBody * | |
283 cpBodyNewStatic() | |
284 { | |
285 return cpBodyInitStatic(cpBodyAlloc()); | |
286 } | |
287 | |
4 | 288 void cpBodyDestroy(cpBody *_body){} |
289 | |
290 void | |
291 cpBodyFree(cpBody *_body) | |
292 { | |
293 if(_body){ | |
294 cpBodyDestroy(_body); | |
295 cpfree(_body); | |
296 } | |
297 } | |
298 | |
299 void | |
300 cpBodySetMass(cpBody *_body, cpFloat mass) | |
301 { | |
302 _body.m = mass; | |
303 _body.m_inv = 1.0f/mass; | |
304 } | |
305 | |
306 void | |
307 cpBodySetMoment(cpBody *_body, cpFloat moment) | |
308 { | |
309 _body.i = moment; | |
310 _body.i_inv = 1.0f/moment; | |
311 } | |
312 | |
313 void | |
314 cpBodySetAngle(cpBody *_body, cpFloat angle) | |
315 { | |
316 _body.a = angle;//fmod(a, (cpFloat)M_PI*2.0f); | |
317 _body.rot = cpvforangle(angle); | |
318 } | |
319 | |
320 void | |
321 cpBodySlew(cpBody *_body, cpVect pos, cpFloat dt) | |
322 { | |
323 cpVect delta = cpvsub(pos, _body.p); | |
324 _body.v = cpvmult(delta, 1.0f/dt); | |
325 } | |
326 | |
327 void | |
328 cpBodyUpdateVelocity(cpBody *_body, cpVect gravity, cpFloat damping, cpFloat dt) | |
329 { | |
330 _body.v = cpvclamp( | |
331 cpvadd(cpvmult(_body.v, damping), cpvmult(cpvadd(gravity, cpvmult(_body.f, _body.m_inv)), dt)), | |
332 _body.v_limit); | |
333 | |
334 cpFloat w_limit = _body.w_limit; | |
335 _body.w = cpfclamp(_body.w*damping + _body.t*_body.i_inv*dt, -w_limit, w_limit); | |
336 } | |
337 | |
338 void | |
339 cpBodyUpdatePosition(cpBody *_body, cpFloat dt) | |
340 { | |
341 _body.p = cpvadd(_body.p, cpvmult(cpvadd(_body.v, _body.v_bias), dt)); | |
342 cpBodySetAngle(_body, _body.a + (_body.w + _body.w_bias)*dt); | |
343 | |
344 _body.v_bias = cpvzero; | |
345 _body.w_bias = 0.0f; | |
346 } | |
347 | |
348 void | |
349 cpBodyResetForces(cpBody *_body) | |
350 { | |
351 _body.f = cpvzero; | |
352 _body.t = 0.0f; | |
353 } | |
354 | |
355 void | |
356 cpBodyApplyForce(cpBody *_body, cpVect force, cpVect r) | |
357 { | |
358 _body.f = cpvadd(_body.f, force); | |
359 _body.t += cpvcross(r, force); | |
360 } | |
361 | |
362 void | |
363 cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt) | |
364 { | |
365 // Calculate the world space anchor coordinates. | |
366 cpVect r1 = cpvrotate(anchr1, a.rot); | |
367 cpVect r2 = cpvrotate(anchr2, b.rot); | |
368 | |
369 cpVect delta = cpvsub(cpvadd(b.p, r2), cpvadd(a.p, r1)); | |
370 cpFloat dist = cpvlength(delta); | |
371 cpVect n = dist ? cpvmult(delta, 1.0f/dist) : cpvzero; | |
372 | |
373 cpFloat f_spring = (dist - rlen)*k; | |
374 | |
375 // Calculate the world relative velocities of the anchor points. | |
376 cpVect v1 = cpvadd(a.v, cpvmult(cpvperp(r1), a.w)); | |
377 cpVect v2 = cpvadd(b.v, cpvmult(cpvperp(r2), b.w)); | |
378 | |
379 // Calculate the damping force. | |
380 // This really should be in the impulse solver and can produce problems when using large damping values. | |
381 cpFloat vrn = cpvdot(cpvsub(v2, v1), n); | |
382 cpFloat f_damp = vrn*cpfmin(dmp, 1.0f/(dt*(a.m_inv + b.m_inv))); | |
383 | |
384 // Apply! | |
385 cpVect f = cpvmult(n, f_spring + f_damp); | |
386 cpBodyApplyForce(a, f, r1); | |
387 cpBodyApplyForce(b, cpvneg(f), r2); | |
23 | 388 } |