Mercurial > projects > chipmunkd
annotate trunk/chipmunkd/constraints/cpPivotJoint.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 |
---|---|
4 | 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); | |
13 | 27 |
28 mixin(CP_DefineConstraintProperty!("cpPivotJoint", "cpVect", "anchr1", "Anchr1")); | |
29 mixin(CP_DefineConstraintProperty!("cpPivotJoint", "cpVect", "anchr2", "Anchr2")); | |
4 | 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 | |
28
4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
Extrawurst
parents:
13
diff
changeset
|
80 __gshared /+const+/ cpConstraintClass klass = { |
4 | 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 } |