|
6
|
1
|
|
|
2 // written in the D programming language
|
|
|
3
|
|
|
4 module chipmunkd.constraints.cpSlideJoint;
|
|
|
5
|
|
|
6 import chipmunkd.chipmunk;
|
|
|
7 import chipmunkd.constraints.util;
|
|
|
8
|
|
|
9 //const cpConstraintClass *cpSlideJointGetClass();
|
|
|
10
|
|
|
11 struct cpSlideJoint {
|
|
|
12 cpConstraint constraint;
|
|
|
13 cpVect anchr1, anchr2;
|
|
|
14 cpFloat min, max;
|
|
|
15
|
|
|
16 cpVect r1, r2;
|
|
|
17 cpVect n;
|
|
|
18 cpFloat nMass;
|
|
|
19
|
|
|
20 cpFloat jnAcc, jnMax;
|
|
|
21 cpFloat bias;
|
|
|
22 }
|
|
|
23
|
|
|
24 //cpSlideJoint *cpSlideJointAlloc(void);
|
|
|
25 //cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
|
|
|
26 //cpConstraint *cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
|
|
7
|
27 //TODO
|
|
6
|
28 //CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr1, Anchr1);
|
|
|
29 //CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr2, Anchr2);
|
|
|
30 //CP_DefineConstraintProperty(cpSlideJoint, cpFloat, min, Min);
|
|
|
31 //CP_DefineConstraintProperty(cpSlideJoint, cpFloat, max, Max);
|
|
|
32
|
|
|
33 // cpSlideJoint.c ---------------------------------
|
|
|
34
|
|
|
35 static void
|
|
|
36 preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv)
|
|
|
37 {
|
|
|
38 mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
|
|
|
39
|
|
|
40 joint.r1 = cpvrotate(joint.anchr1, a.rot);
|
|
|
41 joint.r2 = cpvrotate(joint.anchr2, b.rot);
|
|
|
42
|
|
|
43 cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1));
|
|
|
44 cpFloat dist = cpvlength(delta);
|
|
|
45 cpFloat pdist = 0.0f;
|
|
|
46 if(dist > joint.max) {
|
|
|
47 pdist = dist - joint.max;
|
|
|
48 } else if(dist < joint.min) {
|
|
|
49 pdist = joint.min - dist;
|
|
|
50 dist = -dist;
|
|
|
51 }
|
|
|
52 joint.n = cpvmult(delta, 1.0f/(dist ? dist : cast(cpFloat)INFINITY));
|
|
|
53
|
|
|
54 // calculate mass normal
|
|
|
55 joint.nMass = 1.0f/k_scalar(a, b, joint.r1, joint.r2, joint.n);
|
|
|
56
|
|
|
57 // calculate bias velocity
|
|
|
58 cpFloat maxBias = joint.constraint.maxBias;
|
|
|
59 joint.bias = cpfclamp(-joint.constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias);
|
|
|
60
|
|
|
61 // compute max impulse
|
|
|
62 joint.jnMax = mixin(J_MAX!("joint", "dt"));
|
|
|
63
|
|
|
64 // apply accumulated impulse
|
|
|
65 if(!joint.bias) //{
|
|
|
66 // if bias is 0, then the joint is not at a limit.
|
|
|
67 joint.jnAcc = 0.0f;
|
|
|
68 // } else {
|
|
|
69 cpVect j = cpvmult(joint.n, joint.jnAcc);
|
|
|
70 apply_impulses(a, b, joint.r1, joint.r2, j);
|
|
|
71 // }
|
|
|
72 }
|
|
|
73
|
|
|
74 static void
|
|
|
75 applyImpulse(cpSlideJoint *joint)
|
|
|
76 {
|
|
|
77 if(!joint.bias) return; // early exit
|
|
|
78
|
|
|
79 mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
|
|
|
80
|
|
|
81 cpVect n = joint.n;
|
|
|
82 cpVect r1 = joint.r1;
|
|
|
83 cpVect r2 = joint.r2;
|
|
|
84
|
|
|
85 // compute relative velocity
|
|
|
86 cpVect vr = relative_velocity(a, b, r1, r2);
|
|
|
87 cpFloat vrn = cpvdot(vr, n);
|
|
|
88
|
|
|
89 // compute normal impulse
|
|
|
90 cpFloat jn = (joint.bias - vrn)*joint.nMass;
|
|
|
91 cpFloat jnOld = joint.jnAcc;
|
|
|
92 joint.jnAcc = cpfclamp(jnOld + jn, -joint.jnMax, 0.0f);
|
|
|
93 jn = joint.jnAcc - jnOld;
|
|
|
94
|
|
|
95 // apply impulse
|
|
|
96 apply_impulses(a, b, joint.r1, joint.r2, cpvmult(n, jn));
|
|
|
97 }
|
|
|
98
|
|
|
99 static cpFloat
|
|
|
100 getImpulse(cpConstraint *joint)
|
|
|
101 {
|
|
|
102 return cpfabs((cast(cpSlideJoint *)joint).jnAcc);
|
|
|
103 }
|
|
|
104
|
|
|
105 static /+const+/ cpConstraintClass klass = {
|
|
|
106 cast(cpConstraintPreStepFunction)&preStep,
|
|
|
107 cast(cpConstraintApplyImpulseFunction)&applyImpulse,
|
|
|
108 cast(cpConstraintGetImpulseFunction)&getImpulse,
|
|
|
109 };
|
|
|
110 mixin(CP_DefineClassGetter!("cpSlideJoint"));
|
|
|
111
|
|
|
112 cpSlideJoint *
|
|
|
113 cpSlideJointAlloc()
|
|
|
114 {
|
|
|
115 return cast(cpSlideJoint *)cpmalloc(cpSlideJoint.sizeof);
|
|
|
116 }
|
|
|
117
|
|
|
118 cpSlideJoint *
|
|
|
119 cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
|
|
|
120 {
|
|
|
121 cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
|
|
|
122
|
|
|
123 joint.anchr1 = anchr1;
|
|
|
124 joint.anchr2 = anchr2;
|
|
|
125 joint.min = min;
|
|
|
126 joint.max = max;
|
|
|
127
|
|
|
128 joint.jnAcc = 0.0f;
|
|
|
129
|
|
|
130 return joint;
|
|
|
131 }
|
|
|
132
|
|
|
133 cpConstraint *
|
|
|
134 cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
|
|
|
135 {
|
|
|
136 return cast(cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchr1, anchr2, min, max);
|
|
|
137 } |
