physics library version update
[spacedolphin:spacedolphin.git] / lib / chipmunk / src / constraints / cpDampedRotarySpring.c
1 /* Copyright (c) 2007 Scott Lembcke
2  * 
3  * Permission is hereby granted, free of charge, to any person obtaining a copy
4  * of this software and associated documentation files (the "Software"), to deal
5  * in the Software without restriction, including without limitation the rights
6  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7  * copies of the Software, and to permit persons to whom the Software is
8  * furnished to do so, subject to the following conditions:
9  * 
10  * The above copyright notice and this permission notice shall be included in
11  * all copies or substantial portions of the Software.
12  * 
13  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19  * SOFTWARE.
20  */
21
22 #include "chipmunk_private.h"
23 #include "constraints/util.h"
24
25 static cpFloat
26 defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){
27         return (relativeAngle - spring->restAngle)*spring->stiffness;
28 }
29
30 static void
31 preStep(cpDampedRotarySpring *spring, cpFloat dt)
32 {
33         cpBody *a = spring->constraint.a;
34         cpBody *b = spring->constraint.b;
35         
36         cpFloat moment = a->i_inv + b->i_inv;
37         cpAssertSoft(moment != 0.0, "Unsolvable spring.");
38         spring->iSum = 1.0f/moment;
39
40         spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment);
41         spring->target_wrn = 0.0f;
42
43         // apply spring torque
44         cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt;
45         spring->jAcc = j_spring;
46         
47         a->w -= j_spring*a->i_inv;
48         b->w += j_spring*b->i_inv;
49 }
50
51 static void applyCachedImpulse(cpDampedRotarySpring *spring, cpFloat dt_coef){}
52
53 static void
54 applyImpulse(cpDampedRotarySpring *spring, cpFloat dt)
55 {
56         cpBody *a = spring->constraint.a;
57         cpBody *b = spring->constraint.b;
58         
59         // compute relative velocity
60         cpFloat wrn = a->w - b->w;//normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
61         
62         // compute velocity loss from drag
63         // not 100% certain this is derived correctly, though it makes sense
64         cpFloat w_damp = (spring->target_wrn - wrn)*spring->w_coef;
65         spring->target_wrn = wrn + w_damp;
66         
67         //apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
68         cpFloat j_damp = w_damp*spring->iSum;
69         spring->jAcc += j_damp;
70         
71         a->w += j_damp*a->i_inv;
72         b->w -= j_damp*b->i_inv;
73 }
74
75 static cpFloat
76 getImpulse(cpDampedRotarySpring *spring)
77 {
78         return spring->jAcc;
79 }
80
81 static const cpConstraintClass klass = {
82         (cpConstraintPreStepImpl)preStep,
83         (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
84         (cpConstraintApplyImpulseImpl)applyImpulse,
85         (cpConstraintGetImpulseImpl)getImpulse,
86 };
87 CP_DefineClassGetter(cpDampedRotarySpring)
88
89 cpDampedRotarySpring *
90 cpDampedRotarySpringAlloc(void)
91 {
92         return (cpDampedRotarySpring *)cpcalloc(1, sizeof(cpDampedRotarySpring));
93 }
94
95 cpDampedRotarySpring *
96 cpDampedRotarySpringInit(cpDampedRotarySpring *spring, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
97 {
98         cpConstraintInit((cpConstraint *)spring, &klass, a, b);
99         
100         spring->restAngle = restAngle;
101         spring->stiffness = stiffness;
102         spring->damping = damping;
103         spring->springTorqueFunc = (cpDampedRotarySpringTorqueFunc)defaultSpringTorque;
104         
105         spring->jAcc = 0.0f;
106         
107         return spring;
108 }
109
110 cpConstraint *
111 cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
112 {
113         return (cpConstraint *)cpDampedRotarySpringInit(cpDampedRotarySpringAlloc(), a, b, restAngle, stiffness, damping);
114 }