Mercurial > projects > chipmunkd
annotate trunk/chipmunkd/constraints/cpRatchetJoint.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.cpRatchetJoint; | |
5 | |
6 import chipmunkd.chipmunk; | |
7 import chipmunkd.constraints.util; | |
8 | |
9 //const cpConstraintClass *cpRatchetJointGetClass(); | |
10 | |
11 struct cpRatchetJoint { | |
12 cpConstraint constraint; | |
13 cpFloat angle, phase, ratchet; | |
14 | |
15 cpFloat iSum; | |
16 | |
17 cpFloat bias; | |
18 cpFloat jAcc, jMax; | |
19 } | |
20 | |
21 //cpRatchetJoint *cpRatchetJointAlloc(void); | |
22 //cpRatchetJoint *cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); | |
23 //cpConstraint *cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet); | |
13 | 24 |
25 mixin(CP_DefineConstraintProperty!("cpRatchetJoint", "cpFloat", "angle", "Angle")); | |
26 mixin(CP_DefineConstraintProperty!("cpRatchetJoint", "cpFloat", "phase", "Phase")); | |
27 mixin(CP_DefineConstraintProperty!("cpRatchetJoint", "cpFloat", "ratchet", "Ratchet")); | |
6 | 28 |
29 // cpRatchetJoint.c --------------------------------- | |
30 | |
31 static void | |
32 preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv) | |
33 { | |
34 mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); | |
35 | |
36 cpFloat angle = joint.angle; | |
37 cpFloat phase = joint.phase; | |
38 cpFloat ratchet = joint.ratchet; | |
39 | |
40 cpFloat delta = b.a - a.a; | |
41 cpFloat diff = angle - delta; | |
42 cpFloat pdist = 0.0f; | |
43 | |
44 if(diff*ratchet > 0.0f){ | |
45 pdist = diff; | |
46 } else { | |
47 joint.angle = cpffloor((delta - phase)/ratchet)*ratchet + phase; | |
48 } | |
49 | |
50 // calculate moment of inertia coefficient. | |
51 joint.iSum = 1.0f/(a.i_inv + b.i_inv); | |
52 | |
53 // calculate bias velocity | |
54 cpFloat maxBias = joint.constraint.maxBias; | |
55 joint.bias = cpfclamp(-joint.constraint.biasCoef*dt_inv*pdist, -maxBias, maxBias); | |
56 | |
57 // compute max impulse | |
58 joint.jMax = mixin(J_MAX!("joint", "dt")); | |
59 | |
60 // If the bias is 0, the joint is not at a limit. Reset the impulse. | |
61 if(!joint.bias) | |
62 joint.jAcc = 0.0f; | |
63 | |
64 // apply joint torque | |
65 a.w -= joint.jAcc*a.i_inv; | |
66 b.w += joint.jAcc*b.i_inv; | |
67 } | |
68 | |
69 static void | |
70 applyImpulse(cpRatchetJoint *joint) | |
71 { | |
72 if(!joint.bias) return; // early exit | |
73 | |
74 mixin(CONSTRAINT_BEGIN!("joint", "a", "b")); | |
75 | |
76 // compute relative rotational velocity | |
77 cpFloat wr = b.w - a.w; | |
78 cpFloat ratchet = joint.ratchet; | |
79 | |
80 // compute normal impulse | |
81 cpFloat j = -(joint.bias + wr)*joint.iSum; | |
82 cpFloat jOld = joint.jAcc; | |
83 joint.jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, joint.jMax*cpfabs(ratchet))/ratchet; | |
84 j = joint.jAcc - jOld; | |
85 | |
86 // apply impulse | |
87 a.w -= j*a.i_inv; | |
88 b.w += j*b.i_inv; | |
89 } | |
90 | |
91 static cpFloat | |
92 getImpulse(cpRatchetJoint *joint) | |
93 { | |
94 return cpfabs(joint.jAcc); | |
95 } | |
96 | |
28
4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
Extrawurst
parents:
13
diff
changeset
|
97 __gshared /+const+/ cpConstraintClass klass = { |
6 | 98 cast(cpConstraintPreStepFunction)&preStep, |
99 cast(cpConstraintApplyImpulseFunction)&applyImpulse, | |
100 cast(cpConstraintGetImpulseFunction)&getImpulse, | |
101 }; | |
102 mixin(CP_DefineClassGetter!("cpRatchetJoint")); | |
103 | |
104 cpRatchetJoint * | |
105 cpRatchetJointAlloc() | |
106 { | |
107 return cast(cpRatchetJoint *)cpmalloc(cpRatchetJoint.sizeof); | |
108 } | |
109 | |
110 cpRatchetJoint * | |
111 cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet) | |
112 { | |
113 cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b); | |
114 | |
115 joint.angle = 0.0f; | |
116 joint.phase = phase; | |
117 joint.ratchet = ratchet; | |
118 | |
119 // STATIC_BODY_CHECK | |
120 joint.angle = (b ? b.a : 0.0f) - (a ? a.a : 0.0f); | |
121 | |
122 return joint; | |
123 } | |
124 | |
125 cpConstraint * | |
126 cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet) | |
127 { | |
128 return cast(cpConstraint *)cpRatchetJointInit(cpRatchetJointAlloc(), a, b, phase, ratchet); | |
129 } |