|
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);
|
|
|
25 //
|
|
|
26 //CP_DefineConstraintProperty(cpGearJoint, cpFloat, phase, Phase);
|
|
|
27 //CP_DefineConstraintGetter(cpGearJoint, cpFloat, ratio, Ratio);
|
|
|
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
|
|
|
78 static /+const+/ cpConstraintClass klass = {
|
|
|
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 }
|
