src/traptraj.c

   1 #include "a/traptraj.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_traptraj_gen(a_traptraj *ctx, a_float qm, a_float vm, a_float ac, a_float de, a_float vs, a_float ve)
   9 {
  10     a_float vs2, ve2, vc2;
  11     a_float const _2q = 2 * qm;
  12     a_bool const reversed = qm < 0;
  13     if (vm < 0) { vm = -vm; }
  14     if (vs > vm) { vs = vm; }
  15     else if (vs < -vm) { vs = -vm; }
  16     if (ve > vm) { ve = vm; }
  17     else if (ve < -vm) { ve = -vm; }
  18     if (ac == de) { return 0; }
  19     ctx->ac = ac;
  20     ctx->de = de;
  21     ctx->vs = vs;
  22     ctx->ve = ve;
  23     vs2 = vs * vs;
  24     ve2 = ve * ve;
  25     vc2 = (ve2 * ac - vs2 * de - _2q * 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->t1 = (ctx->vc - vs) / ac;
  31         ctx->t = (ve - ctx->vc) / de;
  32         ctx->q1 = ctx->vs * ctx->t1 + A_FLOAT_C(0.5) * ac * ctx->t1 * ctx->t1;
  33         ctx->q = ctx->vc * ctx->t + A_FLOAT_C(0.5) * de * ctx->t * ctx->t;
  34         ctx->q2 = qm - ctx->q;
  35         ctx->t2 = (ctx->q2 - ctx->q1) / ctx->vc;
  36         ctx->t2 += ctx->t1;
  37         ctx->t += ctx->t2;
  38         ctx->q = qm;
  39     }
  40     else if (vc2 > vs2 && vc2 <= ve2) /* acceleration */
  41     {
  42         ve2 = vs2 + _2q * ac;
  43         if (ve2 < 0) { return 0; }
  44         ctx->ve = a_float_sqrt(ve2);
  45         if (reversed) { ctx->ve = -ctx->ve; }
  46         ctx->vc = ctx->ve;
  47         ctx->t1 = (ctx->ve - vs) / ac;
  48         ctx->t2 = ctx->t1;
  49         ctx->t = ctx->t1;
  50         ctx->q1 = ctx->vs * ctx->t1 + A_FLOAT_C(0.5) * ac * ctx->t1 * ctx->t1;
  51         ctx->q2 = qm;
  52         ctx->q = qm;
  53     }
  54     else if (vc2 <= vs2 && vc2 > ve2) /* deceleration */
  55     {
  56         ve2 = vs2 + _2q * de;
  57         if (ve2 < 0) { return 0; }
  58         ctx->ve = a_float_sqrt(ve2);
  59         if (reversed) { ctx->ve = -ctx->ve; }
  60         ctx->vc = ctx->vs;
  61         ctx->t1 = 0;
  62         ctx->t2 = 0;
  63         ctx->t = (ctx->ve - vs) / de;
  64         ctx->q1 = 0;
  65         ctx->q2 = 0;
  66         ctx->q = ctx->vc * ctx->t + A_FLOAT_C(0.5) * de * ctx->t * ctx->t;
  67     }
  68     else /* acceleration, deceleration */
  69     {
  70         ctx->vc = a_float_sqrt(vc2);
  71         if (reversed) { ctx->vc = -ctx->vc; }
  72         ctx->t1 = (ctx->vc - vs) / ac;
  73         ctx->t2 = ctx->t1;
  74         ctx->t = (ve - ctx->vc) / de;
  75         ctx->q1 = ctx->vs * ctx->t1 + A_FLOAT_C(0.5) * ac * ctx->t1 * ctx->t1;
  76         ctx->q2 = ctx->q1;
  77         ctx->q = ctx->vc * ctx->t + A_FLOAT_C(0.5) * de * ctx->t * ctx->t;
  78         ctx->t += ctx->t1;
  79         ctx->q += ctx->q1;
  80     }
  81     return ctx->t;
  82 }
  83
  84 a_float a_traptraj_pos(a_traptraj const *ctx, a_float dt)
  85 {
  86     if (dt >= ctx->t1)
  87     {
  88         if (dt < ctx->t2) /* linear motion */
  89         {
  90             return ctx->q1 + ctx->vc * (dt - ctx->t1);
  91         }
  92         if (dt < ctx->t) /* final blend */
  93         {
  94             dt -= ctx->t2;
  95             return ctx->q2 + ctx->vc * dt + A_FLOAT_C(0.5) * ctx->de * dt * dt;
  96         }
  97         return ctx->q;
  98     }
  99     if (dt > 0) /* initial blend */
 100     {
 101         return ctx->vs * dt + A_FLOAT_C(0.5) * ctx->ac * dt * dt;
 102     }
 103     return 0;
 104 }
 105
 106 a_float a_traptraj_vel(a_traptraj const *ctx, a_float dt)
 107 {
 108     if (dt >= ctx->t1)
 109     {
 110         if (dt < ctx->t2) /* linear motion */
 111         {
 112             return ctx->vc;
 113         }
 114         if (dt < ctx->t) /* final blend */
 115         {
 116             return ctx->vc + ctx->de * (dt - ctx->t2);
 117         }
 118         return ctx->ve;
 119     }
 120     if (dt > 0) /* initial blend */
 121     {
 122         return ctx->vs + ctx->ac * dt;
 123     }
 124     return ctx->vs;
 125 }
 126
 127 a_float a_traptraj_acc(a_traptraj const *ctx, a_float dt)
 128 {
 129     if (dt < ctx->t1)
 130     {
 131         if (dt >= 0) /* initial blend */
 132         {
 133             return ctx->ac;
 134         }
 135     }
 136     else if (dt >= ctx->t2)
 137     {
 138         if (dt < ctx->t) /* final blend */
 139         {
 140             return ctx->de;
 141         }
 142     }
 143     return 0; /* linear motion */
 144 }