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