physics library version update
[spacedolphin:spacedolphin.git] / lib / chipmunk / src / constraints / cpGrooveJoint.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(cpGrooveJoint *joint, cpFloat dt)
27 {
28         cpBody *a = joint->constraint.a;
29         cpBody *b = joint->constraint.b;
30         
31         // calculate endpoints in worldspace
32         cpVect ta = cpBodyLocal2World(a, joint->grv_a);
33         cpVect tb = cpBodyLocal2World(a, joint->grv_b);
34
35         // calculate axis
36         cpVect n = cpvrotate(joint->grv_n, a->rot);
37         cpFloat d = cpvdot(ta, n);
38         
39         joint->grv_tn = n;
40         joint->r2 = cpvrotate(joint->anchr2, b->rot);
41         
42         // calculate tangential distance along the axis of r2
43         cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
44         // calculate clamping factor and r2
45         if(td <= cpvcross(ta, n)){
46                 joint->clamp = 1.0f;
47                 joint->r1 = cpvsub(ta, a->p);
48         } else if(td >= cpvcross(tb, n)){
49                 joint->clamp = -1.0f;
50                 joint->r1 = cpvsub(tb, a->p);
51         } else {
52                 joint->clamp = 0.0f;
53                 joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
54         }
55         
56         // Calculate mass tensor
57         joint->k = k_tensor(a, b, joint->r1, joint->r2);
58         
59         // calculate bias velocity
60         cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
61         joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
62 }
63
64 static void
65 applyCachedImpulse(cpGrooveJoint *joint, cpFloat dt_coef)
66 {
67         cpBody *a = joint->constraint.a;
68         cpBody *b = joint->constraint.b;
69                 
70         apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef));
71 }
72
73 static inline cpVect
74 grooveConstrain(cpGrooveJoint *joint, cpVect j, cpFloat dt){
75         cpVect n = joint->grv_tn;
76         cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n);
77         return cpvclamp(jClamp, joint->constraint.maxForce*dt);
78 }
79
80 static void
81 applyImpulse(cpGrooveJoint *joint, cpFloat dt)
82 {
83         cpBody *a = joint->constraint.a;
84         cpBody *b = joint->constraint.b;
85         
86         cpVect r1 = joint->r1;
87         cpVect r2 = joint->r2;
88         
89         // compute impulse
90         cpVect vr = relative_velocity(a, b, r1, r2);
91
92         cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr));
93         cpVect jOld = joint->jAcc;
94         joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j), dt);
95         j = cpvsub(joint->jAcc, jOld);
96         
97         // apply impulse
98         apply_impulses(a, b, joint->r1, joint->r2, j);
99 }
100
101 static cpFloat
102 getImpulse(cpGrooveJoint *joint)
103 {
104         return cpvlength(joint->jAcc);
105 }
106
107 static const cpConstraintClass klass = {
108         (cpConstraintPreStepImpl)preStep,
109         (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
110         (cpConstraintApplyImpulseImpl)applyImpulse,
111         (cpConstraintGetImpulseImpl)getImpulse,
112 };
113 CP_DefineClassGetter(cpGrooveJoint)
114
115 cpGrooveJoint *
116 cpGrooveJointAlloc(void)
117 {
118         return (cpGrooveJoint *)cpcalloc(1, sizeof(cpGrooveJoint));
119 }
120
121 cpGrooveJoint *
122 cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
123 {
124         cpConstraintInit((cpConstraint *)joint, &klass, a, b);
125         
126         joint->grv_a = groove_a;
127         joint->grv_b = groove_b;
128         joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a)));
129         joint->anchr2 = anchr2;
130         
131         joint->jAcc = cpvzero;
132         
133         return joint;
134 }
135
136 cpConstraint *
137 cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
138 {
139         return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2);
140 }
141
142 void
143 cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value)
144 {
145         cpGrooveJoint *g = (cpGrooveJoint *)constraint;
146         cpConstraintCheckCast(constraint, cpGrooveJoint);
147         
148         g->grv_a = value;
149         g->grv_n = cpvperp(cpvnormalize(cpvsub(g->grv_b, value)));
150         
151         cpConstraintActivateBodies(constraint);
152 }
153
154 void
155 cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value)
156 {
157         cpGrooveJoint *g = (cpGrooveJoint *)constraint;
158         cpConstraintCheckCast(constraint, cpGrooveJoint);
159         
160         g->grv_b = value;
161         g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a)));
162         
163         cpConstraintActivateBodies(constraint);
164 }
165