src/trajpoly7.c

   1 #include "a/trajpoly7.h"
   2 #include "a/poly.h"
   3
   4 void a_trajpoly7_gen(a_trajpoly7 *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                      a_float j0, a_float j1)
   9 {
  10     a_trajpoly7_gen0(ctx, ts, p0, p1, v0, v1, a0, a1, j0, j1);
  11 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 1)
  12     a_trajpoly7_gen1(ctx);
  13 #endif /* A_TRAJPOLY7 */
  14 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 2)
  15     a_trajpoly7_gen2(ctx);
  16 #endif /* A_TRAJPOLY7 */
  17 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 3)
  18     a_trajpoly7_gen3(ctx);
  19 #endif /* A_TRAJPOLY7 */
  20 }
  21
  22 void a_trajpoly7_gen0(a_trajpoly7 *ctx, a_float ts,
  23                       a_float p0, a_float p1,
  24                       a_float v0, a_float v1,
  25                       a_float a0, a_float a1,
  26                       a_float j0, a_float j1)
  27 {
  28     a_float const p = p1 - p0;
  29     a_float const _t1 = 1 / ts;
  30     a_float const _t2 = _t1 * _t1;
  31     a_float const _t3 = _t1 * _t2;
  32     a_float const _t4 = _t2 * _t2;
  33     a_float const _t5 = _t2 * _t3;
  34     a_float const _t6 = _t3 * _t3;
  35     a_float const _t7 = _t3 * _t4;
  36
  37     ctx->p[0] = p0;
  38     ctx->p[1] = v0;
  39     ctx->p[2] = a0 * (a_float)(1 / 2.0);
  40     ctx->p[3] = j0 * (a_float)(1 / 6.0);
  41     ctx->p[4] = (a_float)(1 / 6.0) *
  42                 (_t1 * (-4 * j0 - j1) +
  43                  _t2 * (15 * a1 - 30 * a0) -
  44                  _t3 * (120 * v0 + 90 * v1) +
  45                  _t4 * p * 210);
  46     ctx->p[5] = (a_float)(1 / 2.0) *
  47                 (_t2 * (2 * j0 + j1) +
  48                  _t3 * (20 * a0 - 14 * a1) +
  49                  _t4 * (90 * v0 + 78 * v1) -
  50                  _t5 * p * 168);
  51     ctx->p[6] = (a_float)(1 / 6.0) *
  52                 (_t3 * (-4 * j0 - 3 * j1) +
  53                  _t4 * (39 * a1 - 45 * a0) -
  54                  _t5 * (216 * v0 + 204 * v1) +
  55                  _t6 * p * 420);
  56     ctx->p[7] = (a_float)(1 / 6.0) *
  57                 (_t4 * (j0 + j1) +
  58                  _t5 * (a0 - a1) * 12 +
  59                  _t6 * (v0 + v1) * 60 -
  60                  _t7 * p * 120);
  61 }
  62
  63 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 1)
  64 void a_trajpoly7_gen1(a_trajpoly7 *ctx)
  65 {
  66     ctx->v[0] = ctx->p[1];
  67     ctx->v[1] = ctx->p[2] * 2;
  68     ctx->v[2] = ctx->p[3] * 3;
  69     ctx->v[3] = ctx->p[4] * 4;
  70     ctx->v[4] = ctx->p[5] * 5;
  71     ctx->v[5] = ctx->p[6] * 6;
  72     ctx->v[6] = ctx->p[7] * 7;
  73 }
  74 #endif /* A_TRAJPOLY7 */
  75
  76 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 2)
  77 void a_trajpoly7_gen2(a_trajpoly7 *ctx)
  78 {
  79     ctx->a[0] = ctx->v[1];
  80     ctx->a[1] = ctx->v[2] * 2;
  81     ctx->a[2] = ctx->v[3] * 3;
  82     ctx->a[3] = ctx->v[4] * 4;
  83     ctx->a[4] = ctx->v[5] * 5;
  84     ctx->a[5] = ctx->v[6] * 6;
  85 }
  86 #endif /* A_TRAJPOLY7 */
  87
  88 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 3)
  89 void a_trajpoly7_gen3(a_trajpoly7 *ctx)
  90 {
  91     ctx->j[0] = ctx->a[1];
  92     ctx->j[1] = ctx->a[2] * 2;
  93     ctx->j[2] = ctx->a[3] * 3;
  94     ctx->j[3] = ctx->a[4] * 4;
  95     ctx->j[4] = ctx->a[5] * 5;
  96 }
  97 #endif /* A_TRAJPOLY7 */
  98
  99 a_float a_trajpoly7_pos(a_trajpoly7 const *ctx, a_float dt)
 100 {
 101     return a_poly_eval_(ctx->p, ctx->p + A_LEN(ctx->p), dt);
 102 }
 103
 104 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 1)
 105 a_float a_trajpoly7_vel(a_trajpoly7 const *ctx, a_float dt)
 106 {
 107     return a_poly_eval_(ctx->v, ctx->v + A_LEN(ctx->v), dt);
 108 }
 109 #endif /* A_TRAJPOLY7 */
 110
 111 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 2)
 112 a_float a_trajpoly7_acc(a_trajpoly7 const *ctx, a_float dt)
 113 {
 114     return a_poly_eval_(ctx->a, ctx->a + A_LEN(ctx->a), dt);
 115 }
 116 #endif /* A_TRAJPOLY7 */
 117
 118 #if defined(A_TRAJPOLY7) && (A_TRAJPOLY7 + 0 > 3)
 119 a_float a_trajpoly7_jer(a_trajpoly7 const *ctx, a_float dt)
 120 {
 121     return a_poly_eval_(ctx->j, ctx->j + A_LEN(ctx->j), dt);
 122 }
 123 #endif /* A_TRAJPOLY7 */