|
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
|
|
|
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.");
|
|
13
|
366 cpSpaceSleepBody(_body.space, _body);
|
|
4
|
367 }
|
