|
6
|
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 }
|
