src/trajtrap.c

   1 #include "a/trajtrap.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_trajtrap_gen(a_trajtrap *ctx, a_float vm, a_float ac, a_float de,
   9                        a_float p0, a_float p1, a_float v0, a_float v1)
  10 {
  11     a_float v02, v12, vc2;
  12     a_float const p = p1 - p0;
  13     a_float const _2p = 2 * p;
  14     a_bool const reversed = p < 0;
  15     if (ac == de) { return 0; }
  16     if (vm < 0) { vm = -vm; }
  17     v0 = A_SAT(v0, -vm, +vm);
  18     v1 = A_SAT(v1, -vm, +vm);
  19     ctx->p0 = p0;
  20     ctx->p1 = p1;
  21     ctx->v0 = v0;
  22     ctx->v1 = v1;
  23     v02 = v0 * v0;
  24     v12 = v1 * v1;
  25     vc2 = (v12 * ac - v02 * de - _2p * ac * de) / (ac - de);
  26     if (vc2 <= 0) { return 0; }
  27     if (vc2 > vm * vm) /* acceleration, constant velocity, deceleration */
  28     {
  29         ctx->vc = reversed ? -vm : vm;
  30         ctx->ta = (ctx->vc - v0) / ac;
  31         ctx->t = (v1 - ctx->vc) / de;
  32         ctx->pa = p0 + ctx->v0 * ctx->ta + A_FLOAT_C(0.5) * ac * ctx->ta * ctx->ta;
  33         ctx->pd = p1 - ctx->vc * ctx->t - A_FLOAT_C(0.5) * de * ctx->t * ctx->t;
  34         ctx->td = ctx->ta + (ctx->pd - ctx->pa) / ctx->vc;
  35         ctx->t += ctx->td;
  36     }
  37     else if (vc2 > v02 && vc2 <= v12) /* acceleration */
  38     {
  39         v12 = v02 + _2p * ac;
  40         if (v12 < 0) { return 0; }
  41         ctx->v1 = a_float_sqrt(v12);
  42         if (reversed) { ctx->v1 = -ctx->v1; }
  43         ctx->vc = ctx->v1;
  44         ctx->t = (ctx->v1 - v0) / ac;
  45         ctx->ta = ctx->t;
  46         ctx->td = ctx->t;
  47         ctx->pa = p0 + ctx->v0 * ctx->t + A_FLOAT_C(0.5) * ac * ctx->t * ctx->t;
  48         ctx->pd = p1;
  49     }
  50     else if (vc2 <= v02 && vc2 > v12) /* deceleration */
  51     {
  52         v12 = v02 + _2p * de;
  53         if (v12 < 0) { return 0; }
  54         ctx->v1 = a_float_sqrt(v12);
  55         if (reversed) { ctx->v1 = -ctx->v1; }
  56         ctx->vc = ctx->v0;
  57         ctx->t = (ctx->v1 - v0) / de;
  58         ctx->ta = 0;
  59         ctx->td = 0;
  60         ctx->pa = p0;
  61         ctx->pd = p0;
  62     }
  63     else /* acceleration, deceleration */
  64     {
  65         ctx->vc = a_float_sqrt(vc2);
  66         if (reversed) { ctx->vc = -ctx->vc; }
  67         ctx->t = (ctx->vc - v0) / ac;
  68         ctx->ta = ctx->t;
  69         ctx->td = ctx->t;
  70         ctx->pa = p0 + ctx->v0 * ctx->t + A_FLOAT_C(0.5) * ac * ctx->t * ctx->t;
  71         ctx->t += (v1 - ctx->vc) / de;
  72         ctx->pd = ctx->pa;
  73     }
  74     ctx->ac = ac;
  75     ctx->de = de;
  76     return ctx->t;
  77 }
  78
  79 a_float a_trajtrap_pos(a_trajtrap const *ctx, a_float dt)
  80 {
  81     if (dt >= ctx->ta)
  82     {
  83         if (dt < ctx->td) /* linear motion */
  84         {
  85             return ctx->pa + ctx->vc * (dt - ctx->ta);
  86         }
  87         if (dt < ctx->t) /* final blend */
  88         {
  89             dt -= ctx->td;
  90             return ctx->pd + ctx->vc * dt + A_FLOAT_C(0.5) * ctx->de * dt * dt;
  91         }
  92         return ctx->p1;
  93     }
  94     if (dt > 0) /* initial blend */
  95     {
  96         return ctx->p0 + ctx->v0 * dt + A_FLOAT_C(0.5) * ctx->ac * dt * dt;
  97     }
  98     return ctx->p0;
  99 }
 100
 101 a_float a_trajtrap_vel(a_trajtrap const *ctx, a_float dt)
 102 {
 103     if (dt >= ctx->ta)
 104     {
 105         if (dt < ctx->td) /* linear motion */
 106         {
 107             return ctx->vc;
 108         }
 109         if (dt < ctx->t) /* final blend */
 110         {
 111             return ctx->vc + ctx->de * (dt - ctx->td);
 112         }
 113         return ctx->v1;
 114     }
 115     if (dt > 0) /* initial blend */
 116     {
 117         return ctx->v0 + ctx->ac * dt;
 118     }
 119     return ctx->v0;
 120 }
 121
 122 a_float a_trajtrap_acc(a_trajtrap const *ctx, a_float dt)
 123 {
 124     if (dt < ctx->ta)
 125     {
 126         if (dt >= 0) /* initial blend */
 127         {
 128             return ctx->ac;
 129         }
 130     }
 131     else if (dt >= ctx->td)
 132     {
 133         if (dt <= ctx->t) /* final blend */
 134         {
 135             return ctx->de;
 136         }
 137     }
 138     return 0; /* linear motion */
 139 }