|
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);
|
|
|
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 }
|
