Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/constraints/cpGrooveJoint.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.cpGrooveJoint;
5
6 import chipmunkd.chipmunk;
7 import chipmunkd.constraints.util;
8
9 //const cpConstraintClass *cpGrooveJointGetClass();
10
11 struct cpGrooveJoint {
12 cpConstraint constraint;
13 cpVect grv_n, grv_a, grv_b;
14 cpVect anchr2;
15
16 cpVect grv_tn;
17 cpFloat clamp;
18 cpVect r1, r2;
19 cpVect k1, k2;
20
21 cpVect jAcc;
22 cpFloat jMaxLen;
23 cpVect bias;
24 }
25
26 //cpGrooveJoint *cpGrooveJointAlloc(void);
27 //cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
28 //cpConstraint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
29 //
30 //
31 //CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_a, GrooveA);
32 //void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value);
33 //CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_b, GrooveB);
34 //void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value);
35 //CP_DefineConstraintProperty(cpGrooveJoint, cpVect, anchr2, Anchr2);
36
37 // cpGrooveJoint.c ---------------------------------
38
39 static void
40 preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv)
41 {
42 mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
43
44 // calculate endpoints in worldspace
45 cpVect ta = cpBodyLocal2World(a, joint.grv_a);
46 cpVect tb = cpBodyLocal2World(a, joint.grv_b);
47
48 // calculate axis
49 cpVect n = cpvrotate(joint.grv_n, a.rot);
50 cpFloat d = cpvdot(ta, n);
51
52 joint.grv_tn = n;
53 joint.r2 = cpvrotate(joint.anchr2, b.rot);
54
55 // calculate tangential distance along the axis of r2
56 cpFloat td = cpvcross(cpvadd(b.p, joint.r2), n);
57 // calculate clamping factor and r2
58 if(td <= cpvcross(ta, n)){
59 joint.clamp = 1.0f;
60 joint.r1 = cpvsub(ta, a.p);
61 } else if(td >= cpvcross(tb, n)){
62 joint.clamp = -1.0f;
63 joint.r1 = cpvsub(tb, a.p);
64 } else {
65 joint.clamp = 0.0f;
66 joint.r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a.p);
67 }
68
69 // Calculate mass tensor
70 k_tensor(a, b, joint.r1, joint.r2, &joint.k1, &joint.k2);
71
72 // compute max impulse
73 joint.jMaxLen = mixin(J_MAX!("joint", "dt"));
74
75 // calculate bias velocity
76 cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1));
77 joint.bias = cpvclamp(cpvmult(delta, -joint.constraint.biasCoef*dt_inv), joint.constraint.maxBias);
78
79 // apply accumulated impulse
80 apply_impulses(a, b, joint.r1, joint.r2, joint.jAcc);
81 }
82
83 static cpVect
84 grooveConstrain(cpGrooveJoint *joint, cpVect j){
85 cpVect n = joint.grv_tn;
86 cpVect jClamp = (joint.clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n);
87 return cpvclamp(jClamp, joint.jMaxLen);
88 }
89
90 static void
91 applyImpulse(cpGrooveJoint *joint)
92 {
93 mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
94
95 cpVect r1 = joint.r1;
96 cpVect r2 = joint.r2;
97
98 // compute impulse
99 cpVect vr = relative_velocity(a, b, r1, r2);
100
101 cpVect j = mult_k(cpvsub(joint.bias, vr), joint.k1, joint.k2);
102 cpVect jOld = joint.jAcc;
103 joint.jAcc = grooveConstrain(joint, cpvadd(jOld, j));
104 j = cpvsub(joint.jAcc, jOld);
105
106 // apply impulse
107 apply_impulses(a, b, joint.r1, joint.r2, j);
108 }
109
110 static cpFloat
111 getImpulse(cpGrooveJoint *joint)
112 {
113 return cpvlength(joint.jAcc);
114 }
115
116 static /+const+/ cpConstraintClass klass = {
117 cast(cpConstraintPreStepFunction)&preStep,
118 cast(cpConstraintApplyImpulseFunction)&applyImpulse,
119 cast(cpConstraintGetImpulseFunction)&getImpulse,
120 };
121 mixin(CP_DefineClassGetter!("cpGrooveJoint"));
122
123 cpGrooveJoint *
124 cpGrooveJointAlloc()
125 {
126 return cast(cpGrooveJoint *)cpmalloc(cpGrooveJoint.sizeof);
127 }
128
129 cpGrooveJoint *
130 cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
131 {
132 cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
133
134 joint.grv_a = groove_a;
135 joint.grv_b = groove_b;
136 joint.grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a)));
137 joint.anchr2 = anchr2;
138
139 joint.jAcc = cpvzero;
140
141 return joint;
142 }
143
144 cpConstraint *
145 cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
146 {
147 return cast(cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2);
148 }
149
150 void
151 cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value)
152 {
153 cpGrooveJoint *g = cast(cpGrooveJoint *)constraint;
154 //TODO:
155 //cpConstraintCheckCast(constraint, cpGrooveJoint);
156
157 g.grv_a = value;
158 g.grv_n = cpvperp(cpvnormalize(cpvsub(g.grv_b, value)));
159
160 cpConstraintActivateBodies(constraint);
161 }
162
163 void
164 cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value)
165 {
166 cpGrooveJoint *g = cast(cpGrooveJoint *)constraint;
167 //TODO:
168 //cpConstraintCheckCast(constraint, cpGrooveJoint);
169
170 g.grv_b = value;
171 g.grv_n = cpvperp(cpvnormalize(cpvsub(value, g.grv_a)));
172
173 cpConstraintActivateBodies(constraint);
174 }
175