dchip.cpSimpleMotor 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.cpSimpleMotor;
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* cpSimpleMotorGetClass();
34 
35 /// @private
36 struct cpSimpleMotor
37 {
38     cpConstraint constraint;
39     cpFloat rate = 0;
40 
41     cpFloat iSum = 0;
42 
43     cpFloat jAcc = 0;
44 }
45 
46 mixin CP_DefineConstraintProperty!("cpSimpleMotor", cpFloat, "rate", "Rate");
47 
48 void preStep(cpSimpleMotor* joint, cpFloat dt)
49 {
50     cpBody* a = joint.constraint.a;
51     cpBody* b = joint.constraint.b;
52 
53     // calculate moment of inertia coefficient.
54     joint.iSum = 1.0f / (a.i_inv + b.i_inv);
55 }
56 
57 void applyCachedImpulse(cpSimpleMotor* joint, cpFloat dt_coef)
58 {
59     cpBody* a = joint.constraint.a;
60     cpBody* b = joint.constraint.b;
61 
62     cpFloat j = joint.jAcc * dt_coef;
63     a.w -= j * a.i_inv;
64     b.w += j * b.i_inv;
65 }
66 
67 void applyImpulse(cpSimpleMotor* joint, cpFloat dt)
68 {
69     cpBody* a = joint.constraint.a;
70     cpBody* b = joint.constraint.b;
71 
72     // compute relative rotational velocity
73     cpFloat wr = b.w - a.w + joint.rate;
74 
75     cpFloat jMax = joint.constraint.maxForce * dt;
76 
77     // compute normal impulse
78     cpFloat j    = -wr * joint.iSum;
79     cpFloat jOld = joint.jAcc;
80     joint.jAcc = cpfclamp(jOld + j, -jMax, jMax);
81     j = joint.jAcc - jOld;
82 
83     // apply impulse
84     a.w -= j * a.i_inv;
85     b.w += j * b.i_inv;
86 }
87 
88 cpFloat getImpulse(cpSimpleMotor* joint)
89 {
90     return cpfabs(joint.jAcc);
91 }
92 
93 __gshared cpConstraintClass klass;
94 
95 void _initModuleCtor_cpSimpleMotor()
96 {
97     klass = cpConstraintClass(
98         cast(cpConstraintPreStepImpl)&preStep,
99         cast(cpConstraintApplyCachedImpulseImpl)&applyCachedImpulse,
100         cast(cpConstraintApplyImpulseImpl)&applyImpulse,
101         cast(cpConstraintGetImpulseImpl)&getImpulse,
102     );
103 }
104 
105 const(cpConstraintClass *) cpSimpleMotorGetClass()
106 {
107     return cast(cpConstraintClass*)&klass;
108 }
109 
110 cpSimpleMotor *
111 cpSimpleMotorAlloc()
112 {
113     return cast(cpSimpleMotor*)cpcalloc(1, cpSimpleMotor.sizeof);
114 }
115 
116 cpSimpleMotor* cpSimpleMotorInit(cpSimpleMotor* joint, cpBody* a, cpBody* b, cpFloat rate)
117 {
118     cpConstraintInit(cast(cpConstraint*)joint, &klass, a, b);
119 
120     joint.rate = rate;
121 
122     joint.jAcc = 0.0f;
123 
124     return joint;
125 }
126 
127 cpConstraint* cpSimpleMotorNew(cpBody* a, cpBody* b, cpFloat rate)
128 {
129     return cast(cpConstraint*)cpSimpleMotorInit(cpSimpleMotorAlloc(), a, b, rate);
130 }