dali-physics/third-party/chipmunk2d/src/cpPivotJoint.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(cpPivotJoint *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         // Calculate mass tensor
  34         joint-> k = k_tensor(a, b, joint->r1, joint->r2);
  35
  36         // calculate bias velocity
  37         cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
  38         joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
  39 }
  40
  41 static void
  42 applyCachedImpulse(cpPivotJoint *joint, cpFloat dt_coef)
  43 {
  44         cpBody *a = joint->constraint.a;
  45         cpBody *b = joint->constraint.b;
  46
  47         apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef));
  48 }
  49
  50 static void
  51 applyImpulse(cpPivotJoint *joint, cpFloat dt)
  52 {
  53         cpBody *a = joint->constraint.a;
  54         cpBody *b = joint->constraint.b;
  55
  56         cpVect r1 = joint->r1;
  57         cpVect r2 = joint->r2;
  58
  59         // compute relative velocity
  60         cpVect vr = relative_velocity(a, b, r1, r2);
  61
  62         // compute normal impulse
  63         cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr));
  64         cpVect jOld = joint->jAcc;
  65         joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->constraint.maxForce*dt);
  66         j = cpvsub(joint->jAcc, jOld);
  67
  68         // apply impulse
  69         apply_impulses(a, b, joint->r1, joint->r2, j);
  70 }
  71
  72 static cpFloat
  73 getImpulse(cpConstraint *joint)
  74 {
  75         return cpvlength(((cpPivotJoint *)joint)->jAcc);
  76 }
  77
  78 static const cpConstraintClass klass = {
  79         (cpConstraintPreStepImpl)preStep,
  80         (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
  81         (cpConstraintApplyImpulseImpl)applyImpulse,
  82         (cpConstraintGetImpulseImpl)getImpulse,
  83 };
  84
  85 cpPivotJoint *
  86 cpPivotJointAlloc(void)
  87 {
  88         return (cpPivotJoint *)cpcalloc(1, sizeof(cpPivotJoint));
  89 }
  90
  91 cpPivotJoint *
  92 cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB)
  93 {
  94         cpConstraintInit((cpConstraint *)joint, &klass, a, b);
  95
  96         joint->anchorA = anchorA;
  97         joint->anchorB = anchorB;
  98
  99         joint->jAcc = cpvzero;
 100
 101         return joint;
 102 }
 103
 104 cpConstraint *
 105 cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB)
 106 {
 107         return (cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchorA, anchorB);
 108 }
 109
 110 cpConstraint *
 111 cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot)
 112 {
 113         cpVect anchorA = (a ? cpBodyWorldToLocal(a, pivot) : pivot);
 114         cpVect anchorB = (b ? cpBodyWorldToLocal(b, pivot) : pivot);
 115         return cpPivotJointNew2(a, b, anchorA, anchorB);
 116 }
 117
 118 cpBool
 119 cpConstraintIsPivotJoint(const cpConstraint *constraint)
 120 {
 121         return (constraint->klass == &klass);
 122 }
 123
 124 cpVect
 125 cpPivotJointGetAnchorA(const cpConstraint *constraint)
 126 {
 127         cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint.");
 128         return ((cpPivotJoint *)constraint)->anchorA;
 129 }
 130
 131 void
 132 cpPivotJointSetAnchorA(cpConstraint *constraint, cpVect anchorA)
 133 {
 134         cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint.");
 135         cpConstraintActivateBodies(constraint);
 136         ((cpPivotJoint *)constraint)->anchorA = anchorA;
 137 }
 138
 139 cpVect
 140 cpPivotJointGetAnchorB(const cpConstraint *constraint)
 141 {
 142         cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint.");
 143         return ((cpPivotJoint *)constraint)->anchorB;
 144 }
 145
 146 void
 147 cpPivotJointSetAnchorB(cpConstraint *constraint, cpVect anchorB)
 148 {
 149         cpAssertHard(cpConstraintIsPivotJoint(constraint), "Constraint is not a pivot joint.");
 150         cpConstraintActivateBodies(constraint);
 151         ((cpPivotJoint *)constraint)->anchorB = anchorB;
 152 }