|
6
|
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 //
|
|
7
|
30 //TODO
|
|
6
|
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
|
