Mercurial > projects > chipmunkd
view trunk/chipmunkd/constraints/cpPinJoint.d @ 6:707dd4e10c28
ported rest of the constraints (chipmunk 5.3.2)
author | Extrawurst |
---|---|
date | Thu, 02 Dec 2010 22:26:04 +0100 |
parents | 7ebbd4d05553 |
children | b68f10432182 |
line wrap: on
line source
// written in the D programming language module chipmunkd.constraints.cpPinJoint; import chipmunkd.chipmunk; import chipmunkd.chipmunk_types_h; import chipmunkd.cpVect,chipmunkd.cpVect_h; import chipmunkd.cpBody; import chipmunkd.constraints.util; //const cpConstraintClass *cpPinJointGetClass(); struct cpPinJoint { cpConstraint constraint; cpVect anchr1, anchr2; cpFloat dist; cpVect r1, r2; cpVect n; cpFloat nMass; cpFloat jnAcc, jnMax; cpFloat bias; } //cpPinJoint *cpPinJointAlloc(void); //cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); //cpConstraint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); // //CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr1, Anchr1); //CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr2, Anchr2); //CP_DefineConstraintProperty(cpPinJoint, cpFloat, dist, Dist); // cpPinJoint.c ------------------------- static void preStep(cpPinJoint *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); 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*(dist - joint.dist), -maxBias, maxBias); // compute max impulse joint.jnMax = mixin(J_MAX!("joint", "dt")); // apply accumulated impulse cpVect j = cpvmult(joint.n, joint.jnAcc); apply_impulses(a, b, joint.r1, joint.r2, j); } static void applyImpulse(cpPinJoint *joint) { mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); cpVect n = joint.n; // compute relative velocity cpFloat vrn = normal_relative_velocity(a, b, joint.r1, joint.r2, n); // compute normal impulse cpFloat jn = (joint.bias - vrn)*joint.nMass; cpFloat jnOld = joint.jnAcc; joint.jnAcc = cpfclamp(jnOld + jn, -joint.jnMax, joint.jnMax); jn = joint.jnAcc - jnOld; // apply impulse apply_impulses(a, b, joint.r1, joint.r2, cpvmult(n, jn)); } static cpFloat getImpulse(cpPinJoint *joint) { return cpfabs(joint.jnAcc); } static /+const+/ cpConstraintClass klass = { cast(cpConstraintPreStepFunction)&preStep, cast(cpConstraintApplyImpulseFunction)&applyImpulse, cast(cpConstraintGetImpulseFunction)&getImpulse, }; mixin(CP_DefineClassGetter!("cpPinJoint")); cpPinJoint * cpPinJointAlloc() { return cast(cpPinJoint *)cpmalloc(cpPinJoint.sizeof); } cpPinJoint * cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) { cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b); joint.anchr1 = anchr1; joint.anchr2 = anchr2; // STATIC_BODY_CHECK cpVect p1 = (a ? cpvadd(a.p, cpvrotate(anchr1, a.rot)) : anchr1); cpVect p2 = (b ? cpvadd(b.p, cpvrotate(anchr2, b.rot)) : anchr2); joint.dist = cpvlength(cpvsub(p2, p1)); joint.jnAcc = 0.0f; return joint; } cpConstraint * cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) { return cast(cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2); }