Mercurial > projects > chipmunkd
view trunk/chipmunkd/constraints/cpRatchetJoint.d @ 13:c03a41d47b60
- finished all constraints properties
- implemented cpAssertWarn the mixin way
author | Extrawurst |
---|---|
date | Fri, 03 Dec 2010 21:38:01 +0100 |
parents | b68f10432182 |
children | 4541ca17975b |
line wrap: on
line source
// written in the D programming language module chipmunkd.constraints.cpRatchetJoint; import chipmunkd.chipmunk; import chipmunkd.constraints.util; //const cpConstraintClass *cpRatchetJointGetClass(); struct cpRatchetJoint { cpConstraint constraint; cpFloat angle, phase, ratchet; cpFloat iSum; cpFloat bias; cpFloat jAcc, jMax; } //cpRatchetJoint *cpRatchetJointAlloc(void); //cpRatchetJoint *cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); //cpConstraint *cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); mixin(CP_DefineConstraintProperty!("cpRatchetJoint", "cpFloat", "angle", "Angle")); mixin(CP_DefineConstraintProperty!("cpRatchetJoint", "cpFloat", "phase", "Phase")); mixin(CP_DefineConstraintProperty!("cpRatchetJoint", "cpFloat", "ratchet", "Ratchet")); // cpRatchetJoint.c --------------------------------- static void preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv) { mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); cpFloat angle = joint.angle; cpFloat phase = joint.phase; cpFloat ratchet = joint.ratchet; cpFloat delta = b.a - a.a; cpFloat diff = angle - delta; cpFloat pdist = 0.0f; if(diff*ratchet > 0.0f){ pdist = diff; } else { joint.angle = cpffloor((delta - phase)/ratchet)*ratchet + phase; } // calculate moment of inertia coefficient. joint.iSum = 1.0f/(a.i_inv + b.i_inv); // calculate bias velocity cpFloat maxBias = joint.constraint.maxBias; joint.bias = cpfclamp(-joint.constraint.biasCoef*dt_inv*pdist, -maxBias, maxBias); // compute max impulse joint.jMax = mixin(J_MAX!("joint", "dt")); // If the bias is 0, the joint is not at a limit. Reset the impulse. if(!joint.bias) joint.jAcc = 0.0f; // apply joint torque a.w -= joint.jAcc*a.i_inv; b.w += joint.jAcc*b.i_inv; } static void applyImpulse(cpRatchetJoint *joint) { if(!joint.bias) return; // early exit mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); // compute relative rotational velocity cpFloat wr = b.w - a.w; cpFloat ratchet = joint.ratchet; // compute normal impulse cpFloat j = -(joint.bias + wr)*joint.iSum; cpFloat jOld = joint.jAcc; joint.jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, joint.jMax*cpfabs(ratchet))/ratchet; j = joint.jAcc - jOld; // apply impulse a.w -= j*a.i_inv; b.w += j*b.i_inv; } static cpFloat getImpulse(cpRatchetJoint *joint) { return cpfabs(joint.jAcc); } static /+const+/ cpConstraintClass klass = { cast(cpConstraintPreStepFunction)&preStep, cast(cpConstraintApplyImpulseFunction)&applyImpulse, cast(cpConstraintGetImpulseFunction)&getImpulse, }; mixin(CP_DefineClassGetter!("cpRatchetJoint")); cpRatchetJoint * cpRatchetJointAlloc() { return cast(cpRatchetJoint *)cpmalloc(cpRatchetJoint.sizeof); } cpRatchetJoint * cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet) { cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b); joint.angle = 0.0f; joint.phase = phase; joint.ratchet = ratchet; // STATIC_BODY_CHECK joint.angle = (b ? b.a : 0.0f) - (a ? a.a : 0.0f); return joint; } cpConstraint * cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet) { return cast(cpConstraint *)cpRatchetJointInit(cpRatchetJointAlloc(), a, b, phase, ratchet); }