|
6
|
1
|
|
|
2 // written in the D programming language
|
|
|
3
|
|
|
4 module chipmunkd.constraints.cpSimpleMotor;
|
|
|
5
|
|
|
6 import chipmunkd.chipmunk;
|
|
|
7 import chipmunkd.constraints.util;
|
|
|
8
|
|
|
9 //const cpConstraintClass *cpSimpleMotorGetClass();
|
|
|
10
|
|
|
11 struct cpSimpleMotor {
|
|
|
12 cpConstraint constraint;
|
|
|
13 cpFloat rate;
|
|
|
14
|
|
|
15 cpFloat iSum;
|
|
|
16
|
|
|
17 cpFloat jAcc, jMax;
|
|
|
18 }
|
|
|
19
|
|
|
20 //cpSimpleMotor *cpSimpleMotorAlloc(void);
|
|
|
21 //cpSimpleMotor *cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate);
|
|
|
22 //cpConstraint *cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate);
|
|
|
23 //
|
|
|
24 //CP_DefineConstraintProperty(cpSimpleMotor, cpFloat, rate, Rate);
|
|
|
25
|
|
|
26 // cpRotaryLimitJoint.c ---------------------------------
|
|
|
27
|
|
|
28 static void
|
|
|
29 preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv)
|
|
|
30 {
|
|
|
31 mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
|
|
|
32
|
|
|
33 // calculate moment of inertia coefficient.
|
|
|
34 joint.iSum = 1.0f/(a.i_inv + b.i_inv);
|
|
|
35
|
|
|
36 // compute max impulse
|
|
|
37 joint.jMax = mixin(J_MAX!("joint", "dt"));
|
|
|
38
|
|
|
39 // apply joint torque
|
|
|
40 a.w -= joint.jAcc*a.i_inv;
|
|
|
41 b.w += joint.jAcc*b.i_inv;
|
|
|
42 }
|
|
|
43
|
|
|
44 static void
|
|
|
45 applyImpulse(cpSimpleMotor *joint)
|
|
|
46 {
|
|
|
47 mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
|
|
|
48
|
|
|
49 // compute relative rotational velocity
|
|
|
50 cpFloat wr = b.w - a.w + joint.rate;
|
|
|
51
|
|
|
52 // compute normal impulse
|
|
|
53 cpFloat j = -wr*joint.iSum;
|
|
|
54 cpFloat jOld = joint.jAcc;
|
|
|
55 joint.jAcc = cpfclamp(jOld + j, -joint.jMax, joint.jMax);
|
|
|
56 j = joint.jAcc - jOld;
|
|
|
57
|
|
|
58 // apply impulse
|
|
|
59 a.w -= j*a.i_inv;
|
|
|
60 b.w += j*b.i_inv;
|
|
|
61 }
|
|
|
62
|
|
|
63 static cpFloat
|
|
|
64 getImpulse(cpSimpleMotor *joint)
|
|
|
65 {
|
|
|
66 return cpfabs(joint.jAcc);
|
|
|
67 }
|
|
|
68
|
|
|
69 static /+const+/ cpConstraintClass klass = {
|
|
|
70 cast(cpConstraintPreStepFunction)&preStep,
|
|
|
71 cast(cpConstraintApplyImpulseFunction)&applyImpulse,
|
|
|
72 cast(cpConstraintGetImpulseFunction)&getImpulse,
|
|
|
73 };
|
|
|
74 mixin(CP_DefineClassGetter!("cpSimpleMotor"));
|
|
|
75
|
|
|
76 cpSimpleMotor *
|
|
|
77 cpSimpleMotorAlloc()
|
|
|
78 {
|
|
|
79 return cast(cpSimpleMotor *)cpmalloc(cpSimpleMotor.sizeof);
|
|
|
80 }
|
|
|
81
|
|
|
82 cpSimpleMotor *
|
|
|
83 cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate)
|
|
|
84 {
|
|
|
85 cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
|
|
|
86
|
|
|
87 joint.rate = rate;
|
|
|
88
|
|
|
89 joint.jAcc = 0.0f;
|
|
|
90
|
|
|
91 return joint;
|
|
|
92 }
|
|
|
93
|
|
|
94 cpConstraint *
|
|
|
95 cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate)
|
|
|
96 {
|
|
|
97 return cast(cpConstraint *)cpSimpleMotorInit(cpSimpleMotorAlloc(), a, b, rate);
|
|
|
98 }
|
