|
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
|
|
|
104 //cpBool cpBodyIsStatic(const cpBody *body);
|
|
|
105
|
|
|
106 static cpBool
|
|
|
107 cpBodyIsRogue(const cpBody* _body)
|
|
|
108 {
|
|
|
109 return (_body.space is null);
|
|
|
110 }
|
|
|
111
|
|
13
|
112 //TODO
|
|
4
|
113 //#define CP_DefineBodyGetter(type, member, name) \
|
|
|
114 //static inline type cpBodyGet##name(const cpBody *body){return body.member;}
|
|
|
115 //
|
|
|
116 //#define CP_DefineBodySetter(type, member, name) \
|
|
|
117 //static inline void \
|
|
|
118 //cpBodySet##name(cpBody *body, const type value){ \
|
|
|
119 // cpBodyActivate(body); \
|
|
|
120 // body.member = value; \
|
|
|
121 //} \
|
|
|
122
|
|
|
123 //#define CP_DefineBodyProperty(type, member, name) \
|
|
|
124 //CP_DefineBodyGetter(type, member, name) \
|
|
|
125 //CP_DefineBodySetter(type, member, name)
|
|
|
126
|
|
14
|
127 template CP_DefineBodyGetter(string type,string member,string name)
|
|
|
128 {
|
|
|
129 enum CP_DefineBodyGetter =
|
|
|
130 "static "~type~" BodyGet"~name~"(const cpBody *_body){"~
|
|
|
131 "return _body."~member~";"~
|
|
|
132 "}";
|
|
|
133 }
|
|
|
134
|
|
|
135 template CP_DefineBodySetter(string type,string member,string name)
|
|
|
136 {
|
|
|
137 enum CP_DefineBodySetter =
|
|
|
138 "static void BodySet"~name~"(cpBody *_body, const "~type~" value){"~
|
|
|
139 "cpBodyActivate(_body);"~
|
|
|
140 "_body."~member~" = value;"~
|
|
|
141 "}";
|
|
|
142 }
|
|
|
143
|
|
|
144 template CP_DefineBodyProperty(string type,string member,string name)
|
|
|
145 {
|
|
|
146 enum CP_DefineBodyProperty =
|
|
|
147 CP_DefineBodyGetter!(type,member,name)~CP_DefineBodySetter!(type,member,name);
|
|
|
148
|
|
|
149 }
|
|
4
|
150
|
|
|
151 //// Accessors for cpBody struct members
|
|
14
|
152 mixin(CP_DefineBodyGetter!("cpFloat","m","Mass"));
|
|
4
|
153 //void cpBodySetMass(cpBody *body, cpFloat m);
|
|
|
154 //
|
|
14
|
155 mixin(CP_DefineBodyGetter!("cpFloat","i","Moment"));
|
|
4
|
156 //void cpBodySetMoment(cpBody *body, cpFloat i);
|
|
|
157 //
|
|
|
158 //
|
|
14
|
159
|
|
|
160 mixin(CP_DefineBodyProperty!("cpVect","p","Pos"));
|
|
|
161 mixin(CP_DefineBodyProperty!("cpVect","v","Vel"));
|
|
|
162 mixin(CP_DefineBodyProperty!("cpVect","f","Force"));
|
|
|
163 mixin(CP_DefineBodyProperty!("cpFloat","a","Angle"));
|
|
|
164 mixin(CP_DefineBodyProperty!("cpFloat","w","AngVel"));
|
|
|
165 mixin(CP_DefineBodyProperty!("cpFloat","t","Torque"));
|
|
|
166 mixin(CP_DefineBodyProperty!("cpVect","rot","Rot"));
|
|
|
167 mixin(CP_DefineBodyProperty!("cpFloat","v_limit","VelLimit"));
|
|
|
168 mixin(CP_DefineBodyProperty!("cpFloat","w_limit","AngVelLimit"));
|
|
4
|
169
|
|
|
170 //// Modify the velocity of the body so that it will move to the specified absolute coordinates in the next timestep.
|
|
|
171 //// Intended for objects that are moved manually with a custom velocity integration function.
|
|
|
172 //void cpBodySlew(cpBody *body, cpVect pos, cpFloat dt);
|
|
|
173 //
|
|
|
174 //// Default Integration functions.
|
|
|
175 //void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt);
|
|
|
176 //void cpBodyUpdatePosition(cpBody *body, cpFloat dt);
|
|
|
177
|
|
|
178 // Convert body local to world coordinates
|
|
|
179 static cpVect
|
|
|
180 cpBodyLocal2World(const cpBody *_body, const cpVect v)
|
|
|
181 {
|
|
|
182 return cpvadd(_body.p, cpvrotate(v, _body.rot));
|
|
|
183 }
|
|
|
184
|
|
|
185 // Convert world to body local coordinates
|
|
|
186 static cpVect
|
|
|
187 cpBodyWorld2Local(const cpBody *_body, const cpVect v)
|
|
|
188 {
|
|
|
189 return cpvunrotate(cpvsub(v, _body.p), _body.rot);
|
|
|
190 }
|
|
|
191
|
|
|
192 // Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates).
|
|
|
193 static void
|
|
|
194 cpBodyApplyImpulse(cpBody *_body, const cpVect j, const cpVect r)
|
|
|
195 {
|
|
|
196 _body.v = cpvadd(_body.v, cpvmult(j, _body.m_inv));
|
|
|
197 _body.w += _body.i_inv*cpvcross(r, j);
|
|
|
198 }
|
|
|
199
|
|
|
200 //// Zero the forces on a body.
|
|
|
201 //void cpBodyResetForces(cpBody *body);
|
|
|
202 //// Apply a force (in world coordinates) to a body at a point relative to the center of gravity (also in world coordinates).
|
|
|
203 //void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r);
|
|
|
204
|
|
|
205 static cpFloat
|
|
|
206 cpBodyKineticEnergy(const cpBody *_body)
|
|
|
207 {
|
|
|
208 // Need to do some fudging to avoid NaNs
|
|
|
209 cpFloat vsq = cpvdot(_body.v, _body.v);
|
|
|
210 cpFloat wsq = _body.w*_body.w;
|
|
|
211 return (vsq ? vsq*_body.m : 0.0f) + (wsq ? wsq*_body.i : 0.0f);
|
|
|
212 }
|
|
|
213
|
|
|
214 //// Apply a damped spring force between two bodies.
|
|
|
215 //// Warning: Large damping values can be unstable. Use a cpDampedSpring constraint for this instead.
|
|
|
216 //void cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt);
|
|
|
217
|
|
|
218 // initialized in cpInitChipmunk()
|
|
|
219 cpBody cpStaticBodySingleton;
|
|
|
220
|
|
|
221 cpBody*
|
|
|
222 cpBodyAlloc()
|
|
|
223 {
|
|
|
224 return cast(cpBody *)cpmalloc(cpBody.sizeof);
|
|
|
225 }
|
|
|
226
|
|
|
227 cpBodyVelocityFunc cpBodyUpdateVelocityDefault = &cpBodyUpdateVelocity;
|
|
|
228 cpBodyPositionFunc cpBodyUpdatePositionDefault = &cpBodyUpdatePosition;
|
|
|
229
|
|
|
230 cpBody*
|
|
|
231 cpBodyInit(cpBody *_body, cpFloat m, cpFloat i)
|
|
|
232 {
|
|
|
233 _body.velocity_func = cpBodyUpdateVelocityDefault;
|
|
|
234 _body.position_func = cpBodyUpdatePositionDefault;
|
|
|
235
|
|
|
236 cpBodySetMass(_body, m);
|
|
|
237 cpBodySetMoment(_body, i);
|
|
|
238
|
|
|
239 _body.p = cpvzero;
|
|
|
240 _body.v = cpvzero;
|
|
|
241 _body.f = cpvzero;
|
|
|
242
|
|
|
243 cpBodySetAngle(_body, 0.0f);
|
|
|
244 _body.w = 0.0f;
|
|
|
245 _body.t = 0.0f;
|
|
|
246
|
|
|
247 _body.v_bias = cpvzero;
|
|
|
248 _body.w_bias = 0.0f;
|
|
|
249
|
|
|
250 _body.data = null;
|
|
|
251 _body.v_limit = INFINITY;
|
|
|
252 _body.w_limit = INFINITY;
|
|
|
253
|
|
|
254 _body.space = null;
|
|
|
255 _body.shapesList = null;
|
|
|
256
|
|
|
257 cpComponentNode node = {null, null, 0, 0.0f};
|
|
|
258 _body.node = node;
|
|
|
259
|
|
|
260 return _body;
|
|
|
261 }
|
|
|
262
|
|
|
263 cpBody*
|
|
|
264 cpBodyNew(cpFloat m, cpFloat i)
|
|
|
265 {
|
|
|
266 return cpBodyInit(cpBodyAlloc(), m, i);
|
|
|
267 }
|
|
|
268
|
|
|
269 void cpBodyDestroy(cpBody *_body){}
|
|
|
270
|
|
|
271 void
|
|
|
272 cpBodyFree(cpBody *_body)
|
|
|
273 {
|
|
|
274 if(_body){
|
|
|
275 cpBodyDestroy(_body);
|
|
|
276 cpfree(_body);
|
|
|
277 }
|
|
|
278 }
|
|
|
279
|
|
|
280 void
|
|
|
281 cpBodySetMass(cpBody *_body, cpFloat mass)
|
|
|
282 {
|
|
|
283 _body.m = mass;
|
|
|
284 _body.m_inv = 1.0f/mass;
|
|
|
285 }
|
|
|
286
|
|
|
287 void
|
|
|
288 cpBodySetMoment(cpBody *_body, cpFloat moment)
|
|
|
289 {
|
|
|
290 _body.i = moment;
|
|
|
291 _body.i_inv = 1.0f/moment;
|
|
|
292 }
|
|
|
293
|
|
|
294 void
|
|
|
295 cpBodySetAngle(cpBody *_body, cpFloat angle)
|
|
|
296 {
|
|
|
297 _body.a = angle;//fmod(a, (cpFloat)M_PI*2.0f);
|
|
|
298 _body.rot = cpvforangle(angle);
|
|
|
299 }
|
|
|
300
|
|
|
301 void
|
|
|
302 cpBodySlew(cpBody *_body, cpVect pos, cpFloat dt)
|
|
|
303 {
|
|
|
304 cpVect delta = cpvsub(pos, _body.p);
|
|
|
305 _body.v = cpvmult(delta, 1.0f/dt);
|
|
|
306 }
|
|
|
307
|
|
|
308 void
|
|
|
309 cpBodyUpdateVelocity(cpBody *_body, cpVect gravity, cpFloat damping, cpFloat dt)
|
|
|
310 {
|
|
|
311 _body.v = cpvclamp(
|
|
|
312 cpvadd(cpvmult(_body.v, damping), cpvmult(cpvadd(gravity, cpvmult(_body.f, _body.m_inv)), dt)),
|
|
|
313 _body.v_limit);
|
|
|
314
|
|
|
315 cpFloat w_limit = _body.w_limit;
|
|
|
316 _body.w = cpfclamp(_body.w*damping + _body.t*_body.i_inv*dt, -w_limit, w_limit);
|
|
|
317 }
|
|
|
318
|
|
|
319 void
|
|
|
320 cpBodyUpdatePosition(cpBody *_body, cpFloat dt)
|
|
|
321 {
|
|
|
322 _body.p = cpvadd(_body.p, cpvmult(cpvadd(_body.v, _body.v_bias), dt));
|
|
|
323 cpBodySetAngle(_body, _body.a + (_body.w + _body.w_bias)*dt);
|
|
|
324
|
|
|
325 _body.v_bias = cpvzero;
|
|
|
326 _body.w_bias = 0.0f;
|
|
|
327 }
|
|
|
328
|
|
|
329 void
|
|
|
330 cpBodyResetForces(cpBody *_body)
|
|
|
331 {
|
|
|
332 _body.f = cpvzero;
|
|
|
333 _body.t = 0.0f;
|
|
|
334 }
|
|
|
335
|
|
|
336 void
|
|
|
337 cpBodyApplyForce(cpBody *_body, cpVect force, cpVect r)
|
|
|
338 {
|
|
|
339 _body.f = cpvadd(_body.f, force);
|
|
|
340 _body.t += cpvcross(r, force);
|
|
|
341 }
|
|
|
342
|
|
|
343 void
|
|
|
344 cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt)
|
|
|
345 {
|
|
|
346 // Calculate the world space anchor coordinates.
|
|
|
347 cpVect r1 = cpvrotate(anchr1, a.rot);
|
|
|
348 cpVect r2 = cpvrotate(anchr2, b.rot);
|
|
|
349
|
|
|
350 cpVect delta = cpvsub(cpvadd(b.p, r2), cpvadd(a.p, r1));
|
|
|
351 cpFloat dist = cpvlength(delta);
|
|
|
352 cpVect n = dist ? cpvmult(delta, 1.0f/dist) : cpvzero;
|
|
|
353
|
|
|
354 cpFloat f_spring = (dist - rlen)*k;
|
|
|
355
|
|
|
356 // Calculate the world relative velocities of the anchor points.
|
|
|
357 cpVect v1 = cpvadd(a.v, cpvmult(cpvperp(r1), a.w));
|
|
|
358 cpVect v2 = cpvadd(b.v, cpvmult(cpvperp(r2), b.w));
|
|
|
359
|
|
|
360 // Calculate the damping force.
|
|
|
361 // This really should be in the impulse solver and can produce problems when using large damping values.
|
|
|
362 cpFloat vrn = cpvdot(cpvsub(v2, v1), n);
|
|
|
363 cpFloat f_damp = vrn*cpfmin(dmp, 1.0f/(dt*(a.m_inv + b.m_inv)));
|
|
|
364
|
|
|
365 // Apply!
|
|
|
366 cpVect f = cpvmult(n, f_spring + f_damp);
|
|
|
367 cpBodyApplyForce(a, f, r1);
|
|
|
368 cpBodyApplyForce(b, cpvneg(f), r2);
|
|
|
369 }
|
|
|
370
|
|
|
371 cpBool
|
|
|
372 cpBodyIsStatic(/+const+/ cpBody *_body)
|
|
|
373 {
|
|
|
374 cpSpace *space = _body.space;
|
|
|
375 return ( (space !is null) && (_body is &space.staticBody) );
|
|
|
376 }
|
|
|
377
|
|
|
378 //void cpSpaceSleepBody(cpSpace *space, cpBody *_body);
|
|
|
379
|
|
|
380 void
|
|
|
381 cpBodySleep(cpBody *_body)
|
|
|
382 {
|
|
|
383 if(cpBodyIsSleeping(_body)) return;
|
|
|
384
|
|
|
385 assert(!cpBodyIsStatic(_body) && !cpBodyIsRogue(_body), "Rogue and static bodies cannot be put to sleep.");
|
|
13
|
386 cpSpaceSleepBody(_body.space, _body);
|
|
4
|
387 }
|
