|
4
|
1
|
|
|
2 // written in the D programming language
|
|
|
3
|
|
|
4 module chipmunkd.constraints.cpDampedSpring;
|
|
|
5
|
|
|
6 import chipmunkd.chipmunk;
|
|
|
7 import chipmunkd.constraints.util;
|
|
|
8
|
|
|
9 alias cpFloat function(cpConstraint *spring, cpFloat dist) cpDampedSpringForceFunc;
|
|
|
10
|
|
|
11 //const cpConstraintClass *cpDampedSpringGetClass();
|
|
|
12
|
|
|
13 struct cpDampedSpring {
|
|
|
14 cpConstraint constraint;
|
|
|
15 cpVect anchr1, anchr2;
|
|
|
16 cpFloat restLength;
|
|
|
17 cpFloat stiffness;
|
|
|
18 cpFloat damping;
|
|
|
19 cpDampedSpringForceFunc springForceFunc;
|
|
|
20
|
|
|
21 cpFloat target_vrn;
|
|
|
22 cpFloat v_coef;
|
|
|
23
|
|
|
24 cpVect r1, r2;
|
|
|
25 cpFloat nMass;
|
|
|
26 cpVect n;
|
|
|
27 }
|
|
|
28
|
|
|
29 //cpDampedSpring *cpDampedSpringAlloc(void);
|
|
|
30 //cpDampedSpring *cpDampedSpringInit(cpDampedSpring *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping);
|
|
|
31 //cpConstraint *cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping);
|
|
|
32
|
|
|
33 mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpVect", "anchr1", "Anchr1"));
|
|
|
34 mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpVect", "anchr2", "Anchr2"));
|
|
|
35 mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpFloat", "restLength", "RestLength"));
|
|
|
36 mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpFloat", "stiffness", "Stiffness"));
|
|
|
37 mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpFloat", "damping", "Damping"));
|
|
|
38 mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpDampedSpringForceFunc", "springForceFunc", "SpringForceFunc"));
|
|
|
39
|
|
|
40 static cpFloat
|
|
|
41 defaultSpringForce(cpDampedSpring *spring, cpFloat dist){
|
|
|
42 return (spring.restLength - dist)*spring.stiffness;
|
|
|
43 }
|
|
|
44
|
|
|
45 static void
|
|
|
46 preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
|
|
|
47 {
|
|
|
48 mixin(CONSTRAINT_BEGIN!("spring", "a", "b"));
|
|
|
49
|
|
|
50 spring.r1 = cpvrotate(spring.anchr1, a.rot);
|
|
|
51 spring.r2 = cpvrotate(spring.anchr2, b.rot);
|
|
|
52
|
|
|
53 cpVect delta = cpvsub(cpvadd(b.p, spring.r2), cpvadd(a.p, spring.r1));
|
|
|
54 cpFloat dist = cpvlength(delta);
|
|
|
55 spring.n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
|
|
|
56
|
|
|
57 cpFloat k = k_scalar(a, b, spring.r1, spring.r2, spring.n);
|
|
|
58 spring.nMass = 1.0f/k;
|
|
|
59
|
|
|
60 spring.target_vrn = 0.0f;
|
|
|
61 spring.v_coef = 1.0f - cpfexp(-spring.damping*dt*k);
|
|
|
62
|
|
|
63 // apply spring force
|
|
|
64 cpFloat f_spring = spring.springForceFunc(cast(cpConstraint *)spring, dist);
|
|
|
65 apply_impulses(a, b, spring.r1, spring.r2, cpvmult(spring.n, f_spring*dt));
|
|
|
66 }
|
|
|
67
|
|
|
68 static void
|
|
|
69 applyImpulse(cpDampedSpring *spring)
|
|
|
70 {
|
|
|
71 mixin(CONSTRAINT_BEGIN!("spring", "a", "b"));
|
|
|
72
|
|
|
73 cpVect n = spring.n;
|
|
|
74 cpVect r1 = spring.r1;
|
|
|
75 cpVect r2 = spring.r2;
|
|
|
76
|
|
|
77 // compute relative velocity
|
|
|
78 cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring.target_vrn;
|
|
|
79
|
|
|
80 // compute velocity loss from drag
|
|
|
81 // not 100% certain this is derived correctly, though it makes sense
|
|
|
82 cpFloat v_damp = -vrn*spring.v_coef;
|
|
|
83 spring.target_vrn = vrn + v_damp;
|
|
|
84
|
|
|
85 apply_impulses(a, b, spring.r1, spring.r2, cpvmult(spring.n, v_damp*spring.nMass));
|
|
|
86 }
|
|
|
87
|
|
|
88 static cpFloat
|
|
|
89 getImpulse(cpConstraint *constraint)
|
|
|
90 {
|
|
|
91 return 0.0f;
|
|
|
92 }
|
|
|
93
|
|
|
94 static /+const+/ cpConstraintClass klass = {
|
|
|
95 cast(cpConstraintPreStepFunction)&preStep,
|
|
|
96 cast(cpConstraintApplyImpulseFunction)&applyImpulse,
|
|
|
97 cast(cpConstraintGetImpulseFunction)&getImpulse,
|
|
|
98 };
|
|
|
99 mixin(CP_DefineClassGetter!("cpDampedSpring"));
|
|
|
100
|
|
|
101 cpDampedSpring *
|
|
|
102 cpDampedSpringAlloc()
|
|
|
103 {
|
|
|
104 return cast(cpDampedSpring *)cpmalloc(cpDampedSpring.sizeof);
|
|
|
105 }
|
|
|
106
|
|
|
107 cpDampedSpring *
|
|
|
108 cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping)
|
|
|
109 {
|
|
|
110 cpConstraintInit(cast(cpConstraint *)spring, cpDampedSpringGetClass(), a, b);
|
|
|
111
|
|
|
112 spring.anchr1 = anchr1;
|
|
|
113 spring.anchr2 = anchr2;
|
|
|
114
|
|
|
115 spring.restLength = restLength;
|
|
|
116 spring.stiffness = stiffness;
|
|
|
117 spring.damping = damping;
|
|
|
118 spring.springForceFunc = cast(cpDampedSpringForceFunc)&defaultSpringForce;
|
|
|
119
|
|
|
120 return spring;
|
|
|
121 }
|
|
|
122
|
|
|
123 cpConstraint *
|
|
|
124 cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping)
|
|
|
125 {
|
|
|
126 return cast(cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchr1, anchr2, restLength, stiffness, damping);
|
|
|
127 }
|
