|
4
|
1
|
|
|
2 // written in the D programming language
|
|
|
3
|
|
|
4 module chipmunkd.constraints.cpPinJoint;
|
|
|
5
|
|
|
6 import chipmunkd.chipmunk;
|
|
|
7 import chipmunkd.chipmunk_types_h;
|
|
|
8 import chipmunkd.cpVect,chipmunkd.cpVect_h;
|
|
|
9 import chipmunkd.cpBody;
|
|
|
10 import chipmunkd.constraints.util;
|
|
|
11
|
|
|
12 //const cpConstraintClass *cpPinJointGetClass();
|
|
|
13
|
|
|
14 struct cpPinJoint {
|
|
|
15 cpConstraint constraint;
|
|
|
16 cpVect anchr1, anchr2;
|
|
|
17 cpFloat dist;
|
|
|
18
|
|
|
19 cpVect r1, r2;
|
|
|
20 cpVect n;
|
|
|
21 cpFloat nMass;
|
|
|
22
|
|
|
23 cpFloat jnAcc, jnMax;
|
|
|
24 cpFloat bias;
|
|
|
25 }
|
|
|
26
|
|
|
27 //cpPinJoint *cpPinJointAlloc(void);
|
|
|
28 //cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
|
|
|
29 //cpConstraint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
|
|
|
30 //
|
|
|
31 //CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr1, Anchr1);
|
|
|
32 //CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr2, Anchr2);
|
|
|
33 //CP_DefineConstraintProperty(cpPinJoint, cpFloat, dist, Dist);
|
|
|
34
|
|
|
35 // cpPinJoint.c -------------------------
|
|
|
36
|
|
|
37 static void
|
|
|
38 preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv)
|
|
|
39 {
|
|
|
40 mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
|
|
|
41
|
|
|
42 joint.r1 = cpvrotate(joint.anchr1, a.rot);
|
|
|
43 joint.r2 = cpvrotate(joint.anchr2, b.rot);
|
|
|
44
|
|
|
45 cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1));
|
|
|
46 cpFloat dist = cpvlength(delta);
|
|
|
47 joint.n = cpvmult(delta, 1.0f/(dist ? dist : cast(cpFloat)INFINITY));
|
|
|
48
|
|
|
49 // calculate mass normal
|
|
|
50 joint.nMass = 1.0f/k_scalar(a, b, joint.r1, joint.r2, joint.n);
|
|
|
51
|
|
|
52 // calculate bias velocity
|
|
|
53 cpFloat maxBias = joint.constraint.maxBias;
|
|
|
54 joint.bias = cpfclamp(-joint.constraint.biasCoef*dt_inv*(dist - joint.dist), -maxBias, maxBias);
|
|
|
55
|
|
|
56 // compute max impulse
|
|
|
57 joint.jnMax = mixin(J_MAX!("joint", "dt"));
|
|
|
58
|
|
|
59 // apply accumulated impulse
|
|
|
60 cpVect j = cpvmult(joint.n, joint.jnAcc);
|
|
|
61 apply_impulses(a, b, joint.r1, joint.r2, j);
|
|
|
62 }
|
|
|
63
|
|
|
64 static void
|
|
|
65 applyImpulse(cpPinJoint *joint)
|
|
|
66 {
|
|
|
67 mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
|
|
|
68 cpVect n = joint.n;
|
|
|
69
|
|
|
70 // compute relative velocity
|
|
|
71 cpFloat vrn = normal_relative_velocity(a, b, joint.r1, joint.r2, n);
|
|
|
72
|
|
|
73 // compute normal impulse
|
|
|
74 cpFloat jn = (joint.bias - vrn)*joint.nMass;
|
|
|
75 cpFloat jnOld = joint.jnAcc;
|
|
|
76 joint.jnAcc = cpfclamp(jnOld + jn, -joint.jnMax, joint.jnMax);
|
|
|
77 jn = joint.jnAcc - jnOld;
|
|
|
78
|
|
|
79 // apply impulse
|
|
|
80 apply_impulses(a, b, joint.r1, joint.r2, cpvmult(n, jn));
|
|
|
81 }
|
|
|
82
|
|
|
83 static cpFloat
|
|
|
84 getImpulse(cpPinJoint *joint)
|
|
|
85 {
|
|
|
86 return cpfabs(joint.jnAcc);
|
|
|
87 }
|
|
|
88
|
|
|
89 static /+const+/ cpConstraintClass klass = {
|
|
|
90 cast(cpConstraintPreStepFunction)&preStep,
|
|
|
91 cast(cpConstraintApplyImpulseFunction)&applyImpulse,
|
|
|
92 cast(cpConstraintGetImpulseFunction)&getImpulse,
|
|
|
93 };
|
|
|
94 mixin(CP_DefineClassGetter!("cpPinJoint"));
|
|
|
95
|
|
|
96 cpPinJoint *
|
|
|
97 cpPinJointAlloc()
|
|
|
98 {
|
|
|
99 return cast(cpPinJoint *)cpmalloc(cpPinJoint.sizeof);
|
|
|
100 }
|
|
|
101
|
|
|
102 cpPinJoint *
|
|
|
103 cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
|
|
|
104 {
|
|
|
105 cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
|
|
|
106
|
|
|
107 joint.anchr1 = anchr1;
|
|
|
108 joint.anchr2 = anchr2;
|
|
|
109
|
|
|
110 // STATIC_BODY_CHECK
|
|
|
111 cpVect p1 = (a ? cpvadd(a.p, cpvrotate(anchr1, a.rot)) : anchr1);
|
|
|
112 cpVect p2 = (b ? cpvadd(b.p, cpvrotate(anchr2, b.rot)) : anchr2);
|
|
|
113 joint.dist = cpvlength(cpvsub(p2, p1));
|
|
|
114
|
|
|
115 joint.jnAcc = 0.0f;
|
|
|
116
|
|
|
117 return joint;
|
|
|
118 }
|
|
|
119
|
|
|
120 cpConstraint *
|
|
|
121 cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
|
|
|
122 {
|
|
|
123 return cast(cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2);
|
|
|
124 }
|
