1 /*
2 * Copyright (c) 2007-2013 Scott Lembcke and Howling Moon Software
3 *
4 * Permission is hereby granted, free of charge, to any person obtaining a copy
5 * of this software and associated documentation files (the "Software"), to deal
6 * in the Software without restriction, including without limitation the rights
7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8 * copies of the Software, and to permit persons to whom the Software is
9 * furnished to do so, subject to the following conditions:
10 *
11 * The above copyright notice and this permission notice shall be included in
12 * all copies or substantial portions of the Software.
13 *
14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
20 * SOFTWARE.
21 */
22 module dchip.cpPinJoint;
23
24 import std..string;
25
26 import dchip.constraints_util;
27 import dchip.chipmunk;
28 import dchip.cpBody;
29 import dchip.cpConstraint;
30 import dchip.chipmunk_types;
31 import dchip.cpVect;
32
33 //~ const cpConstraintClass* cpPinJointGetClass();
34
35 /// @private
36 struct cpPinJoint
37 {
38 cpConstraint constraint;
39 cpVect anchr1, anchr2;
40 cpFloat dist = 0;
41
42 cpVect r1, r2;
43 cpVect n;
44 cpFloat nMass = 0;
45
46 cpFloat jnAcc = 0;
47 cpFloat bias = 0;
48 }
49
50 mixin CP_DefineConstraintProperty!("cpPinJoint", cpVect, "anchr1", "Anchr1");
51 mixin CP_DefineConstraintProperty!("cpPinJoint", cpVect, "anchr2", "Anchr2");
52 mixin CP_DefineConstraintProperty!("cpPinJoint", cpFloat, "dist", "Dist");
53
54 void preStep(cpPinJoint* joint, cpFloat dt)
55 {
56 cpBody* a = joint.constraint.a;
57 cpBody* b = joint.constraint.b;
58
59 joint.r1 = cpvrotate(joint.anchr1, a.rot);
60 joint.r2 = cpvrotate(joint.anchr2, b.rot);
61
62 cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1));
63 cpFloat dist = cpvlength(delta);
64 joint.n = cpvmult(delta, 1.0f / (dist ? dist : cast(cpFloat)INFINITY));
65
66 // calculate mass normal
67 joint.nMass = 1.0f / k_scalar(a, b, joint.r1, joint.r2, joint.n);
68
69 // calculate bias velocity
70 cpFloat maxBias = joint.constraint.maxBias;
71 joint.bias = cpfclamp(-bias_coef(joint.constraint.errorBias, dt) * (dist - joint.dist) / dt, -maxBias, maxBias);
72 }
73
74 void applyCachedImpulse(cpPinJoint* joint, cpFloat dt_coef)
75 {
76 cpBody* a = joint.constraint.a;
77 cpBody* b = joint.constraint.b;
78
79 cpVect j = cpvmult(joint.n, joint.jnAcc * dt_coef);
80 apply_impulses(a, b, joint.r1, joint.r2, j);
81 }
82
83 void applyImpulse(cpPinJoint* joint, cpFloat dt)
84 {
85 cpBody* a = joint.constraint.a;
86 cpBody* b = joint.constraint.b;
87 cpVect n = joint.n;
88
89 // compute relative velocity
90 cpFloat vrn = normal_relative_velocity(a, b, joint.r1, joint.r2, n);
91
92 cpFloat jnMax = joint.constraint.maxForce * dt;
93
94 // compute normal impulse
95 cpFloat jn = (joint.bias - vrn) * joint.nMass;
96 cpFloat jnOld = joint.jnAcc;
97 joint.jnAcc = cpfclamp(jnOld + jn, -jnMax, jnMax);
98 jn = joint.jnAcc - jnOld;
99
100 // apply impulse
101 apply_impulses(a, b, joint.r1, joint.r2, cpvmult(n, jn));
102 }
103
104 cpFloat getImpulse(cpPinJoint* joint)
105 {
106 return cpfabs(joint.jnAcc);
107 }
108
109
110 __gshared cpConstraintClass klass;
111
112 void _initModuleCtor_cpPinJoint()
113 {
114 klass = cpConstraintClass(
115 cast(cpConstraintPreStepImpl)&preStep,
116 cast(cpConstraintApplyCachedImpulseImpl)&applyCachedImpulse,
117 cast(cpConstraintApplyImpulseImpl)&applyImpulse,
118 cast(cpConstraintGetImpulseImpl)&getImpulse,
119 );
120 }
121
122 const(cpConstraintClass *) cpPinJointGetClass()
123 {
124 return cast(cpConstraintClass*)&klass;
125 }
126
127 cpPinJoint *
128 cpPinJointAlloc()
129 {
130 return cast(cpPinJoint*)cpcalloc(1, cpPinJoint.sizeof);
131 }
132
133 cpPinJoint* cpPinJointInit(cpPinJoint* joint, cpBody* a, cpBody* b, cpVect anchr1, cpVect anchr2)
134 {
135 cpConstraintInit(cast(cpConstraint*)joint, &klass, a, b);
136
137 joint.anchr1 = anchr1;
138 joint.anchr2 = anchr2;
139
140 // STATIC_BODY_CHECK
141 cpVect p1 = (a ? cpvadd(a.p, cpvrotate(anchr1, a.rot)) : anchr1);
142 cpVect p2 = (b ? cpvadd(b.p, cpvrotate(anchr2, b.rot)) : anchr2);
143 joint.dist = cpvlength(cpvsub(p2, p1));
144
145 cpAssertWarn(joint.dist > 0.0, "You created a 0 length pin joint. A pivot joint will be much more stable.");
146
147 joint.jnAcc = 0.0f;
148
149 return joint;
150 }
151
152 cpConstraint* cpPinJointNew(cpBody* a, cpBody* b, cpVect anchr1, cpVect anchr2)
153 {
154 return cast(cpConstraint*)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2);
155 }