|
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);
|
|
|
24 //
|
|
|
25 //CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, min, Min);
|
|
|
26 //CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, max, Max);
|
|
|
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
|
|
|
93 static /+const+/ cpConstraintClass klass = {
|
|
|
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 }
|
