dali-physics/third-party/chipmunk2d/src/cpSlideJoint.c

   1 /* Copyright (c) 2013 Scott Lembcke and Howling Moon Software
   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/chipmunk_private.h"
  23
  24 static void
  25 preStep(cpSlideJoint *joint, cpFloat dt)
  26 {
  27         cpBody *a = joint->constraint.a;
  28         cpBody *b = joint->constraint.b;
  29
  30         joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog));
  31         joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog));
  32
  33         cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
  34         cpFloat dist = cpvlength(delta);
  35         cpFloat pdist = 0.0f;
  36         if(dist > joint->max) {
  37                 pdist = dist - joint->max;
  38                 joint->n = cpvnormalize(delta);
  39         } else if(dist < joint->min) {
  40                 pdist = joint->min - dist;
  41                 joint->n = cpvneg(cpvnormalize(delta));
  42         } else {
  43                 joint->n = cpvzero;
  44                 joint->jnAcc = 0.0f;
  45         }
  46
  47         // calculate mass normal
  48         joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
  49
  50         // calculate bias velocity
  51         cpFloat maxBias = joint->constraint.maxBias;
  52         joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*pdist/dt, -maxBias, maxBias);
  53 }
  54
  55 static void
  56 applyCachedImpulse(cpSlideJoint *joint, cpFloat dt_coef)
  57 {
  58         cpBody *a = joint->constraint.a;
  59         cpBody *b = joint->constraint.b;
  60
  61         cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef);
  62         apply_impulses(a, b, joint->r1, joint->r2, j);
  63 }
  64
  65 static void
  66 applyImpulse(cpSlideJoint *joint, cpFloat dt)
  67 {
  68         if(cpveql(joint->n, cpvzero)) return;  // early exit
  69
  70         cpBody *a = joint->constraint.a;
  71         cpBody *b = joint->constraint.b;
  72
  73         cpVect n = joint->n;
  74         cpVect r1 = joint->r1;
  75         cpVect r2 = joint->r2;
  76
  77         // compute relative velocity
  78         cpVect vr = relative_velocity(a, b, r1, r2);
  79         cpFloat vrn = cpvdot(vr, n);
  80
  81         // compute normal impulse
  82         cpFloat jn = (joint->bias - vrn)*joint->nMass;
  83         cpFloat jnOld = joint->jnAcc;
  84         joint->jnAcc = cpfclamp(jnOld + jn, -joint->constraint.maxForce*dt, 0.0f);
  85         jn = joint->jnAcc - jnOld;
  86
  87         // apply impulse
  88         apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
  89 }
  90
  91 static cpFloat
  92 getImpulse(cpConstraint *joint)
  93 {
  94         return cpfabs(((cpSlideJoint *)joint)->jnAcc);
  95 }
  96
  97 static const cpConstraintClass klass = {
  98         (cpConstraintPreStepImpl)preStep,
  99         (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
 100         (cpConstraintApplyImpulseImpl)applyImpulse,
 101         (cpConstraintGetImpulseImpl)getImpulse,
 102 };
 103
 104 cpSlideJoint *
 105 cpSlideJointAlloc(void)
 106 {
 107         return (cpSlideJoint *)cpcalloc(1, sizeof(cpSlideJoint));
 108 }
 109
 110 cpSlideJoint *
 111 cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat min, cpFloat max)
 112 {
 113         cpConstraintInit((cpConstraint *)joint, &klass, a, b);
 114
 115         joint->anchorA = anchorA;
 116         joint->anchorB = anchorB;
 117         joint->min = min;
 118         joint->max = max;
 119
 120         joint->jnAcc = 0.0f;
 121
 122         return joint;
 123 }
 124
 125 cpConstraint *
 126 cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat min, cpFloat max)
 127 {
 128         return (cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchorA, anchorB, min, max);
 129 }
 130
 131 cpBool
 132 cpConstraintIsSlideJoint(const cpConstraint *constraint)
 133 {
 134         return (constraint->klass == &klass);
 135 }
 136
 137 cpVect
 138 cpSlideJointGetAnchorA(const cpConstraint *constraint)
 139 {
 140         cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
 141         return ((cpSlideJoint *)constraint)->anchorA;
 142 }
 143
 144 void
 145 cpSlideJointSetAnchorA(cpConstraint *constraint, cpVect anchorA)
 146 {
 147         cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
 148         cpConstraintActivateBodies(constraint);
 149         ((cpSlideJoint *)constraint)->anchorA = anchorA;
 150 }
 151
 152 cpVect
 153 cpSlideJointGetAnchorB(const cpConstraint *constraint)
 154 {
 155         cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
 156         return ((cpSlideJoint *)constraint)->anchorB;
 157 }
 158
 159 void
 160 cpSlideJointSetAnchorB(cpConstraint *constraint, cpVect anchorB)
 161 {
 162         cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
 163         cpConstraintActivateBodies(constraint);
 164         ((cpSlideJoint *)constraint)->anchorB = anchorB;
 165 }
 166
 167 cpFloat
 168 cpSlideJointGetMin(const cpConstraint *constraint)
 169 {
 170         cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
 171         return ((cpSlideJoint *)constraint)->min;
 172 }
 173
 174 void
 175 cpSlideJointSetMin(cpConstraint *constraint, cpFloat min)
 176 {
 177         cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
 178         cpConstraintActivateBodies(constraint);
 179         ((cpSlideJoint *)constraint)->min = min;
 180 }
 181
 182 cpFloat
 183 cpSlideJointGetMax(const cpConstraint *constraint)
 184 {
 185         cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
 186         return ((cpSlideJoint *)constraint)->max;
 187 }
 188
 189 void
 190 cpSlideJointSetMax(cpConstraint *constraint, cpFloat max)
 191 {
 192         cpAssertHard(cpConstraintIsSlideJoint(constraint), "Constraint is not a slide joint.");
 193         cpConstraintActivateBodies(constraint);
 194         ((cpSlideJoint *)constraint)->max = max;
 195 }