Mercurial > projects > chipmunkd
view trunk/chipmunkd/cpBody.d @ 13:c03a41d47b60
- finished all constraints properties
- implemented cpAssertWarn the mixin way
author | Extrawurst |
---|---|
date | Fri, 03 Dec 2010 21:38:01 +0100 |
parents | 7ebbd4d05553 |
children | d88862c82f06 |
line wrap: on
line source
// written in the D programming language module chipmunkd.cpBody; import chipmunkd.chipmunk; import chipmunkd.chipmunk_types_h; import chipmunkd.cpVect,chipmunkd.cpVect_h; import chipmunkd.cpShape; alias void function(cpBody *_body, cpVect gravity, cpFloat damping, cpFloat dt)cpBodyVelocityFunc; alias void function(cpBody *_body, cpFloat dt)cpBodyPositionFunc; //extern cpBodyVelocityFunc cpBodyUpdateVelocityDefault; //extern cpBodyPositionFunc cpBodyUpdatePositionDefault; // Structure to hold information about the contact graph components // when putting groups of objects to sleep. // No interesting user accessible fields. struct cpComponentNode { cpBody *parent; cpBody *next; int rank; cpFloat idleTime; } struct cpBody{ // *** Integration Functions. // Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity) cpBodyVelocityFunc velocity_func; // Function that is called to integrate the body's position. (Defaults to cpBodyUpdatePosition) cpBodyPositionFunc position_func; // *** Mass Properties // Mass and it's inverse. // Always use cpBodySetMass() whenever changing the mass as these values must agree. cpFloat m, m_inv; // Moment of inertia and it's inverse. // Always use cpBodySetMoment() whenever changing the moment as these values must agree. cpFloat i, i_inv; // *** Positional Properties // Linear components of motion (position, velocity, and force) cpVect p, v, f; // Angular components of motion (angle, angular velocity, and torque) // Always use cpBodySetAngle() to set the angle of the body as a and rot must agree. cpFloat a, w, t; // Cached unit length vector representing the angle of the body. // Used for fast vector rotation using cpvrotate(). cpVect rot; // *** User Definable Fields // User defined data pointer. cpDataPointer data; // *** Other Fields // Maximum velocities this body can move at after integrating velocity cpFloat v_limit, w_limit; // *** Internally Used Fields // Velocity bias values used when solving penetrations and correcting constraints. cpVect v_bias; cpFloat w_bias; // Space this body has been added to cpSpace* space; // Pointer to the shape list. // Shapes form a linked list using cpShape.next when added to a space. cpShape *shapesList; // Used by cpSpaceStep() to store contact graph information. cpComponentNode node; } //// Basic allocation/destruction functions //cpBody *cpBodyAlloc(void); //cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i); //cpBody *cpBodyNew(cpFloat m, cpFloat i); // //void cpBodyDestroy(cpBody *body); //void cpBodyFree(cpBody *body); // //// Wake up a sleeping or idle body. (defined in cpSpace.c) //void cpBodyActivate(cpBody *body); // //// Force a body to sleep; //void cpBodySleep(cpBody *body); ////void cpBodySleepGroup(cpBody *body, ...); static cpBool cpBodyIsSleeping(const cpBody *_body) { return (_body.node.next !is null); } //cpBool cpBodyIsStatic(const cpBody *body); static cpBool cpBodyIsRogue(const cpBody* _body) { return (_body.space is null); } //TODO //#define CP_DefineBodyGetter(type, member, name) \ //static inline type cpBodyGet##name(const cpBody *body){return body.member;} // //#define CP_DefineBodySetter(type, member, name) \ //static inline void \ //cpBodySet##name(cpBody *body, const type value){ \ // cpBodyActivate(body); \ // body.member = value; \ //} \ //#define CP_DefineBodyProperty(type, member, name) \ //CP_DefineBodyGetter(type, member, name) \ //CP_DefineBodySetter(type, member, name) //// Accessors for cpBody struct members //CP_DefineBodyGetter(cpFloat, m, Mass); //void cpBodySetMass(cpBody *body, cpFloat m); // //CP_DefineBodyGetter(cpFloat, i, Moment); //void cpBodySetMoment(cpBody *body, cpFloat i); // // //CP_DefineBodyProperty(cpVect, p, Pos); //CP_DefineBodyProperty(cpVect, v, Vel); //CP_DefineBodyProperty(cpVect, f, Force); static cpFloat cpBodyGetAngle(const cpBody *_body){return _body.a;} static cpFloat cpBodySetAngle(cpBody *_body, const cpFloat value){cpBodyActivate(_body); return _body.a = value;} //CP_DefineBodyProperty(cpFloat, w, AngVel); //CP_DefineBodyProperty(cpFloat, t, Torque); //CP_DefineBodyGetter(cpVect, rot, Rot); //CP_DefineBodyProperty(cpFloat, v_limit, VelLimit); //CP_DefineBodyProperty(cpFloat, w_limit, AngVelLimit); //// Modify the velocity of the body so that it will move to the specified absolute coordinates in the next timestep. //// Intended for objects that are moved manually with a custom velocity integration function. //void cpBodySlew(cpBody *body, cpVect pos, cpFloat dt); // //// Default Integration functions. //void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); //void cpBodyUpdatePosition(cpBody *body, cpFloat dt); // Convert body local to world coordinates static cpVect cpBodyLocal2World(const cpBody *_body, const cpVect v) { return cpvadd(_body.p, cpvrotate(v, _body.rot)); } // Convert world to body local coordinates static cpVect cpBodyWorld2Local(const cpBody *_body, const cpVect v) { return cpvunrotate(cpvsub(v, _body.p), _body.rot); } // Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates). static void cpBodyApplyImpulse(cpBody *_body, const cpVect j, const cpVect r) { _body.v = cpvadd(_body.v, cpvmult(j, _body.m_inv)); _body.w += _body.i_inv*cpvcross(r, j); } //// Zero the forces on a body. //void cpBodyResetForces(cpBody *body); //// Apply a force (in world coordinates) to a body at a point relative to the center of gravity (also in world coordinates). //void cpBodyApplyForce(cpBody *body, const cpVect f, const cpVect r); static cpFloat cpBodyKineticEnergy(const cpBody *_body) { // Need to do some fudging to avoid NaNs cpFloat vsq = cpvdot(_body.v, _body.v); cpFloat wsq = _body.w*_body.w; return (vsq ? vsq*_body.m : 0.0f) + (wsq ? wsq*_body.i : 0.0f); } //// Apply a damped spring force between two bodies. //// Warning: Large damping values can be unstable. Use a cpDampedSpring constraint for this instead. //void cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt); // initialized in cpInitChipmunk() cpBody cpStaticBodySingleton; cpBody* cpBodyAlloc() { return cast(cpBody *)cpmalloc(cpBody.sizeof); } cpBodyVelocityFunc cpBodyUpdateVelocityDefault = &cpBodyUpdateVelocity; cpBodyPositionFunc cpBodyUpdatePositionDefault = &cpBodyUpdatePosition; cpBody* cpBodyInit(cpBody *_body, cpFloat m, cpFloat i) { _body.velocity_func = cpBodyUpdateVelocityDefault; _body.position_func = cpBodyUpdatePositionDefault; cpBodySetMass(_body, m); cpBodySetMoment(_body, i); _body.p = cpvzero; _body.v = cpvzero; _body.f = cpvzero; cpBodySetAngle(_body, 0.0f); _body.w = 0.0f; _body.t = 0.0f; _body.v_bias = cpvzero; _body.w_bias = 0.0f; _body.data = null; _body.v_limit = INFINITY; _body.w_limit = INFINITY; _body.space = null; _body.shapesList = null; cpComponentNode node = {null, null, 0, 0.0f}; _body.node = node; return _body; } cpBody* cpBodyNew(cpFloat m, cpFloat i) { return cpBodyInit(cpBodyAlloc(), m, i); } void cpBodyDestroy(cpBody *_body){} void cpBodyFree(cpBody *_body) { if(_body){ cpBodyDestroy(_body); cpfree(_body); } } void cpBodySetMass(cpBody *_body, cpFloat mass) { _body.m = mass; _body.m_inv = 1.0f/mass; } void cpBodySetMoment(cpBody *_body, cpFloat moment) { _body.i = moment; _body.i_inv = 1.0f/moment; } void cpBodySetAngle(cpBody *_body, cpFloat angle) { _body.a = angle;//fmod(a, (cpFloat)M_PI*2.0f); _body.rot = cpvforangle(angle); } void cpBodySlew(cpBody *_body, cpVect pos, cpFloat dt) { cpVect delta = cpvsub(pos, _body.p); _body.v = cpvmult(delta, 1.0f/dt); } void cpBodyUpdateVelocity(cpBody *_body, cpVect gravity, cpFloat damping, cpFloat dt) { _body.v = cpvclamp( cpvadd(cpvmult(_body.v, damping), cpvmult(cpvadd(gravity, cpvmult(_body.f, _body.m_inv)), dt)), _body.v_limit); cpFloat w_limit = _body.w_limit; _body.w = cpfclamp(_body.w*damping + _body.t*_body.i_inv*dt, -w_limit, w_limit); } void cpBodyUpdatePosition(cpBody *_body, cpFloat dt) { _body.p = cpvadd(_body.p, cpvmult(cpvadd(_body.v, _body.v_bias), dt)); cpBodySetAngle(_body, _body.a + (_body.w + _body.w_bias)*dt); _body.v_bias = cpvzero; _body.w_bias = 0.0f; } void cpBodyResetForces(cpBody *_body) { _body.f = cpvzero; _body.t = 0.0f; } void cpBodyApplyForce(cpBody *_body, cpVect force, cpVect r) { _body.f = cpvadd(_body.f, force); _body.t += cpvcross(r, force); } void cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt) { // Calculate the world space anchor coordinates. cpVect r1 = cpvrotate(anchr1, a.rot); cpVect r2 = cpvrotate(anchr2, b.rot); cpVect delta = cpvsub(cpvadd(b.p, r2), cpvadd(a.p, r1)); cpFloat dist = cpvlength(delta); cpVect n = dist ? cpvmult(delta, 1.0f/dist) : cpvzero; cpFloat f_spring = (dist - rlen)*k; // Calculate the world relative velocities of the anchor points. cpVect v1 = cpvadd(a.v, cpvmult(cpvperp(r1), a.w)); cpVect v2 = cpvadd(b.v, cpvmult(cpvperp(r2), b.w)); // Calculate the damping force. // This really should be in the impulse solver and can produce problems when using large damping values. cpFloat vrn = cpvdot(cpvsub(v2, v1), n); cpFloat f_damp = vrn*cpfmin(dmp, 1.0f/(dt*(a.m_inv + b.m_inv))); // Apply! cpVect f = cpvmult(n, f_spring + f_damp); cpBodyApplyForce(a, f, r1); cpBodyApplyForce(b, cpvneg(f), r2); } cpBool cpBodyIsStatic(/+const+/ cpBody *_body) { cpSpace *space = _body.space; return ( (space !is null) && (_body is &space.staticBody) ); } //void cpSpaceSleepBody(cpSpace *space, cpBody *_body); void cpBodySleep(cpBody *_body) { if(cpBodyIsSleeping(_body)) return; assert(!cpBodyIsStatic(_body) && !cpBodyIsRogue(_body), "Rogue and static bodies cannot be put to sleep."); cpSpaceSleepBody(_body.space, _body); }