physics library version update
[spacedolphin:spacedolphin.git] / lib / chipmunk / src / constraints / cpRotaryLimitJoint.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 void
26 preStep(cpRotaryLimitJoint *joint, cpFloat dt)
27 {
28         cpBody *a = joint->constraint.a;
29         cpBody *b = joint->constraint.b;
30         
31         cpFloat dist = b->a - a->a;
32         cpFloat pdist = 0.0f;
33         if(dist > joint->max) {
34                 pdist = joint->max - dist;
35         } else if(dist < joint->min) {
36                 pdist = joint->min - dist;
37         }
38         
39         // calculate moment of inertia coefficient.
40         joint->iSum = 1.0f/(1.0f/a->i + 1.0f/b->i);
41         
42         // calculate bias velocity
43         cpFloat maxBias = joint->constraint.maxBias;
44         joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias);
45
46         // If the bias is 0, the joint is not at a limit. Reset the impulse.
47         if(!joint->bias) joint->jAcc = 0.0f;
48 }
49
50 static void
51 applyCachedImpulse(cpRotaryLimitJoint *joint, cpFloat dt_coef)
52 {
53         cpBody *a = joint->constraint.a;
54         cpBody *b = joint->constraint.b;
55         
56         cpFloat j = joint->jAcc*dt_coef;
57         a->w -= j*a->i_inv;
58         b->w += j*b->i_inv;
59 }
60
61 static void
62 applyImpulse(cpRotaryLimitJoint *joint, cpFloat dt)
63 {
64         if(!joint->bias) return; // early exit
65
66         cpBody *a = joint->constraint.a;
67         cpBody *b = joint->constraint.b;
68         
69         // compute relative rotational velocity
70         cpFloat wr = b->w - a->w;
71         
72         cpFloat jMax = joint->constraint.maxForce*dt;
73         
74         // compute normal impulse       
75         cpFloat j = -(joint->bias + wr)*joint->iSum;
76         cpFloat jOld = joint->jAcc;
77         if(joint->bias < 0.0f){
78                 joint->jAcc = cpfclamp(jOld + j, 0.0f, jMax);
79         } else {
80                 joint->jAcc = cpfclamp(jOld + j, -jMax, 0.0f);
81         }
82         j = joint->jAcc - jOld;
83         
84         // apply impulse
85         a->w -= j*a->i_inv;
86         b->w += j*b->i_inv;
87 }
88
89 static cpFloat
90 getImpulse(cpRotaryLimitJoint *joint)
91 {
92         return cpfabs(joint->jAcc);
93 }
94
95 static const cpConstraintClass klass = {
96         (cpConstraintPreStepImpl)preStep,
97         (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
98         (cpConstraintApplyImpulseImpl)applyImpulse,
99         (cpConstraintGetImpulseImpl)getImpulse,
100 };
101 CP_DefineClassGetter(cpRotaryLimitJoint)
102
103 cpRotaryLimitJoint *
104 cpRotaryLimitJointAlloc(void)
105 {
106         return (cpRotaryLimitJoint *)cpcalloc(1, sizeof(cpRotaryLimitJoint));
107 }
108
109 cpRotaryLimitJoint *
110 cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max)
111 {
112         cpConstraintInit((cpConstraint *)joint, &klass, a, b);
113         
114         joint->min = min;
115         joint->max  = max;
116         
117         joint->jAcc = 0.0f;
118         
119         return joint;
120 }
121
122 cpConstraint *
123 cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max)
124 {
125         return (cpConstraint *)cpRotaryLimitJointInit(cpRotaryLimitJointAlloc(), a, b, min, max);
126 }