Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/constraints/cpSlideJoint.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.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);
27 //
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 }