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