1 /* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
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:
10 * The above copyright notice and this permission notice shall be included in
11 * all copies or substantial portions of the Software.
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
22 #include "chipmunk/chipmunk_private.h"
25 preStep(cpGrooveJoint *joint, cpFloat dt)
27 cpBody *a = joint->constraint.a;
28 cpBody *b = joint->constraint.b;
30 // calculate endpoints in worldspace
31 cpVect ta = cpTransformPoint(a->transform, joint->grv_a);
32 cpVect tb = cpTransformPoint(a->transform, joint->grv_b);
35 cpVect n = cpTransformVect(a->transform, joint->grv_n);
36 cpFloat d = cpvdot(ta, n);
39 joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog));
41 // calculate tangential distance along the axis of r2
42 cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
43 // calculate clamping factor and r2
44 if(td <= cpvcross(ta, n)){
46 joint->r1 = cpvsub(ta, a->p);
47 } else if(td >= cpvcross(tb, n)){
49 joint->r1 = cpvsub(tb, a->p);
52 joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
55 // Calculate mass tensor
56 joint->k = k_tensor(a, b, joint->r1, joint->r2);
58 // calculate bias velocity
59 cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
60 joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
64 applyCachedImpulse(cpGrooveJoint *joint, cpFloat dt_coef)
66 cpBody *a = joint->constraint.a;
67 cpBody *b = joint->constraint.b;
69 apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef));
73 grooveConstrain(cpGrooveJoint *joint, cpVect j, cpFloat dt){
74 cpVect n = joint->grv_tn;
75 cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n);
76 return cpvclamp(jClamp, joint->constraint.maxForce*dt);
80 applyImpulse(cpGrooveJoint *joint, cpFloat dt)
82 cpBody *a = joint->constraint.a;
83 cpBody *b = joint->constraint.b;
85 cpVect r1 = joint->r1;
86 cpVect r2 = joint->r2;
89 cpVect vr = relative_velocity(a, b, r1, r2);
91 cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr));
92 cpVect jOld = joint->jAcc;
93 joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j), dt);
94 j = cpvsub(joint->jAcc, jOld);
97 apply_impulses(a, b, joint->r1, joint->r2, j);
101 getImpulse(cpGrooveJoint *joint)
103 return cpvlength(joint->jAcc);
106 static const cpConstraintClass klass = {
107 (cpConstraintPreStepImpl)preStep,
108 (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
109 (cpConstraintApplyImpulseImpl)applyImpulse,
110 (cpConstraintGetImpulseImpl)getImpulse,
114 cpGrooveJointAlloc(void)
116 return (cpGrooveJoint *)cpcalloc(1, sizeof(cpGrooveJoint));
120 cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB)
122 cpConstraintInit((cpConstraint *)joint, &klass, a, b);
124 joint->grv_a = groove_a;
125 joint->grv_b = groove_b;
126 joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a)));
127 joint->anchorB = anchorB;
129 joint->jAcc = cpvzero;
135 cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchorB)
137 return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchorB);
141 cpConstraintIsGrooveJoint(const cpConstraint *constraint)
143 return (constraint->klass == &klass);
147 cpGrooveJointGetGrooveA(const cpConstraint *constraint)
149 cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
150 return ((cpGrooveJoint *)constraint)->grv_a;
154 cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value)
156 cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
157 cpGrooveJoint *g = (cpGrooveJoint *)constraint;
160 g->grv_n = cpvperp(cpvnormalize(cpvsub(g->grv_b, value)));
162 cpConstraintActivateBodies(constraint);
166 cpGrooveJointGetGrooveB(const cpConstraint *constraint)
168 cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
169 return ((cpGrooveJoint *)constraint)->grv_b;
173 cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value)
175 cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
176 cpGrooveJoint *g = (cpGrooveJoint *)constraint;
179 g->grv_n = cpvperp(cpvnormalize(cpvsub(value, g->grv_a)));
181 cpConstraintActivateBodies(constraint);
185 cpGrooveJointGetAnchorB(const cpConstraint *constraint)
187 cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
188 return ((cpGrooveJoint *)constraint)->anchorB;
192 cpGrooveJointSetAnchorB(cpConstraint *constraint, cpVect anchorB)
194 cpAssertHard(cpConstraintIsGrooveJoint(constraint), "Constraint is not a groove joint.");
195 cpConstraintActivateBodies(constraint);
196 ((cpGrooveJoint *)constraint)->anchorB = anchorB;