Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/constraints/cpGearJoint.d @ 6:707dd4e10c28

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
ported rest of the constraints (chipmunk 5.3.2)
author Extrawurst
date Thu, 02 Dec 2010 22:26:04 +0100
parents
children b68f10432182
comparison
equal deleted inserted replaced
5:6dd4bcf20f03 6:707dd4e10c28
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 }