dali-physics/third-party/chipmunk2d/src/cpDampedSpring.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 cpFloat
  25 defaultSpringForce(cpDampedSpring *spring, cpFloat dist){
  26         return (spring->restLength - dist)*spring->stiffness;
  27 }
  28
  29 static void
  30 preStep(cpDampedSpring *spring, cpFloat dt)
  31 {
  32         cpBody *a = spring->constraint.a;
  33         cpBody *b = spring->constraint.b;
  34
  35         spring->r1 = cpTransformVect(a->transform, cpvsub(spring->anchorA, a->cog));
  36         spring->r2 = cpTransformVect(b->transform, cpvsub(spring->anchorB, b->cog));
  37
  38         cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
  39         cpFloat dist = cpvlength(delta);
  40         spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
  41
  42         cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n);
  43         cpAssertSoft(k != 0.0, "Unsolvable spring.");
  44         spring->nMass = 1.0f/k;
  45
  46         spring->target_vrn = 0.0f;
  47         spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k);
  48
  49         // apply spring force
  50         cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
  51         cpFloat j_spring = spring->jAcc = f_spring*dt;
  52         apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, j_spring));
  53 }
  54
  55 static void applyCachedImpulse(cpDampedSpring *spring, cpFloat dt_coef){}
  56
  57 static void
  58 applyImpulse(cpDampedSpring *spring, cpFloat dt)
  59 {
  60         cpBody *a = spring->constraint.a;
  61         cpBody *b = spring->constraint.b;
  62
  63         cpVect n = spring->n;
  64         cpVect r1 = spring->r1;
  65         cpVect r2 = spring->r2;
  66
  67         // compute relative velocity
  68         cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n);
  69
  70         // compute velocity loss from drag
  71         cpFloat v_damp = (spring->target_vrn - vrn)*spring->v_coef;
  72         spring->target_vrn = vrn + v_damp;
  73
  74         cpFloat j_damp = v_damp*spring->nMass;
  75         spring->jAcc += j_damp;
  76         apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, j_damp));
  77 }
  78
  79 static cpFloat
  80 getImpulse(cpDampedSpring *spring)
  81 {
  82         return spring->jAcc;
  83 }
  84
  85 static const cpConstraintClass klass = {
  86         (cpConstraintPreStepImpl)preStep,
  87         (cpConstraintApplyCachedImpulseImpl)applyCachedImpulse,
  88         (cpConstraintApplyImpulseImpl)applyImpulse,
  89         (cpConstraintGetImpulseImpl)getImpulse,
  90 };
  91
  92 cpDampedSpring *
  93 cpDampedSpringAlloc(void)
  94 {
  95         return (cpDampedSpring *)cpcalloc(1, sizeof(cpDampedSpring));
  96 }
  97
  98 cpDampedSpring *
  99 cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat restLength, cpFloat stiffness, cpFloat damping)
 100 {
 101         cpConstraintInit((cpConstraint *)spring, &klass, a, b);
 102
 103         spring->anchorA = anchorA;
 104         spring->anchorB = anchorB;
 105
 106         spring->restLength = restLength;
 107         spring->stiffness = stiffness;
 108         spring->damping = damping;
 109         spring->springForceFunc = (cpDampedSpringForceFunc)defaultSpringForce;
 110
 111         spring->jAcc = 0.0f;
 112
 113         return spring;
 114 }
 115
 116 cpConstraint *
 117 cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchorA, cpVect anchorB, cpFloat restLength, cpFloat stiffness, cpFloat damping)
 118 {
 119         return (cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchorA, anchorB, restLength, stiffness, damping);
 120 }
 121
 122 cpBool
 123 cpConstraintIsDampedSpring(const cpConstraint *constraint)
 124 {
 125         return (constraint->klass == &klass);
 126 }
 127
 128 cpVect
 129 cpDampedSpringGetAnchorA(const cpConstraint *constraint)
 130 {
 131         cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
 132         return ((cpDampedSpring *)constraint)->anchorA;
 133 }
 134
 135 void
 136 cpDampedSpringSetAnchorA(cpConstraint *constraint, cpVect anchorA)
 137 {
 138         cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
 139         cpConstraintActivateBodies(constraint);
 140         ((cpDampedSpring *)constraint)->anchorA = anchorA;
 141 }
 142
 143 cpVect
 144 cpDampedSpringGetAnchorB(const cpConstraint *constraint)
 145 {
 146         cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
 147         return ((cpDampedSpring *)constraint)->anchorB;
 148 }
 149
 150 void
 151 cpDampedSpringSetAnchorB(cpConstraint *constraint, cpVect anchorB)
 152 {
 153         cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
 154         cpConstraintActivateBodies(constraint);
 155         ((cpDampedSpring *)constraint)->anchorB = anchorB;
 156 }
 157
 158 cpFloat
 159 cpDampedSpringGetRestLength(const cpConstraint *constraint)
 160 {
 161         cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
 162         return ((cpDampedSpring *)constraint)->restLength;
 163 }
 164
 165 void
 166 cpDampedSpringSetRestLength(cpConstraint *constraint, cpFloat restLength)
 167 {
 168         cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
 169         cpConstraintActivateBodies(constraint);
 170         ((cpDampedSpring *)constraint)->restLength = restLength;
 171 }
 172
 173 cpFloat
 174 cpDampedSpringGetStiffness(const cpConstraint *constraint)
 175 {
 176         cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
 177         return ((cpDampedSpring *)constraint)->stiffness;
 178 }
 179
 180 void
 181 cpDampedSpringSetStiffness(cpConstraint *constraint, cpFloat stiffness)
 182 {
 183         cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
 184         cpConstraintActivateBodies(constraint);
 185         ((cpDampedSpring *)constraint)->stiffness = stiffness;
 186 }
 187
 188 cpFloat
 189 cpDampedSpringGetDamping(const cpConstraint *constraint)
 190 {
 191         cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
 192         return ((cpDampedSpring *)constraint)->damping;
 193 }
 194
 195 void
 196 cpDampedSpringSetDamping(cpConstraint *constraint, cpFloat damping)
 197 {
 198         cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
 199         cpConstraintActivateBodies(constraint);
 200         ((cpDampedSpring *)constraint)->damping = damping;
 201 }
 202
 203 cpDampedSpringForceFunc
 204 cpDampedSpringGetSpringForceFunc(const cpConstraint *constraint)
 205 {
 206         cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
 207         return ((cpDampedSpring *)constraint)->springForceFunc;
 208 }
 209
 210 void
 211 cpDampedSpringSetSpringForceFunc(cpConstraint *constraint, cpDampedSpringForceFunc springForceFunc)
 212 {
 213         cpAssertHard(cpConstraintIsDampedSpring(constraint), "Constraint is not a damped spring.");
 214         cpConstraintActivateBodies(constraint);
 215         ((cpDampedSpring *)constraint)->springForceFunc = springForceFunc;
 216 }