dchip.cpPinJoint source code

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 }