Mercurial > projects > chipmunkd
annotate trunk/chipmunkd/constraints/cpPinJoint.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 | df4ebc8add66 |
| children |
| rev | line source |
|---|---|
| 4 | 1 |
| 2 // written in the D programming language | |
| 3 | |
| 4 module chipmunkd.constraints.cpPinJoint; | |
| 5 | |
| 6 import chipmunkd.chipmunk; | |
| 7 import chipmunkd.constraints.util; | |
| 8 | |
| 9 //const cpConstraintClass *cpPinJointGetClass(); | |
| 10 | |
| 11 struct cpPinJoint { | |
| 12 cpConstraint constraint; | |
| 13 cpVect anchr1, anchr2; | |
| 14 cpFloat dist; | |
| 15 | |
| 16 cpVect r1, r2; | |
| 17 cpVect n; | |
| 18 cpFloat nMass; | |
| 19 | |
| 20 cpFloat jnAcc, jnMax; | |
| 21 cpFloat bias; | |
| 22 } | |
| 23 | |
| 24 //cpPinJoint *cpPinJointAlloc(void); | |
| 25 //cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); | |
| 26 //cpConstraint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); | |
| 7 | 27 |
| 28 mixin(CP_DefineConstraintProperty!("cpPinJoint", "cpVect", "anchr1", "Anchr1")); | |
| 29 mixin(CP_DefineConstraintProperty!("cpPinJoint", "cpVect", "anchr2", "Anchr2")); | |
| 30 mixin(CP_DefineConstraintProperty!("cpPinJoint", "cpFloat", "dist", "Dist")); | |
| 4 | 31 |
| 32 // cpPinJoint.c ------------------------- | |
| 33 | |
| 34 static void | |
| 35 preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv) | |
| 36 { | |
| 37 mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); | |
| 38 | |
| 39 joint.r1 = cpvrotate(joint.anchr1, a.rot); | |
| 40 joint.r2 = cpvrotate(joint.anchr2, b.rot); | |
| 41 | |
| 42 cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1)); | |
| 43 cpFloat dist = cpvlength(delta); | |
| 44 joint.n = cpvmult(delta, 1.0f/(dist ? dist : cast(cpFloat)INFINITY)); | |
| 45 | |
| 46 // calculate mass normal | |
| 47 joint.nMass = 1.0f/k_scalar(a, b, joint.r1, joint.r2, joint.n); | |
| 48 | |
| 49 // calculate bias velocity | |
| 50 cpFloat maxBias = joint.constraint.maxBias; | |
| 51 joint.bias = cpfclamp(-joint.constraint.biasCoef*dt_inv*(dist - joint.dist), -maxBias, maxBias); | |
| 52 | |
| 53 // compute max impulse | |
| 54 joint.jnMax = mixin(J_MAX!("joint", "dt")); | |
| 55 | |
| 56 // apply accumulated impulse | |
| 57 cpVect j = cpvmult(joint.n, joint.jnAcc); | |
| 58 apply_impulses(a, b, joint.r1, joint.r2, j); | |
| 59 } | |
| 60 | |
| 61 static void | |
| 62 applyImpulse(cpPinJoint *joint) | |
| 63 { | |
| 64 mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); | |
| 65 cpVect n = joint.n; | |
| 66 | |
| 67 // compute relative velocity | |
| 68 cpFloat vrn = normal_relative_velocity(a, b, joint.r1, joint.r2, n); | |
| 69 | |
| 70 // compute normal impulse | |
| 71 cpFloat jn = (joint.bias - vrn)*joint.nMass; | |
| 72 cpFloat jnOld = joint.jnAcc; | |
| 73 joint.jnAcc = cpfclamp(jnOld + jn, -joint.jnMax, joint.jnMax); | |
| 74 jn = joint.jnAcc - jnOld; | |
| 75 | |
| 76 // apply impulse | |
| 77 apply_impulses(a, b, joint.r1, joint.r2, cpvmult(n, jn)); | |
| 78 } | |
| 79 | |
| 80 static cpFloat | |
| 81 getImpulse(cpPinJoint *joint) | |
| 82 { | |
| 83 return cpfabs(joint.jnAcc); | |
| 84 } | |
| 85 | |
|
28
4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
Extrawurst
parents:
15
diff
changeset
|
86 __gshared /+const+/ cpConstraintClass klass = { |
| 4 | 87 cast(cpConstraintPreStepFunction)&preStep, |
| 88 cast(cpConstraintApplyImpulseFunction)&applyImpulse, | |
| 89 cast(cpConstraintGetImpulseFunction)&getImpulse, | |
| 90 }; | |
| 91 mixin(CP_DefineClassGetter!("cpPinJoint")); | |
| 92 | |
| 93 cpPinJoint * | |
| 94 cpPinJointAlloc() | |
| 95 { | |
| 96 return cast(cpPinJoint *)cpmalloc(cpPinJoint.sizeof); | |
| 97 } | |
| 98 | |
| 99 cpPinJoint * | |
| 100 cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) | |
| 101 { | |
| 102 cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b); | |
| 103 | |
| 104 joint.anchr1 = anchr1; | |
| 105 joint.anchr2 = anchr2; | |
| 106 | |
| 107 // STATIC_BODY_CHECK | |
| 108 cpVect p1 = (a ? cpvadd(a.p, cpvrotate(anchr1, a.rot)) : anchr1); | |
| 109 cpVect p2 = (b ? cpvadd(b.p, cpvrotate(anchr2, b.rot)) : anchr2); | |
| 110 joint.dist = cpvlength(cpvsub(p2, p1)); | |
| 111 | |
| 112 joint.jnAcc = 0.0f; | |
| 113 | |
| 114 return joint; | |
| 115 } | |
| 116 | |
| 117 cpConstraint * | |
| 118 cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) | |
| 119 { | |
| 120 return cast(cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2); | |
| 121 } |