Mercurial > projects > chipmunkd
annotate trunk/chipmunkd/constraints/cpGrooveJoint.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 |
| rev | line source |
|---|---|
| 6 | 1 |
| 2 // written in the D programming language | |
| 3 | |
| 4 module chipmunkd.constraints.cpGrooveJoint; | |
| 5 | |
| 6 import chipmunkd.chipmunk; | |
| 7 import chipmunkd.constraints.util; | |
| 8 | |
| 9 //const cpConstraintClass *cpGrooveJointGetClass(); | |
| 10 | |
| 11 struct cpGrooveJoint { | |
| 12 cpConstraint constraint; | |
| 13 cpVect grv_n, grv_a, grv_b; | |
| 14 cpVect anchr2; | |
| 15 | |
| 16 cpVect grv_tn; | |
| 17 cpFloat clamp; | |
| 18 cpVect r1, r2; | |
| 19 cpVect k1, k2; | |
| 20 | |
| 21 cpVect jAcc; | |
| 22 cpFloat jMaxLen; | |
| 23 cpVect bias; | |
| 24 } | |
| 25 | |
| 26 //cpGrooveJoint *cpGrooveJointAlloc(void); | |
| 27 //cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); | |
| 28 //cpConstraint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); | |
| 13 | 29 |
| 30 mixin(CP_DefineConstraintGetter!("cpGrooveJoint", "cpVect", "grv_a", "GrooveA")); | |
| 6 | 31 //void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value); |
| 13 | 32 mixin(CP_DefineConstraintGetter!("cpGrooveJoint", "cpVect", "grv_b", "GrooveB")); |
| 6 | 33 //void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value); |
| 13 | 34 mixin(CP_DefineConstraintProperty!("cpGrooveJoint", "cpVect", "anchr2", "Anchr2")); |
| 6 | 35 |
| 36 // cpGrooveJoint.c --------------------------------- | |
| 37 | |
| 38 static void | |
| 39 preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv) | |
| 40 { | |
| 41 mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); | |
| 42 | |
| 43 // calculate endpoints in worldspace | |
| 44 cpVect ta = cpBodyLocal2World(a, joint.grv_a); | |
| 45 cpVect tb = cpBodyLocal2World(a, joint.grv_b); | |
| 46 | |
| 47 // calculate axis | |
| 48 cpVect n = cpvrotate(joint.grv_n, a.rot); | |
| 49 cpFloat d = cpvdot(ta, n); | |
| 50 | |
| 51 joint.grv_tn = n; | |
| 52 joint.r2 = cpvrotate(joint.anchr2, b.rot); | |
| 53 | |
| 54 // calculate tangential distance along the axis of r2 | |
| 55 cpFloat td = cpvcross(cpvadd(b.p, joint.r2), n); | |
| 56 // calculate clamping factor and r2 | |
| 57 if(td <= cpvcross(ta, n)){ | |
| 58 joint.clamp = 1.0f; | |
| 59 joint.r1 = cpvsub(ta, a.p); | |
| 60 } else if(td >= cpvcross(tb, n)){ | |
| 61 joint.clamp = -1.0f; | |
| 62 joint.r1 = cpvsub(tb, a.p); | |
| 63 } else { | |
| 64 joint.clamp = 0.0f; | |
| 65 joint.r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a.p); | |
| 66 } | |
| 67 | |
| 68 // Calculate mass tensor | |
| 69 k_tensor(a, b, joint.r1, joint.r2, &joint.k1, &joint.k2); | |
| 70 | |
| 71 // compute max impulse | |
| 72 joint.jMaxLen = mixin(J_MAX!("joint", "dt")); | |
| 73 | |
| 74 // calculate bias velocity | |
| 75 cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1)); | |
| 76 joint.bias = cpvclamp(cpvmult(delta, -joint.constraint.biasCoef*dt_inv), joint.constraint.maxBias); | |
| 77 | |
| 78 // apply accumulated impulse | |
| 79 apply_impulses(a, b, joint.r1, joint.r2, joint.jAcc); | |
| 80 } | |
| 81 | |
| 82 static cpVect | |
| 83 grooveConstrain(cpGrooveJoint *joint, cpVect j){ | |
| 84 cpVect n = joint.grv_tn; | |
| 85 cpVect jClamp = (joint.clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n); | |
| 86 return cpvclamp(jClamp, joint.jMaxLen); | |
| 87 } | |
| 88 | |
| 89 static void | |
| 90 applyImpulse(cpGrooveJoint *joint) | |
| 91 { | |
| 92 mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); | |
| 93 | |
| 94 cpVect r1 = joint.r1; | |
| 95 cpVect r2 = joint.r2; | |
| 96 | |
| 97 // compute impulse | |
| 98 cpVect vr = relative_velocity(a, b, r1, r2); | |
| 99 | |
| 100 cpVect j = mult_k(cpvsub(joint.bias, vr), joint.k1, joint.k2); | |
| 101 cpVect jOld = joint.jAcc; | |
| 102 joint.jAcc = grooveConstrain(joint, cpvadd(jOld, j)); | |
| 103 j = cpvsub(joint.jAcc, jOld); | |
| 104 | |
| 105 // apply impulse | |
| 106 apply_impulses(a, b, joint.r1, joint.r2, j); | |
| 107 } | |
| 108 | |
| 109 static cpFloat | |
| 110 getImpulse(cpGrooveJoint *joint) | |
| 111 { | |
| 112 return cpvlength(joint.jAcc); | |
| 113 } | |
| 114 | |
|
28
4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
Extrawurst
parents:
13
diff
changeset
|
115 __gshared /+const+/ cpConstraintClass klass = { |
| 6 | 116 cast(cpConstraintPreStepFunction)&preStep, |
| 117 cast(cpConstraintApplyImpulseFunction)&applyImpulse, | |
| 118 cast(cpConstraintGetImpulseFunction)&getImpulse, | |
| 119 }; | |
| 120 mixin(CP_DefineClassGetter!("cpGrooveJoint")); | |
| 121 | |
| 122 cpGrooveJoint * | |
| 123 cpGrooveJointAlloc() | |
| 124 { | |
| 125 return cast(cpGrooveJoint *)cpmalloc(cpGrooveJoint.sizeof); | |
| 126 } | |
| 127 | |
| 128 cpGrooveJoint * | |
| 129 cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2) | |
| 130 { | |
| 131 cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b); | |
| 132 | |
| 133 joint.grv_a = groove_a; | |
| 134 joint.grv_b = groove_b; | |
| 135 joint.grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a))); | |
| 136 joint.anchr2 = anchr2; | |
| 137 | |
| 138 joint.jAcc = cpvzero; | |
| 139 | |
| 140 return joint; | |
| 141 } | |
| 142 | |
| 143 cpConstraint * | |
| 144 cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2) | |
| 145 { | |
| 146 return cast(cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2); | |
| 147 } | |
| 148 | |
| 149 void | |
| 150 cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value) | |
| 151 { | |
| 152 cpGrooveJoint *g = cast(cpGrooveJoint *)constraint; | |
| 153 //TODO: | |
| 154 //cpConstraintCheckCast(constraint, cpGrooveJoint); | |
| 155 | |
| 156 g.grv_a = value; | |
| 157 g.grv_n = cpvperp(cpvnormalize(cpvsub(g.grv_b, value))); | |
| 158 | |
| 159 cpConstraintActivateBodies(constraint); | |
| 160 } | |
| 161 | |
| 162 void | |
| 163 cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value) | |
| 164 { | |
| 165 cpGrooveJoint *g = cast(cpGrooveJoint *)constraint; | |
| 166 //TODO: | |
| 167 //cpConstraintCheckCast(constraint, cpGrooveJoint); | |
| 168 | |
| 169 g.grv_b = value; | |
| 170 g.grv_n = cpvperp(cpvnormalize(cpvsub(value, g.grv_a))); | |
| 171 | |
| 172 cpConstraintActivateBodies(constraint); | |
| 173 } | |
| 174 |