Mercurial > projects > chipmunkd
annotate trunk/chipmunkd/constraints/cpSlideJoint.d @ 28:4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
author | Extrawurst |
---|---|
date | Mon, 13 Dec 2010 21:40:56 +0100 |
parents | c03a41d47b60 |
children |
rev | line source |
---|---|
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); | |
13 | 27 |
28 mixin(CP_DefineConstraintProperty!("cpSlideJoint", "cpVect", "anchr1", "Anchr1")); | |
29 mixin(CP_DefineConstraintProperty!("cpSlideJoint", "cpVect", "anchr2", "Anchr2")); | |
30 mixin(CP_DefineConstraintProperty!("cpSlideJoint", "cpFloat", "min", "Min")); | |
31 mixin(CP_DefineConstraintProperty!("cpSlideJoint", "cpFloat", "max", "Max")); | |
6 | 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 | |
28
4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
Extrawurst
parents:
13
diff
changeset
|
105 __gshared /+const+/ cpConstraintClass klass = { |
6 | 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 } |