Mercurial > projects > chipmunkd
view trunk/chipmunkd/constraints/cpPivotJoint.d @ 28:4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
author | Extrawurst |
---|---|
date | Mon, 13 Dec 2010 21:40:56 +0100 |
parents | c03a41d47b60 |
children |
line wrap: on
line source
// written in the D programming language module chipmunkd.constraints.cpPivotJoint; import chipmunkd.chipmunk; import chipmunkd.constraints.util; //const cpConstraintClass *cpPivotJointGetClass(); struct cpPivotJoint { cpConstraint constraint; cpVect anchr1, anchr2; cpVect r1, r2; cpVect k1, k2; cpVect jAcc; cpFloat jMaxLen; cpVect bias; } //cpPivotJoint *cpPivotJointAlloc(void); //cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); //cpConstraint *cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot); //cpConstraint *cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); mixin(CP_DefineConstraintProperty!("cpPivotJoint", "cpVect", "anchr1", "Anchr1")); mixin(CP_DefineConstraintProperty!("cpPivotJoint", "cpVect", "anchr2", "Anchr2")); static void preStep(cpPivotJoint *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); // Calculate mass tensor k_tensor(a, b, joint.r1, joint.r2, &joint.k1, &joint.k2); // compute max impulse joint.jMaxLen = mixin(J_MAX!("joint", "dt")); // calculate bias velocity cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1)); joint.bias = cpvclamp(cpvmult(delta, -joint.constraint.biasCoef*dt_inv), joint.constraint.maxBias); // apply accumulated impulse apply_impulses(a, b, joint.r1, joint.r2, joint.jAcc); } static void applyImpulse(cpPivotJoint *joint) { mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); cpVect r1 = joint.r1; cpVect r2 = joint.r2; // compute relative velocity cpVect vr = relative_velocity(a, b, r1, r2); // compute normal impulse cpVect j = mult_k(cpvsub(joint.bias, vr), joint.k1, joint.k2); cpVect jOld = joint.jAcc; joint.jAcc = cpvclamp(cpvadd(joint.jAcc, j), joint.jMaxLen); j = cpvsub(joint.jAcc, jOld); // apply impulse apply_impulses(a, b, joint.r1, joint.r2, j); } static cpFloat getImpulse(cpConstraint *joint) { return cpvlength((cast(cpPivotJoint *)joint).jAcc); } __gshared /+const+/ cpConstraintClass klass = { cast(cpConstraintPreStepFunction)&preStep, cast(cpConstraintApplyImpulseFunction)&applyImpulse, cast(cpConstraintGetImpulseFunction)&getImpulse, }; mixin(CP_DefineClassGetter!("cpPivotJoint")); cpPivotJoint * cpPivotJointAlloc() { return cast(cpPivotJoint *)cpmalloc(cpPivotJoint.sizeof); } cpPivotJoint * cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) { cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b); joint.anchr1 = anchr1; joint.anchr2 = anchr2; joint.jAcc = cpvzero; return joint; } cpConstraint * cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) { return cast(cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchr1, anchr2); } cpConstraint * cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot) { cpVect anchr1 = (a ? cpBodyWorld2Local(a, pivot) : pivot); cpVect anchr2 = (b ? cpBodyWorld2Local(b, pivot) : pivot); return cpPivotJointNew2(a, b, anchr1, anchr2); }