Mercurial > projects > chipmunkd

comparison trunk/chipmunkd/constraints/cpDampedRotarySpring.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.cpDampedRotarySpring;
5
6 import chipmunkd.chipmunk;
7 import chipmunkd.constraints.util;
8
9 alias cpFloat function(cpConstraint *spring, cpFloat relativeAngle) cpDampedRotarySpringTorqueFunc;
10
11 //const cpConstraintClass *cpDampedRotarySpringGetClass();
12
13 struct cpDampedRotarySpring {
14 cpConstraint constraint;
15 cpFloat restAngle;
16 cpFloat stiffness;
17 cpFloat damping;
18 cpDampedRotarySpringTorqueFunc springTorqueFunc;
19
20 cpFloat target_wrn;
21 cpFloat w_coef;
22
23 cpFloat iSum;
24 }
25
26 //cpDampedRotarySpring *cpDampedRotarySpringAlloc(void);
27 //cpDampedRotarySpring *cpDampedRotarySpringInit(cpDampedRotarySpring *joint, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping);
28 //cpConstraint *cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping);
29 //
30 //CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, restAngle, RestAngle);
31 //CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, stiffness, Stiffness);
32 //CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, damping, Damping);
33 //CP_DefineConstraintProperty(cpDampedRotarySpring, cpDampedRotarySpringTorqueFunc, springTorqueFunc, SpringTorqueFunc);
34
35 // cpDampedRotarySpring.c ---------------------------------
36
37 static cpFloat
38 defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){
39 return (relativeAngle - spring.restAngle)*spring.stiffness;
40 }
41
42 static void
43 preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv)
44 {
45 mixin(CONSTRAINT_BEGIN!("spring", "a", "b"));
46
47 cpFloat moment = a.i_inv + b.i_inv;
48 spring.iSum = 1.0f/moment;
49
50 spring.w_coef = 1.0f - cpfexp(-spring.damping*dt*moment);
51 spring.target_wrn = 0.0f;
52
53 // apply spring torque
54 cpFloat j_spring = spring.springTorqueFunc(cast(cpConstraint *)spring, a.a - b.a)*dt;
55 a.w -= j_spring*a.i_inv;
56 b.w += j_spring*b.i_inv;
57 }
58
59 static void
60 applyImpulse(cpDampedRotarySpring *spring)
61 {
62 mixin(CONSTRAINT_BEGIN!("spring", "a", "b"));
63
64 // compute relative velocity
65 cpFloat wrn = a.w - b.w;//normal_relative_velocity(a, b, r1, r2, n) - spring.target_vrn;
66
67 // compute velocity loss from drag
68 // not 100% certain this is derived correctly, though it makes sense
69 cpFloat w_damp = wrn*spring.w_coef;
70 spring.target_wrn = wrn - w_damp;
71
72 //apply_impulses(a, b, spring.r1, spring.r2, cpvmult(spring.n, v_damp*spring.nMass));
73 cpFloat j_damp = w_damp*spring.iSum;
74 a.w -= j_damp*a.i_inv;
75 b.w += j_damp*b.i_inv;
76 }
77
78 static cpFloat
79 getImpulse(cpConstraint *constraint)
80 {
81 return 0.0f;
82 }
83
84 static /+const+/ cpConstraintClass klass = {
85 cast(cpConstraintPreStepFunction)&preStep,
86 cast(cpConstraintApplyImpulseFunction)&applyImpulse,
87 cast(cpConstraintGetImpulseFunction)&getImpulse,
88 };
89 mixin(CP_DefineClassGetter!("cpDampedRotarySpring"));
90
91 cpDampedRotarySpring *
92 cpDampedRotarySpringAlloc()
93 {
94 return cast(cpDampedRotarySpring *)cpmalloc(cpDampedRotarySpring.sizeof);
95 }
96
97 cpDampedRotarySpring *
98 cpDampedRotarySpringInit(cpDampedRotarySpring *spring, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
99 {
100 cpConstraintInit(cast(cpConstraint *)spring, &klass, a, b);
101
102 spring.restAngle = restAngle;
103 spring.stiffness = stiffness;
104 spring.damping = damping;
105 spring.springTorqueFunc = cast(cpDampedRotarySpringTorqueFunc)&defaultSpringTorque;
106
107 return spring;
108 }
109
110 cpConstraint *
111 cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
112 {
113 return cast(cpConstraint *)cpDampedRotarySpringInit(cpDampedRotarySpringAlloc(), a, b, restAngle, stiffness, damping);
114 }