lua/src/trajbell.c

   1 /***
   2  bell-shaped velocity trajectory
   3  @module liba.trajbell
   4 */
   5
   6 #include "trajbell.h"
   7 #include "a/trajbell.h"
   8
   9 /***
  10  constructor for bell-shaped velocity trajectory
  11  @treturn a.trajbell bell-shaped velocity trajectory userdata
  12  @function new
  13 */
  14 int liba_trajbell_new(lua_State *L)
  15 {
  16     a_trajbell *const ctx = lua_newclass(L, a_trajbell);
  17     a_zero(ctx, sizeof(a_trajbell));
  18     lua_registry_get(L, liba_trajbell_new);
  19     lua_setmetatable(L, -2);
  20     return 1;
  21 }
  22
  23 /***
  24  generate for bell-shaped velocity trajectory
  25  @tparam a.trajbell ctx bell-shaped velocity trajectory userdata
  26  @tparam number jm defines the maximum jerk during system operation
  27  @tparam number am defines the maximum acceleration during system operation
  28  @tparam number vm defines the maximum velocity during system operation
  29  @tparam number p0 defines the initial position
  30  @tparam number p1 defines the final position
  31  @tparam[opt] number v0 defines the initial velocity
  32  @tparam[opt] number v1 defines the final velocity
  33  @treturn number total duration
  34  @function gen
  35 */
  36 int liba_trajbell_gen(lua_State *L)
  37 {
  38     a_float v0 = 0, v1 = 0;
  39     a_float jm, am, vm, p0, p1;
  40     int const top = lua_gettop(L);
  41     a_trajbell *const ctx = (a_trajbell *)lua_touserdata(L, 1);
  42     if (ctx)
  43     {
  44         jm = (a_float)luaL_checknumber(L, 2);
  45         am = (a_float)luaL_checknumber(L, 3);
  46         vm = (a_float)luaL_checknumber(L, 4);
  47         p0 = (a_float)luaL_checknumber(L, 5);
  48         p1 = (a_float)luaL_checknumber(L, 6);
  49         if (top >= 7) { v0 = (a_float)luaL_checknumber(L, 7); }
  50         if (top >= 8) { v1 = (a_float)luaL_checknumber(L, 8); }
  51         a_trajbell_gen(ctx, jm, am, vm, p0, p1, v0, v1);
  52         lua_pushnumber(L, (lua_Number)ctx->t);
  53         return 1;
  54     }
  55     return 0;
  56 }
  57
  58 /***
  59  calculate position for bell-shaped velocity trajectory
  60  @tparam a.trajbell ctx bell-shaped velocity trajectory userdata
  61  @tparam number x difference between current time and initial time
  62  @treturn number position output
  63  @function pos
  64 */
  65 int liba_trajbell_pos(lua_State *L)
  66 {
  67     a_trajbell const *const ctx = (a_trajbell const *)lua_touserdata(L, 1);
  68     if (ctx)
  69     {
  70         a_float const x = (a_float)luaL_checknumber(L, 2);
  71         lua_pushnumber(L, (lua_Number)a_trajbell_pos(ctx, x));
  72         return 1;
  73     }
  74     return 0;
  75 }
  76
  77 /***
  78  calculate velocity for bell-shaped velocity trajectory
  79  @tparam a.trajbell ctx bell-shaped velocity trajectory userdata
  80  @tparam number x difference between current time and initial time
  81  @treturn number velocity output
  82  @function vel
  83 */
  84 int liba_trajbell_vel(lua_State *L)
  85 {
  86     a_trajbell const *const ctx = (a_trajbell const *)lua_touserdata(L, 1);
  87     if (ctx)
  88     {
  89         a_float const x = (a_float)luaL_checknumber(L, 2);
  90         lua_pushnumber(L, (lua_Number)a_trajbell_vel(ctx, x));
  91         return 1;
  92     }
  93     return 0;
  94 }
  95
  96 /***
  97  calculate acceleration for bell-shaped velocity trajectory
  98  @tparam a.trajbell ctx bell-shaped velocity trajectory userdata
  99  @tparam number x difference between current time and initial time
 100  @treturn number acceleration output
 101  @function acc
 102 */
 103 int liba_trajbell_acc(lua_State *L)
 104 {
 105     a_trajbell const *const ctx = (a_trajbell const *)lua_touserdata(L, 1);
 106     if (ctx)
 107     {
 108         a_float const x = (a_float)luaL_checknumber(L, 2);
 109         lua_pushnumber(L, (lua_Number)a_trajbell_acc(ctx, x));
 110         return 1;
 111     }
 112     return 0;
 113 }
 114
 115 /***
 116  calculate jerk for bell-shaped velocity trajectory
 117  @tparam a.trajbell ctx bell-shaped velocity trajectory userdata
 118  @tparam number x difference between current time and initial time
 119  @treturn number jerk output
 120  @function jer
 121 */
 122 int liba_trajbell_jer(lua_State *L)
 123 {
 124     a_trajbell const *const ctx = (a_trajbell const *)lua_touserdata(L, 1);
 125     if (ctx)
 126     {
 127         a_float const x = (a_float)luaL_checknumber(L, 2);
 128         lua_pushnumber(L, (lua_Number)a_trajbell_jer(ctx, x));
 129         return 1;
 130     }
 131     return 0;
 132 }
 133
 134 static int liba_trajbell_set(lua_State *L)
 135 {
 136     switch (a_hash_bkdr(lua_tostring(L, 2), 0))
 137     {
 138     case 0xE8859EEB: /* __name */
 139     case 0xA65758B2: /* __index */
 140     case 0xAEB551C6: /* __newindex */
 141         break;
 142     default:
 143         lua_getmetatable(L, 1);
 144         lua_replace(L, 1);
 145         lua_rawset(L, 1);
 146     }
 147     return 0;
 148 }
 149
 150 static int liba_trajbell_get(lua_State *L)
 151 {
 152     a_trajbell const *const ctx = (a_trajbell const *)lua_touserdata(L, 1);
 153     switch (a_hash_bkdr(lua_tostring(L, 2), 0))
 154     {
 155     case 0x00000074: /* t */
 156         lua_pushnumber(L, (lua_Number)ctx->t);
 157         break;
 158     case 0x00003BD2: /* tv */
 159         lua_pushnumber(L, (lua_Number)ctx->tv);
 160         break;
 161     case 0x00003BBD: /* ta */
 162         lua_pushnumber(L, (lua_Number)ctx->ta);
 163         break;
 164     case 0x00003BC0: /* td */
 165         lua_pushnumber(L, (lua_Number)ctx->td);
 166         break;
 167     case 0x001E9221: /* taj */
 168         lua_pushnumber(L, (lua_Number)ctx->taj);
 169         break;
 170     case 0x001E93AA: /* tdj */
 171         lua_pushnumber(L, (lua_Number)ctx->tdj);
 172         break;
 173     case 0x00003980: /* p0 */
 174         lua_pushnumber(L, (lua_Number)ctx->p0);
 175         break;
 176     case 0x00003981: /* p1 */
 177         lua_pushnumber(L, (lua_Number)ctx->p1);
 178         break;
 179     case 0x00003C92: /* v0 */
 180         lua_pushnumber(L, (lua_Number)ctx->v0);
 181         break;
 182     case 0x00003C93: /* v1 */
 183         lua_pushnumber(L, (lua_Number)ctx->v1);
 184         break;
 185     case 0x00003CCF: /* vm */
 186         lua_pushnumber(L, (lua_Number)ctx->vm);
 187         break;
 188     case 0x000036AB: /* jm */
 189         lua_pushnumber(L, (lua_Number)ctx->jm);
 190         break;
 191     case 0x00003210: /* am */
 192         lua_pushnumber(L, (lua_Number)ctx->am);
 193         break;
 194     case 0x00003399: /* dm */
 195         lua_pushnumber(L, (lua_Number)ctx->dm);
 196         break;
 197     case 0xA65758B2: /* __index */
 198         lua_registry_get(L, liba_trajbell_new);
 199         lua_num_set(L, -1, "t", ctx->t);
 200         lua_num_set(L, -1, "tv", ctx->tv);
 201         lua_num_set(L, -1, "ta", ctx->ta);
 202         lua_num_set(L, -1, "td", ctx->td);
 203         lua_num_set(L, -1, "taj", ctx->taj);
 204         lua_num_set(L, -1, "tdj", ctx->tdj);
 205         lua_num_set(L, -1, "p0", ctx->p0);
 206         lua_num_set(L, -1, "p1", ctx->p1);
 207         lua_num_set(L, -1, "v0", ctx->v0);
 208         lua_num_set(L, -1, "v1", ctx->v1);
 209         lua_num_set(L, -1, "vm", ctx->vm);
 210         lua_num_set(L, -1, "jm", ctx->jm);
 211         lua_num_set(L, -1, "am", ctx->am);
 212         lua_num_set(L, -1, "dm", ctx->dm);
 213         break;
 214     default:
 215         lua_getmetatable(L, 1);
 216         lua_replace(L, 1);
 217         lua_rawget(L, 1);
 218     }
 219     return 1;
 220 }
 221
 222 static int liba_trajbell_(lua_State *L)
 223 {
 224     lua_pushcfunction(L, liba_trajbell_new);
 225     lua_replace(L, 1);
 226     lua_call(L, lua_gettop(L) - 1, 1);
 227     return 1;
 228 }
 229
 230 int luaopen_liba_trajbell(lua_State *L)
 231 {
 232     static lua_fun const funcs[] = {
 233         {"new", liba_trajbell_new},
 234         {"gen", liba_trajbell_gen},
 235         {"pos", liba_trajbell_pos},
 236         {"vel", liba_trajbell_vel},
 237         {"acc", liba_trajbell_acc},
 238         {"jer", liba_trajbell_jer},
 239     };
 240     static lua_fun const metas[] = {
 241         {"__newindex", liba_trajbell_set},
 242         {"__index", liba_trajbell_get},
 243     };
 244
 245     lua_createtable(L, 0, A_LEN(funcs));
 246     lua_fun_reg(L, -1, funcs, A_LEN(funcs));
 247     lua_createtable(L, 0, 1);
 248     lua_fun_set(L, -1, "__call", liba_trajbell_);
 249     lua_setmetatable(L, -2);
 250
 251     lua_createtable(L, 0, A_LEN(metas) + A_LEN(funcs) + 1);
 252     lua_fun_reg(L, -1, metas, A_LEN(metas));
 253     lua_fun_reg(L, -1, funcs, A_LEN(funcs));
 254     lua_str_set(L, -1, "__name", "a.trajbell");
 255     lua_registry_set(L, liba_trajbell_new);
 256
 257     return 1;
 258 }