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