Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/constraints/cpPivotJoint.d @ 4:7ebbd4d05553

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
initial commit
author Extrawurst
date Thu, 02 Dec 2010 02:11:26 +0100
parents
children b68f10432182
comparison
equal deleted inserted replaced
3:81145a61c2fe 4:7ebbd4d05553
1
2 // written in the D programming language
3
4 module chipmunkd.constraints.cpPivotJoint;
5
6 import chipmunkd.chipmunk;
7 import chipmunkd.constraints.util;
8
9 //const cpConstraintClass *cpPivotJointGetClass();
10
11 struct cpPivotJoint {
12 cpConstraint constraint;
13 cpVect anchr1, anchr2;
14
15 cpVect r1, r2;
16 cpVect k1, k2;
17
18 cpVect jAcc;
19 cpFloat jMaxLen;
20 cpVect bias;
21 }
22
23 //cpPivotJoint *cpPivotJointAlloc(void);
24 //cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
25 //cpConstraint *cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot);
26 //cpConstraint *cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
27 //
28 //CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr1, Anchr1);
29 //CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr2, Anchr2);
30
31 static void
32 preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv)
33 {
34 mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
35
36 joint.r1 = cpvrotate(joint.anchr1, a.rot);
37 joint.r2 = cpvrotate(joint.anchr2, b.rot);
38
39 // Calculate mass tensor
40 k_tensor(a, b, joint.r1, joint.r2, &joint.k1, &joint.k2);
41
42 // compute max impulse
43 joint.jMaxLen = mixin(J_MAX!("joint", "dt"));
44
45 // calculate bias velocity
46 cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1));
47 joint.bias = cpvclamp(cpvmult(delta, -joint.constraint.biasCoef*dt_inv), joint.constraint.maxBias);
48
49 // apply accumulated impulse
50 apply_impulses(a, b, joint.r1, joint.r2, joint.jAcc);
51 }
52
53 static void
54 applyImpulse(cpPivotJoint *joint)
55 {
56 mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
57
58 cpVect r1 = joint.r1;
59 cpVect r2 = joint.r2;
60
61 // compute relative velocity
62 cpVect vr = relative_velocity(a, b, r1, r2);
63
64 // compute normal impulse
65 cpVect j = mult_k(cpvsub(joint.bias, vr), joint.k1, joint.k2);
66 cpVect jOld = joint.jAcc;
67 joint.jAcc = cpvclamp(cpvadd(joint.jAcc, j), joint.jMaxLen);
68 j = cpvsub(joint.jAcc, jOld);
69
70 // apply impulse
71 apply_impulses(a, b, joint.r1, joint.r2, j);
72 }
73
74 static cpFloat
75 getImpulse(cpConstraint *joint)
76 {
77 return cpvlength((cast(cpPivotJoint *)joint).jAcc);
78 }
79
80 static /+const+/ cpConstraintClass klass = {
81 cast(cpConstraintPreStepFunction)&preStep,
82 cast(cpConstraintApplyImpulseFunction)&applyImpulse,
83 cast(cpConstraintGetImpulseFunction)&getImpulse,
84 };
85 mixin(CP_DefineClassGetter!("cpPivotJoint"));
86
87 cpPivotJoint *
88 cpPivotJointAlloc()
89 {
90 return cast(cpPivotJoint *)cpmalloc(cpPivotJoint.sizeof);
91 }
92
93 cpPivotJoint *
94 cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
95 {
96 cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
97
98 joint.anchr1 = anchr1;
99 joint.anchr2 = anchr2;
100
101 joint.jAcc = cpvzero;
102
103 return joint;
104 }
105
106 cpConstraint *
107 cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
108 {
109 return cast(cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchr1, anchr2);
110 }
111
112 cpConstraint *
113 cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot)
114 {
115 cpVect anchr1 = (a ? cpBodyWorld2Local(a, pivot) : pivot);
116 cpVect anchr2 = (b ? cpBodyWorld2Local(b, pivot) : pivot);
117 return cpPivotJointNew2(a, b, anchr1, anchr2);
118 }