|
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()
|
|
|
223 cpBody cpStaticBodySingleton;
|
|
|
224
|
|
|
225 cpBody*
|
|
|
226 cpBodyAlloc()
|
|
|
227 {
|
|
|
228 return cast(cpBody *)cpmalloc(cpBody.sizeof);
|
|
|
229 }
|
|
|
230
|
|
|
231 cpBodyVelocityFunc cpBodyUpdateVelocityDefault = &cpBodyUpdateVelocity;
|
|
|
232 cpBodyPositionFunc cpBodyUpdatePositionDefault = &cpBodyUpdatePosition;
|
|
|
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 } |
