src/trajpoly3.c

   1 #include "a/trajpoly3.h"
   2 #include "a/poly.h"
   3
   4 void a_trajpoly3_gen(a_trajpoly3 *ctx, a_float ts,
   5                      a_float p0, a_float p1,
   6                      a_float v0, a_float v1)
   7 {
   8     a_trajpoly3_gen0(ctx, ts, p0, p1, v0, v1);
   9 #if defined(A_TRAJPOLY3) && (A_TRAJPOLY3 + 0 > 1)
  10     a_trajpoly3_gen1(ctx);
  11 #endif /* A_TRAJPOLY3 */
  12 #if defined(A_TRAJPOLY3) && (A_TRAJPOLY3 + 0 > 2)
  13     a_trajpoly3_gen2(ctx);
  14 #endif /* A_TRAJPOLY3 */
  15 }
  16
  17 void a_trajpoly3_gen0(a_trajpoly3 *ctx, a_float ts,
  18                       a_float p0, a_float p1,
  19                       a_float v0, a_float v1)
  20 {
  21     a_float const p = p1 - p0;
  22     a_float const _t1 = 1 / ts;
  23     a_float const _t2 = _t1 * _t1;
  24     a_float const _t3 = _t1 * _t2;
  25
  26     ctx->p[0] = p0;
  27     ctx->p[1] = v0;
  28     ctx->p[2] = _t1 * (-2 * v0 - v1) +
  29                 _t2 * p * 3;
  30     ctx->p[3] = _t2 * (v0 + v1) -
  31                 _t3 * p * 2;
  32 }
  33
  34 #if defined(A_TRAJPOLY3) && (A_TRAJPOLY3 + 0 > 1)
  35 void a_trajpoly3_gen1(a_trajpoly3 *ctx)
  36 {
  37     ctx->v[0] = ctx->p[1];
  38     ctx->v[1] = ctx->p[2] * 2;
  39     ctx->v[2] = ctx->p[3] * 3;
  40 }
  41 #endif /* A_TRAJPOLY3 */
  42
  43 #if defined(A_TRAJPOLY3) && (A_TRAJPOLY3 + 0 > 2)
  44 void a_trajpoly3_gen2(a_trajpoly3 *ctx)
  45 {
  46     ctx->a[0] = ctx->v[1];
  47     ctx->a[1] = ctx->v[2] * 2;
  48 }
  49 #endif /* A_TRAJPOLY3 */
  50
  51 a_float a_trajpoly3_pos(a_trajpoly3 const *ctx, a_float dt)
  52 {
  53     return a_poly_eval_(ctx->p, ctx->p + A_LEN(ctx->p), dt);
  54 }
  55
  56 #if defined(A_TRAJPOLY3) && (A_TRAJPOLY3 + 0 > 1)
  57 a_float a_trajpoly3_vel(a_trajpoly3 const *ctx, a_float dt)
  58 {
  59     return a_poly_eval_(ctx->v, ctx->v + A_LEN(ctx->v), dt);
  60 }
  61 #endif /* A_TRAJPOLY3 */
  62
  63 #if defined(A_TRAJPOLY3) && (A_TRAJPOLY3 + 0 > 2)
  64 a_float a_trajpoly3_acc(a_trajpoly3 const *ctx, a_float dt)
  65 {
  66     return a_poly_eval_(ctx->a, ctx->a + A_LEN(ctx->a), dt);
  67 }
  68 #endif /* A_TRAJPOLY3 */