dali-physics/third-party/chipmunk2d/src/cpPinJoint.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(cpPinJoint *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         joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
  36
  37         // calculate mass normal
  38         joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
  39
  40         // calculate bias velocity
  41         cpFloat maxBias = joint->constraint.maxBias;
  42         joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(dist - joint->dist)/dt, -maxBias, maxBias);
  43 }
  44
  45 static void
  46 applyCachedImpulse(cpPinJoint *joint, cpFloat dt_coef)
  47 {
  48         cpBody *a = joint->constraint.a;
  49         cpBody *b = joint->constraint.b;
  50
  51         cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef);
  52         apply_impulses(a, b, joint->r1, joint->r2, j);
  53 }
  54
  55 static void
  56 applyImpulse(cpPinJoint *joint, cpFloat dt)
  57 {
  58         cpBody *a = joint->constraint.a;
  59         cpBody *b = joint->constraint.b;
  60         cpVect n = joint->n;
  61
  62         // compute relative velocity
  63         cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n);
  64
  65         cpFloat jnMax = joint->constraint.maxForce*dt;
  66
  67         // compute normal impulse
  68         cpFloat jn = (joint->bias - vrn)*joint->nMass;
  69         cpFloat jnOld = joint->jnAcc;
  70         joint->jnAcc = cpfclamp(jnOld + jn, -jnMax, jnMax);
  71         jn = joint->jnAcc - jnOld;
  72
  73         // apply impulse
  74         apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
  75 }
  76
  77 static cpFloat
  78 getImpulse(cpPinJoint *joint)
  79 {
  80         return cpfabs(joint->jnAcc);
  81 }
  82
  83 static const cpConstraintClass klass = {
  84         (cpConstraintPreStepImpl)preStep,
  85         (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
  86         (cpConstraintApplyImpulseImpl)applyImpulse,
  87         (cpConstraintGetImpulseImpl)getImpulse,
  88 };
  89
  90
  91 cpPinJoint *
  92 cpPinJointAlloc(void)
  93 {
  94         return (cpPinJoint *)cpcalloc(1, sizeof(cpPinJoint));
  95 }
  96
  97 cpPinJoint *
  98 cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB)
  99 {
 100         cpConstraintInit((cpConstraint *)joint, &klass, a, b);
 101
 102         joint->anchorA = anchorA;
 103         joint->anchorB = anchorB;
 104
 105         // STATIC_BODY_CHECK
 106         cpVect p1 = (a ? cpTransformPoint(a->transform, anchorA) : anchorA);
 107         cpVect p2 = (b ? cpTransformPoint(b->transform, anchorB) : anchorB);
 108         joint->dist = cpvlength(cpvsub(p2, p1));
 109
 110         cpAssertWarn(joint->dist > 0.0, "You created a 0 length pin joint. A pivot joint will be much more stable.");
 111
 112         joint->jnAcc = 0.0f;
 113
 114         return joint;
 115 }
 116
 117 cpConstraint *
 118 cpPinJointNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB)
 119 {
 120         return (cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchorA, anchorB);
 121 }
 122
 123 cpBool
 124 cpConstraintIsPinJoint(const cpConstraint *constraint)
 125 {
 126         return (constraint->klass == &klass);
 127 }
 128
 129 cpVect
 130 cpPinJointGetAnchorA(const cpConstraint *constraint)
 131 {
 132         cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint.");
 133         return ((cpPinJoint *)constraint)->anchorA;
 134 }
 135
 136 void
 137 cpPinJointSetAnchorA(cpConstraint *constraint, cpVect anchorA)
 138 {
 139         cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint.");
 140         cpConstraintActivateBodies(constraint);
 141         ((cpPinJoint *)constraint)->anchorA = anchorA;
 142 }
 143
 144 cpVect
 145 cpPinJointGetAnchorB(const cpConstraint *constraint)
 146 {
 147         cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint.");
 148         return ((cpPinJoint *)constraint)->anchorB;
 149 }
 150
 151 void
 152 cpPinJointSetAnchorB(cpConstraint *constraint, cpVect anchorB)
 153 {
 154         cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint.");
 155         cpConstraintActivateBodies(constraint);
 156         ((cpPinJoint *)constraint)->anchorB = anchorB;
 157 }
 158
 159 cpFloat
 160 cpPinJointGetDist(const cpConstraint *constraint)
 161 {
 162         cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint.");
 163         return ((cpPinJoint *)constraint)->dist;
 164 }
 165
 166 void
 167 cpPinJointSetDist(cpConstraint *constraint, cpFloat dist)
 168 {
 169         cpAssertHard(cpConstraintIsPinJoint(constraint), "Constraint is not a pin joint.");
 170         cpConstraintActivateBodies(constraint);
 171         ((cpPinJoint *)constraint)->dist = dist;
 172 }