Mercurial > projects > chipmunkd
view trunk/chipmunkd/constraints/cpSlideJoint.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.cpSlideJoint; import chipmunkd.chipmunk; import chipmunkd.constraints.util; //const cpConstraintClass *cpSlideJointGetClass(); struct cpSlideJoint { cpConstraint constraint; cpVect anchr1, anchr2; cpFloat min, max; cpVect r1, r2; cpVect n; cpFloat nMass; cpFloat jnAcc, jnMax; cpFloat bias; } //cpSlideJoint *cpSlideJointAlloc(void); //cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); //cpConstraint *cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); mixin(CP_DefineConstraintProperty!("cpSlideJoint", "cpVect", "anchr1", "Anchr1")); mixin(CP_DefineConstraintProperty!("cpSlideJoint", "cpVect", "anchr2", "Anchr2")); mixin(CP_DefineConstraintProperty!("cpSlideJoint", "cpFloat", "min", "Min")); mixin(CP_DefineConstraintProperty!("cpSlideJoint", "cpFloat", "max", "Max")); // cpSlideJoint.c --------------------------------- static void preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv) { mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); joint.r1 = cpvrotate(joint.anchr1, a.rot); joint.r2 = cpvrotate(joint.anchr2, b.rot); cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1)); cpFloat dist = cpvlength(delta); cpFloat pdist = 0.0f; if(dist > joint.max) { pdist = dist - joint.max; } else if(dist < joint.min) { pdist = joint.min - dist; dist = -dist; } joint.n = cpvmult(delta, 1.0f/(dist ? dist : cast(cpFloat)INFINITY)); // calculate mass normal joint.nMass = 1.0f/k_scalar(a, b, joint.r1, joint.r2, joint.n); // calculate bias velocity cpFloat maxBias = joint.constraint.maxBias; joint.bias = cpfclamp(-joint.constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias); // compute max impulse joint.jnMax = mixin(J_MAX!("joint", "dt")); // apply accumulated impulse if(!joint.bias) //{ // if bias is 0, then the joint is not at a limit. joint.jnAcc = 0.0f; // } else { cpVect j = cpvmult(joint.n, joint.jnAcc); apply_impulses(a, b, joint.r1, joint.r2, j); // } } static void applyImpulse(cpSlideJoint *joint) { if(!joint.bias) return; // early exit mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); cpVect n = joint.n; cpVect r1 = joint.r1; cpVect r2 = joint.r2; // compute relative velocity cpVect vr = relative_velocity(a, b, r1, r2); cpFloat vrn = cpvdot(vr, n); // compute normal impulse cpFloat jn = (joint.bias - vrn)*joint.nMass; cpFloat jnOld = joint.jnAcc; joint.jnAcc = cpfclamp(jnOld + jn, -joint.jnMax, 0.0f); jn = joint.jnAcc - jnOld; // apply impulse apply_impulses(a, b, joint.r1, joint.r2, cpvmult(n, jn)); } static cpFloat getImpulse(cpConstraint *joint) { return cpfabs((cast(cpSlideJoint *)joint).jnAcc); } static /+const+/ cpConstraintClass klass = { cast(cpConstraintPreStepFunction)&preStep, cast(cpConstraintApplyImpulseFunction)&applyImpulse, cast(cpConstraintGetImpulseFunction)&getImpulse, }; mixin(CP_DefineClassGetter!("cpSlideJoint")); cpSlideJoint * cpSlideJointAlloc() { return cast(cpSlideJoint *)cpmalloc(cpSlideJoint.sizeof); } cpSlideJoint * cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max) { cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b); joint.anchr1 = anchr1; joint.anchr2 = anchr2; joint.min = min; joint.max = max; joint.jnAcc = 0.0f; return joint; } cpConstraint * cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max) { return cast(cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchr1, anchr2, min, max); }