Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/constraints/cpSimpleMotor.d @ 6:707dd4e10c28

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
ported rest of the constraints (chipmunk 5.3.2)
author Extrawurst
date Thu, 02 Dec 2010 22:26:04 +0100
parents
children b68f10432182
comparison
equal deleted inserted replaced
5:6dd4bcf20f03 6:707dd4e10c28
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 }