Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/cpBody.d @ 4:7ebbd4d05553

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