Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/constraints/cpRatchetJoint.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.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);
24 //
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 }