|
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);
|
|
7
|
24 //TODO
|
|
6
|
25 //CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, angle, Angle);
|
|
|
26 //CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, phase, Phase);
|
|
|
27 //CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, ratchet, Ratchet);
|
|
|
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
|
|
|
97 static /+const+/ cpConstraintClass klass = {
|
|
|
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 }
|
