dchip.cpGearJoint 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.cpGearJoint;
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 
32 //~ const cpConstraintClass* cpGearJointGetClass();
33 
34 /// @private
35 struct cpGearJoint
36 {
37     cpConstraint constraint;
38     cpFloat phase = 0, ratio = 0;
39     cpFloat ratio_inv = 0;
40 
41     cpFloat iSum = 0;
42 
43     cpFloat bias = 0;
44     cpFloat jAcc = 0;
45 }
46 
47 mixin CP_DefineConstraintProperty!("cpGearJoint", cpFloat, "phase", "Phase");
48 mixin CP_DefineConstraintGetter!("cpGearJoint", cpFloat, "ratio", "Ratio");
49 
50 void preStep(cpGearJoint* joint, cpFloat dt)
51 {
52     cpBody* a = joint.constraint.a;
53     cpBody* b = joint.constraint.b;
54 
55     // calculate moment of inertia coefficient.
56     joint.iSum = 1.0f / (a.i_inv * joint.ratio_inv + joint.ratio * b.i_inv);
57 
58     // calculate bias velocity
59     cpFloat maxBias = joint.constraint.maxBias;
60     joint.bias = cpfclamp(-bias_coef(joint.constraint.errorBias, dt) * (b.a * joint.ratio - a.a - joint.phase) / dt, -maxBias, maxBias);
61 }
62 
63 void applyCachedImpulse(cpGearJoint* joint, cpFloat dt_coef)
64 {
65     cpBody* a = joint.constraint.a;
66     cpBody* b = joint.constraint.b;
67 
68     cpFloat j = joint.jAcc * dt_coef;
69     a.w -= j * a.i_inv * joint.ratio_inv;
70     b.w += j * b.i_inv;
71 }
72 
73 void applyImpulse(cpGearJoint* joint, cpFloat dt)
74 {
75     cpBody* a = joint.constraint.a;
76     cpBody* b = joint.constraint.b;
77 
78     // compute relative rotational velocity
79     cpFloat wr = b.w * joint.ratio - a.w;
80 
81     cpFloat jMax = joint.constraint.maxForce * dt;
82 
83     // compute normal impulse
84     cpFloat j    = (joint.bias - wr) * joint.iSum;
85     cpFloat jOld = joint.jAcc;
86     joint.jAcc = cpfclamp(jOld + j, -jMax, jMax);
87     j = joint.jAcc - jOld;
88 
89     // apply impulse
90     a.w -= j * a.i_inv * joint.ratio_inv;
91     b.w += j * b.i_inv;
92 }
93 
94 cpFloat getImpulse(cpGearJoint* joint)
95 {
96     return cpfabs(joint.jAcc);
97 }
98 
99 __gshared cpConstraintClass klass;
100 
101 void _initModuleCtor_cpGearJoint()
102 {
103     klass = cpConstraintClass(
104         cast(cpConstraintPreStepImpl)&preStep,
105         cast(cpConstraintApplyCachedImpulseImpl)&applyCachedImpulse,
106         cast(cpConstraintApplyImpulseImpl)&applyImpulse,
107         cast(cpConstraintGetImpulseImpl)&getImpulse,
108     );
109 }
110 
111 const(cpConstraintClass *) cpGearJointGetClass()
112 {
113     return cast(cpConstraintClass*)&klass;
114 }
115 
116 cpGearJoint *
117 cpGearJointAlloc()
118 {
119     return cast(cpGearJoint*)cpcalloc(1, cpGearJoint.sizeof);
120 }
121 
122 cpGearJoint* cpGearJointInit(cpGearJoint* joint, cpBody* a, cpBody* b, cpFloat phase, cpFloat ratio)
123 {
124     cpConstraintInit(cast(cpConstraint*)joint, &klass, a, b);
125 
126     joint.phase     = phase;
127     joint.ratio     = ratio;
128     joint.ratio_inv = 1.0f / ratio;
129 
130     joint.jAcc = 0.0f;
131 
132     return joint;
133 }
134 
135 cpConstraint* cpGearJointNew(cpBody* a, cpBody* b, cpFloat phase, cpFloat ratio)
136 {
137     return cast(cpConstraint*)cpGearJointInit(cpGearJointAlloc(), a, b, phase, ratio);
138 }
139 
140 void cpGearJointSetRatio(cpConstraint* constraint, cpFloat value)
141 {
142     mixin(cpConstraintCheckCast("constraint", "cpGearJoint"));
143     (cast(cpGearJoint*)constraint).ratio     = value;
144     (cast(cpGearJoint*)constraint).ratio_inv = 1.0f / value;
145     cpConstraintActivateBodies(constraint);
146 }