Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/constraints/cpRotaryLimitJoint.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.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 }