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