src/trajpoly5.c

   1 #include "a/trajpoly5.h"
   2 #include "a/poly.h"
   3
   4 void a_trajpoly5_gen(a_trajpoly5 *ctx, a_float ts,
   5                      a_float p0, a_float p1,
   6                      a_float v0, a_float v1,
   7                      a_float a0, a_float a1)
   8 {
   9     a_trajpoly5_gen0(ctx, ts, p0, p1, v0, v1, a0, a1);
  10 #if defined(A_TRAJPOLY5) && (A_TRAJPOLY5 + 0 > 1)
  11     a_trajpoly5_gen1(ctx);
  12 #endif /* A_TRAJPOLY5 */
  13 #if defined(A_TRAJPOLY5) && (A_TRAJPOLY5 + 0 > 2)
  14     a_trajpoly5_gen2(ctx);
  15 #endif /* A_TRAJPOLY5 */
  16 }
  17
  18 void a_trajpoly5_gen0(a_trajpoly5 *ctx, a_float ts,
  19                       a_float p0, a_float p1,
  20                       a_float v0, a_float v1,
  21                       a_float a0, a_float a1)
  22 {
  23     a_float const p = p1 - p0;
  24     a_float const _t1 = 1 / ts;
  25     a_float const _t2 = _t1 * _t1;
  26     a_float const _t3 = _t1 * _t2;
  27     a_float const _t4 = _t2 * _t2;
  28     a_float const _t5 = _t2 * _t3;
  29
  30     ctx->p[0] = p0;
  31     ctx->p[1] = v0;
  32     ctx->p[2] = a0 * (a_float)(1 / 2.0);
  33     ctx->p[3] = (a_float)(1 / 2.0) *
  34                 (_t1 * (a1 - 3 * a0) -
  35                  _t2 * (12 * v0 + 8 * v1) +
  36                  _t3 * p * 20);
  37     ctx->p[4] = (a_float)(1 / 2.0) *
  38                 (_t2 * (3 * a0 - 2 * a1) +
  39                  _t3 * (16 * v0 + 14 * v1) -
  40                  _t4 * p * 30);
  41     ctx->p[5] = (a_float)(1 / 2.0) *
  42                 (_t3 * (a1 - a0) -
  43                  _t4 * (v0 + v1) * 6 +
  44                  _t5 * p * 12);
  45 }
  46
  47 #if defined(A_TRAJPOLY5) && (A_TRAJPOLY5 + 0 > 1)
  48 void a_trajpoly5_gen1(a_trajpoly5 *ctx)
  49 {
  50     ctx->v[0] = ctx->p[1];
  51     ctx->v[1] = ctx->p[2] * 2;
  52     ctx->v[2] = ctx->p[3] * 3;
  53     ctx->v[3] = ctx->p[4] * 4;
  54     ctx->v[4] = ctx->p[5] * 5;
  55 }
  56 #endif /* A_TRAJPOLY5 */
  57
  58 #if defined(A_TRAJPOLY5) && (A_TRAJPOLY5 + 0 > 2)
  59 void a_trajpoly5_gen2(a_trajpoly5 *ctx)
  60 {
  61     ctx->a[0] = ctx->v[1];
  62     ctx->a[1] = ctx->v[2] * 2;
  63     ctx->a[2] = ctx->v[3] * 3;
  64     ctx->a[3] = ctx->v[4] * 4;
  65 }
  66 #endif /* A_TRAJPOLY5 */
  67
  68 a_float a_trajpoly5_pos(a_trajpoly5 const *ctx, a_float dt)
  69 {
  70     return a_poly_eval_(ctx->p, ctx->p + A_LEN(ctx->p), dt);
  71 }
  72
  73 #if defined(A_TRAJPOLY5) && (A_TRAJPOLY5 + 0 > 1)
  74 a_float a_trajpoly5_vel(a_trajpoly5 const *ctx, a_float dt)
  75 {
  76     return a_poly_eval_(ctx->v, ctx->v + A_LEN(ctx->v), dt);
  77 }
  78 #endif /* A_TRAJPOLY5 */
  79
  80 #if defined(A_TRAJPOLY5) && (A_TRAJPOLY5 + 0 > 2)
  81 a_float a_trajpoly5_acc(a_trajpoly5 const *ctx, a_float dt)
  82 {
  83     return a_poly_eval_(ctx->a, ctx->a + A_LEN(ctx->a), dt);
  84 }
  85 #endif /* A_TRAJPOLY5 */