src/trajbell.c

   1 #include "a/trajbell.h"
   2 #include "a/math.h"
   3
   4 #if A_PREREQ_GNUC(3, 0) || __has_warning("-Wfloat-equal")
   5 #pragma GCC diagnostic ignored "-Wfloat-equal"
   6 #endif /* -Wfloat-equal */
   7
   8 a_float a_trajbell_gen(a_trajbell *ctx, a_float jm, a_float am, a_float vm,
   9                        a_float p0, a_float p1, a_float v0, a_float v1)
  10 {
  11     a_float p, ac, tj, _2tj, _2v0, _2v1, _tmp, temp, v0pv1, _2v02pv12;
  12     if (jm < 0) { jm = -jm; }
  13     if (am < 0) { am = -am; }
  14     if (vm < 0) { vm = -vm; }
  15     if (v0 < -vm) { v0 = -vm; }
  16     else if (v0 > vm) { v0 = vm; }
  17     if (v1 < -vm) { v1 = -vm; }
  18     else if (v1 > vm) { v1 = vm; }
  19     ctx->p0 = p0;
  20     ctx->p1 = p1;
  21     ctx->v0 = v0;
  22     ctx->v1 = v1;
  23     if (p0 > p1)
  24     {
  25         p0 = -p0;
  26         p1 = -p1;
  27         v0 = -v0;
  28         v1 = -v1;
  29     }
  30     ctx->vm = vm;
  31     ctx->jm = jm;
  32     _tmp = am * am;
  33     _2v0 = vm - v0;
  34     if (_2v0 * jm < _tmp)
  35     {
  36         ctx->taj = a_float_sqrt(_2v0 / jm);
  37         ctx->ta = 2 * ctx->taj;
  38         ctx->am = +jm * ctx->taj;
  39     }
  40     else
  41     {
  42         ctx->taj = am / jm;
  43         ctx->ta = ctx->taj + _2v0 / am;
  44         ctx->am = +am;
  45     }
  46     _2v1 = vm - v1;
  47     if (_2v1 * jm < _tmp)
  48     {
  49         ctx->tdj = a_float_sqrt(_2v1 / jm);
  50         ctx->td = 2 * ctx->tdj;
  51         ctx->dm = -jm * ctx->tdj;
  52     }
  53     else
  54     {
  55         ctx->tdj = am / jm;
  56         ctx->td = ctx->tdj + _2v1 / am;
  57         ctx->dm = -am;
  58     }
  59     p = p1 - p0;
  60     ctx->tv = p / vm - A_FLOAT_C(0.5) * ctx->ta * (1 + v0 / vm) - A_FLOAT_C(0.5) * ctx->td * (1 + v1 / vm);
  61     if (ctx->tv > 0) { goto out; }
  62     ctx->tv = 0;
  63     _2v0 = 2 * v0;
  64     _2v1 = 2 * v1;
  65     v0pv1 = v0 + v1;
  66     ctx->am = ac = am;
  67     ctx->dm = ac * A_FLOAT_C(0.0625);
  68     _2v02pv12 = 2 * (v0 * v0 + v1 * v1);
  69     do {
  70         tj = am / jm;
  71         _2tj = 2 * tj;
  72         ctx->taj = tj;
  73         ctx->tdj = tj;
  74         _tmp = am * tj;
  75         temp = _tmp * _tmp + _2v02pv12 + (4 * p - _2tj * v0pv1) * am;
  76         _tmp += a_float_sqrt(temp);
  77         temp = 2 * am;
  78         ctx->ta = (_tmp - _2v0) / temp;
  79         if (ctx->ta < 0)
  80         {
  81             if (am == ctx->am || ac < ctx->dm)
  82             {
  83                 ctx->ta = 0;
  84                 ctx->taj = 0;
  85                 ctx->td = 2 * p / v0pv1;
  86                 _tmp = jm * p;
  87                 temp = jm * (_tmp * p + (v1 - v0) * v0pv1 * v0pv1);
  88                 if (temp < 0) { goto fail; }
  89                 ctx->tdj = (_tmp - a_float_sqrt(temp)) / (jm * v0pv1);
  90                 ctx->am = 0;
  91                 ctx->dm = -jm * ctx->tdj;
  92                 ctx->vm = v0;
  93                 goto out;
  94             }
  95             ac *= A_FLOAT_C(0.5);
  96             am += ac;
  97             continue;
  98         }
  99         ctx->td = (_tmp - _2v1) / temp;
 100         if (ctx->td < 0)
 101         {
 102             if (am == ctx->am || ac < ctx->dm)
 103             {
 104                 ctx->td = 0;
 105                 ctx->tdj = 0;
 106                 ctx->ta = 2 * p / v0pv1;
 107                 _tmp = jm * p;
 108                 temp = jm * (_tmp * p + (v0 - v1) * v0pv1 * v0pv1);
 109                 if (temp < 0) { goto fail; }
 110                 ctx->taj = (_tmp - a_float_sqrt(temp)) / (jm * v0pv1);
 111                 ctx->am = +jm * ctx->taj;
 112                 ctx->dm = 0;
 113                 ctx->vm = v0 + ctx->am * (ctx->ta - ctx->taj);
 114                 goto out;
 115             }
 116             ac *= A_FLOAT_C(0.5);
 117             am += ac;
 118             continue;
 119         }
 120         if (ctx->ta >= _2tj && ctx->td >= _2tj)
 121         {
 122             if (am == ctx->am || ac < ctx->dm)
 123             {
 124                 ctx->am = +am;
 125                 ctx->dm = -am;
 126                 ctx->vm = v0 + ctx->am * (ctx->ta - tj);
 127                 goto out;
 128             }
 129             ac *= A_FLOAT_C(0.5);
 130             am += ac;
 131             continue;
 132         }
 133         ac *= A_FLOAT_C(0.5);
 134         am -= ac;
 135     } while (ac > A_FLOAT_EPSILON);
 136 fail:
 137     ctx->t = 0;
 138     return 0;
 139 out:
 140     ctx->t = ctx->ta + ctx->tv + ctx->td;
 141     return ctx->t;
 142 }
 143
 144 a_float a_trajbell_pos(a_trajbell const *ctx, a_float dt)
 145 {
 146     a_float out;
 147     a_float p0 = ctx->p0;
 148     a_float p1 = ctx->p1;
 149     a_float v0 = ctx->v0;
 150     a_float v1 = ctx->v1;
 151     a_bool const rev = ctx->p0 > ctx->p1;
 152     if (rev)
 153     {
 154         p0 = -p0;
 155         p1 = -p1;
 156         v0 = -v0;
 157         v1 = -v1;
 158     }
 159     if (dt < ctx->ta)
 160     {
 161         if (dt < ctx->taj)
 162         {
 163             if (dt <= 0) { return ctx->p0; }
 164             out = p0 + v0 * dt + ctx->jm * dt * dt * dt / 6;
 165             goto out;
 166         }
 167         if (dt < ctx->ta - ctx->taj)
 168         {
 169             out = p0 + v0 * dt + ctx->am * (3 * dt * dt - 3 * dt * ctx->taj + ctx->taj * ctx->taj) / 6;
 170             goto out;
 171         }
 172         dt = ctx->ta - dt;
 173         out = p0 + A_FLOAT_C(0.5) * (ctx->vm + v0) * ctx->ta - ctx->vm * dt + ctx->jm * dt * dt * dt / 6;
 174         goto out;
 175     }
 176     if (dt < ctx->t - ctx->td + ctx->tdj)
 177     {
 178         if (dt < ctx->ta + ctx->tv)
 179         {
 180             out = p0 + A_FLOAT_C(0.5) * (ctx->vm + v0) * ctx->ta + ctx->vm * (dt - ctx->ta);
 181             goto out;
 182         }
 183         dt -= ctx->t - ctx->td;
 184         out = p1 - A_FLOAT_C(0.5) * (ctx->vm + v1) * ctx->td + ctx->vm * dt - ctx->jm * dt * dt * dt / 6;
 185         goto out;
 186     }
 187     if (dt < ctx->t)
 188     {
 189         if (dt < ctx->t - ctx->tdj)
 190         {
 191             dt -= ctx->t - ctx->td;
 192             out = p1 - A_FLOAT_C(0.5) * (ctx->vm + v1) * ctx->td + ctx->vm * dt +
 193                   ctx->dm * (3 * dt * dt - 3 * dt * ctx->tdj + ctx->tdj * ctx->tdj) / 6;
 194             goto out;
 195         }
 196         dt = ctx->t - dt;
 197         out = p1 - v1 * dt - ctx->jm * dt * dt * dt / 6;
 198         goto out;
 199     }
 200     return ctx->p1;
 201 out:
 202     return rev ? -out : out;
 203 }
 204
 205 a_float a_trajbell_vel(a_trajbell const *ctx, a_float dt)
 206 {
 207     a_float out;
 208     a_float v0 = ctx->v0;
 209     a_float v1 = ctx->v1;
 210     a_bool const rev = ctx->p0 > ctx->p1;
 211     if (rev)
 212     {
 213         v0 = -v0;
 214         v1 = -v1;
 215     }
 216     if (dt < ctx->ta)
 217     {
 218         if (dt < ctx->taj)
 219         {
 220             if (dt <= 0) { return ctx->v0; }
 221             out = v0 + A_FLOAT_C(0.5) * ctx->jm * dt * dt;
 222             goto out;
 223         }
 224         if (dt < ctx->ta - ctx->taj)
 225         {
 226             out = v0 + ctx->am * (dt - A_FLOAT_C(0.5) * ctx->taj);
 227             goto out;
 228         }
 229         dt = ctx->ta - dt;
 230         out = ctx->vm - A_FLOAT_C(0.5) * ctx->jm * dt * dt;
 231         goto out;
 232     }
 233     if (dt < ctx->t - ctx->td + ctx->tdj)
 234     {
 235         if (dt < ctx->ta + ctx->tv)
 236         {
 237             out = ctx->vm;
 238             goto out;
 239         }
 240         dt -= ctx->t - ctx->td;
 241         out = ctx->vm - A_FLOAT_C(0.5) * ctx->jm * dt * dt;
 242         goto out;
 243     }
 244     if (dt < ctx->t)
 245     {
 246         if (dt < ctx->t - ctx->tdj)
 247         {
 248             out = ctx->vm + ctx->dm * (dt - ctx->t + ctx->td - A_FLOAT_C(0.5) * ctx->tdj);
 249             goto out;
 250         }
 251         dt = ctx->t - dt;
 252         out = v1 + A_FLOAT_C(0.5) * ctx->jm * dt * dt;
 253         goto out;
 254     }
 255     return ctx->v1;
 256 out:
 257     return rev ? -out : out;
 258 }
 259
 260 a_float a_trajbell_acc(a_trajbell const *ctx, a_float dt)
 261 {
 262     a_float out;
 263     if (dt < ctx->ta)
 264     {
 265         if (dt >= ctx->taj)
 266         {
 267             if (dt < ctx->ta - ctx->taj)
 268             {
 269                 out = ctx->am;
 270                 goto out;
 271             }
 272             out = ctx->jm * (ctx->ta - dt);
 273             goto out;
 274         }
 275         if (dt > 0)
 276         {
 277             out = ctx->jm * dt;
 278             goto out;
 279         }
 280     }
 281     else if (dt < ctx->t - ctx->td + ctx->tdj)
 282     {
 283         if (dt >= ctx->ta + ctx->tv)
 284         {
 285             out = -ctx->jm * (dt - ctx->t + ctx->td);
 286             goto out;
 287         }
 288     }
 289     else if (dt < ctx->t)
 290     {
 291         if (dt < ctx->t - ctx->tdj)
 292         {
 293             out = ctx->dm;
 294             goto out;
 295         }
 296         out = -ctx->jm * (ctx->t - dt);
 297         goto out;
 298     }
 299     return 0;
 300 out:
 301     if (ctx->p0 > ctx->p1)
 302     {
 303         return -out;
 304     }
 305     return out;
 306 }
 307
 308 a_float a_trajbell_jer(a_trajbell const *ctx, a_float dt)
 309 {
 310     a_float out;
 311     if (dt < ctx->ta)
 312     {
 313         if (dt >= ctx->ta - ctx->taj)
 314         {
 315             out = -ctx->jm;
 316             goto out;
 317         }
 318         if (dt < ctx->taj && dt >= 0)
 319         {
 320             out = +ctx->jm;
 321             goto out;
 322         }
 323     }
 324     else if (dt < ctx->t - ctx->td + ctx->tdj)
 325     {
 326         if (dt >= ctx->ta + ctx->tv)
 327         {
 328             out = -ctx->jm;
 329             goto out;
 330         }
 331     }
 332     else if (dt <= ctx->t)
 333     {
 334         if (dt >= ctx->t - ctx->tdj)
 335         {
 336             out = +ctx->jm;
 337             goto out;
 338         }
 339     }
 340     return 0;
 341 out:
 342     if (ctx->p0 > ctx->p1)
 343     {
 344         return -out;
 345     }
 346     return out;
 347 }