File: D:\TMTmethod\simulator\codegen\mex\testspline\EvalSpline.c

    1   /*
    2    * EvalSpline.c
    3    *
    4    * Code generation for function 'EvalSpline'
    5    *
    6    * C source code generated on: Thu Aug 30 14:03:55 2012
    7    *
    8    */
    9   
   10   /* Include files */
   11   #include "rt_nonfinite.h"
   12   #include "testspline.h"
   13   #include "EvalSpline.h"
   14   #include "mpower.h"
   15   #include "mrdivide.h"
   16   #include "testspline_mexutil.h"
   17   
   18   /* Type Definitions */
   19   
   20   /* Named Constants */
   21   
   22   /* Variable Declarations */
   23   
   24   /* Variable Definitions */
   25   static emlrtRSInfo g_emlrtRSI = { 10, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   26   static emlrtRSInfo h_emlrtRSI = { 13, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   27   static emlrtRSInfo i_emlrtRSI = { 14, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   28   static emlrtRSInfo j_emlrtRSI = { 26, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   29   static emlrtRSInfo k_emlrtRSI = { 28, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   30   static emlrtRSInfo l_emlrtRSI = { 31, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   31   static emlrtRSInfo m_emlrtRSI = { 37, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   32   static emlrtRSInfo n_emlrtRSI = { 38, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   33   static emlrtRSInfo rd_emlrtRSI = { 16, "max", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/datafun/max.m" };
   34   static emlrtRSInfo sd_emlrtRSI = { 18, "eml_min_or_max", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/eml_min_or_max.m" };
   35   static emlrtRSInfo td_emlrtRSI = { 59, "eml_min_or_max", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/eml_min_or_max.m" };
   36   static emlrtRSInfo ud_emlrtRSI = { 16, "min", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/datafun/min.m" };
   37   static emlrtRSInfo vd_emlrtRSI = { 18, "eml_min_or_max", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/eml_min_or_max.m" };
   38   static emlrtRSInfo wd_emlrtRSI = { 59, "eml_min_or_max", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/eml_min_or_max.m" };
   39   static emlrtRSInfo nj_emlrtRSI = { 21, "mtimes", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/ops/mtimes.m" };
   40   static emlrtRSInfo pj_emlrtRSI = { 84, "mtimes", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/ops/mtimes.m" };
   41   static emlrtRSInfo ok_emlrtRSI = { 41, "mpower", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/ops/mpower.m" };
   42   static emlrtRSInfo rk_emlrtRSI = { 4, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   43   static emlrtRSInfo sk_emlrtRSI = { 10, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   44   static emlrtRSInfo tk_emlrtRSI = { 26, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   45   static emlrtRSInfo uk_emlrtRSI = { 28, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   46   static emlrtRSInfo vk_emlrtRSI = { 31, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   47   static emlrtRSInfo wk_emlrtRSI = { 37, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   48   static emlrtRSInfo xk_emlrtRSI = { 38, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   49   static emlrtRSInfo yk_emlrtRSI = { 41, "find", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/elmat/find.m" };
   50   static emlrtRSInfo al_emlrtRSI = { 1, "mrdivide", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/ops/mrdivide.p" };
   51   static emlrtRSInfo bl_emlrtRSI = { 1, "mldivide", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/ops/mldivide.p" };
   52   static emlrtRSInfo cl_emlrtRSI = { 20, "eml_lusolve", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/eml_lusolve.m" };
   53   static emlrtRSInfo dl_emlrtRSI = { 92, "eml_lusolve", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/eml_lusolve.m" };
   54   static emlrtRSInfo el_emlrtRSI = { 90, "eml_lusolve", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/eml_lusolve.m" };
   55   static emlrtRSInfo fl_emlrtRSI = { 54, "eml_xtrsm", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/blas/eml_xtrsm.m" };
   56   static emlrtRSInfo gl_emlrtRSI = { 28, "eml_blas_xtrsm", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/blas/external/eml_blas_xtrsm.m" };
   57   static emlrtRSInfo il_emlrtRSI = { 80, "eml_blas_xtrsm", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/blas/external/eml_blas_xtrsm.m" };
   58   static emlrtRSInfo jl_emlrtRSI = { 54, "eml_xtrsm", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/blas/eml_xtrsm.m" };
   59   static emlrtRSInfo kl_emlrtRSI = { 28, "eml_blas_xtrsm", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/blas/external/eml_blas_xtrsm.m" };
   60   static emlrtRSInfo ml_emlrtRSI = { 80, "eml_blas_xtrsm", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/blas/external/eml_blas_xtrsm.m" };
   61   static emlrtMCInfo rb_emlrtMCI = { 85, 13, "mtimes", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/ops/mtimes.m" };
   62   static emlrtMCInfo sb_emlrtMCI = { 84, 23, "mtimes", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/ops/mtimes.m" };
   63   static emlrtBCInfo b_emlrtBCI = { -1, -1, 123, 5, "", "eml_min_or_max", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/eml_min_or_max.m", 0 };
   64   static emlrtBCInfo c_emlrtBCI = { -1, -1, 121, 10, "", "eml_min_or_max", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/eml_min_or_max.m", 0 };
   65   static emlrtBCInfo d_emlrtBCI = { -1, -1, 123, 5, "", "eml_min_or_max", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/eml_min_or_max.m", 0 };
   66   static emlrtBCInfo e_emlrtBCI = { -1, -1, 121, 10, "", "eml_min_or_max", "C:/Program Files/MATLAB/R2011b/toolbox/eml/lib/matlab/eml/eml_min_or_max.m", 0 };
   67   static emlrtBCInfo f_emlrtBCI = { -1, -1, 38, 1, "x(3) = x(3)/spline.duration(n)^2", "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m", 0 };
   68   static emlrtECInfo emlrtECI = { -1, 38, 1, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   69   static emlrtBCInfo g_emlrtBCI = { -1, -1, 37, 1, "x(2) = x(2)/spline.duration(n)", "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m", 0 };
   70   static emlrtECInfo b_emlrtECI = { -1, 37, 1, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   71   static emlrtBCInfo h_emlrtBCI = { -1, -1, 31, 9, "x(3) + (i-2)*(i-1)*t^(i-3)*spline.coefs(n, i)", "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m", 0 };
   72   static emlrtECInfo c_emlrtECI = { -1, 31, 9, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   73   static emlrtBCInfo i_emlrtBCI = { -1, -1, 28, 9, "x(2) + (i-1)*t^(i-2)*spline.coefs(n, i)", "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m", 0 };
   74   static emlrtECInfo d_emlrtECI = { -1, 28, 9, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   75   static emlrtBCInfo j_emlrtBCI = { -1, -1, 26, 5, "x(1) + t^(i-1)*spline.coefs(n, i)", "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m", 0 };
   76   static emlrtECInfo e_emlrtECI = { -1, 26, 5, "EvalSpline", "D:/TMTmethod/simulator/splines/EvalSpline.m" };
   77   
   78   /* Function Declarations */
   79   
   80   /* Function Definitions */
   81   
   82   void EvalSpline(real_T t, real_T x[3])
   83   {
   84       int32_T ii_sizes_idx_0;
   85       int32_T ii_sizes_idx_1;
   86       real_T t_data[1];
   87       int32_T t_sizes[2];
   88       int32_T loop_ub;
   89       int32_T i0;
   90       real_T tmp_data[1];
   91       int32_T tmp_sizes[2];
   92       int32_T b_t_sizes[2];
   93       real_T b_t_data[1];
   94       int32_T x_sizes[2];
   95       real_T x_data[1];
   96       int8_T iv1[2];
   97       int32_T k;
   98       int32_T i;
   99       static const int8_T iv2[4] = { 0, 0, 3, -2 };
  100       int8_T b_data[1];
  101       const mxArray *y;
  102       static const int32_T iv3[2] = { 1, 45 };
  103       const mxArray *m1;
  104       static const char_T cv1[45] = { 'C', 'o', 'd', 'e', 'r', ':', 't', 'o', 'o', 'l', 'b', 'o', 'x', ':', 'm', 't', 'i', 'm', 'e', 's', '_', 'n', 'o', 'D', 'y', 'n', 'a', 'm', 'i', 'c', 'S', 'c', 'a', 'l', 'a', 'r', 'E', 'x', 'p', 'a', 'n', 's', 'i', 'o', 'n' };
  105       real_T C_data[1];
  106       const mxArray *b_y;
  107       static const int32_T iv4[2] = { 1, 45 };
  108       const mxArray *c_y;
  109       static const int32_T iv5[2] = { 1, 45 };
  110       int32_T b_tmp_sizes[2];
  111       int32_T c_tmp_sizes[2];
  112       /*  multipal knots splines */
  113       if (0.0 < t) {
  114           ii_sizes_idx_0 = 1;
  115           ii_sizes_idx_1 = 1;
  116       } else {
  117           ii_sizes_idx_0 = 0;
  118           ii_sizes_idx_1 = 0;
  119       }
  120       if ((ii_sizes_idx_0 == 0) || (ii_sizes_idx_1 == 0)) {
  121           ii_sizes_idx_0 = 1;
  122           ii_sizes_idx_1 = 1;
  123       }
  124       /*  shift and scale time */
  125       EMLRTPUSHRTSTACK(&g_emlrtRSI);
  126       t_sizes[0] = ii_sizes_idx_0;
  127       t_sizes[1] = ii_sizes_idx_1;
  128       loop_ub = ii_sizes_idx_0 * ii_sizes_idx_1 - 1;
  129       i0 = 0;
  130       while (i0 <= loop_ub) {
  131           t_data[0] = t;
  132           i0 = 1;
  133       }
  134       tmp_sizes[0] = ii_sizes_idx_0;
  135       tmp_sizes[1] = ii_sizes_idx_1;
  136       loop_ub = ii_sizes_idx_0 * ii_sizes_idx_1 - 1;
  137       i0 = 0;
  138       while (i0 <= loop_ub) {
  139           tmp_data[0] = 1.0;
  140           i0 = 1;
  141       }
  142       mrdivide(t_data, t_sizes, tmp_data, tmp_sizes, b_t_data, b_t_sizes);
  143       EMLRTPOPRTSTACK(&g_emlrtRSI);
  144       /*  clip t between 0 and 1 */
  145       EMLRTPUSHRTSTACK(&h_emlrtRSI);
  146       EMLRTPUSHRTSTACK(&rd_emlrtRSI);
  147       EMLRTPUSHRTSTACK(&sd_emlrtRSI);
  148       EMLRTPUSHRTSTACK(&td_emlrtRSI);
  149       x_sizes[0] = b_t_sizes[0];
  150       x_sizes[1] = b_t_sizes[1];
  151       loop_ub = b_t_sizes[0] * b_t_sizes[1] - 1;
  152       for (i0 = 0; i0 <= loop_ub; i0++) {
  153           x_data[i0] = b_t_data[i0];
  154       }
  155       for (i0 = 0; i0 < 2; i0++) {
  156           iv1[i0] = (int8_T)b_t_sizes[i0];
  157       }
  158       b_t_sizes[0] = (int32_T)iv1[0];
  159       b_t_sizes[1] = (int32_T)iv1[1];
  160       i0 = b_t_sizes[0] * b_t_sizes[1];
  161       k = 1;
  162       while (k <= i0) {
  163           emlrtDynamicBoundsCheck(1, 1, x_sizes[0] * x_sizes[1], &e_emlrtBCI);
  164           emlrtDynamicBoundsCheck(1, 1, b_t_sizes[0] * b_t_sizes[1], &d_emlrtBCI);
  165           b_t_data[0] = muDoubleScalarMax(x_data[0], 0.0);
  166           k = 2;
  167       }
  168       EMLRTPOPRTSTACK(&td_emlrtRSI);
  169       EMLRTPOPRTSTACK(&sd_emlrtRSI);
  170       EMLRTPOPRTSTACK(&rd_emlrtRSI);
  171       EMLRTPOPRTSTACK(&h_emlrtRSI);
  172       EMLRTPUSHRTSTACK(&i_emlrtRSI);
  173       EMLRTPUSHRTSTACK(&ud_emlrtRSI);
  174       EMLRTPUSHRTSTACK(&vd_emlrtRSI);
  175       EMLRTPUSHRTSTACK(&wd_emlrtRSI);
  176       x_sizes[0] = b_t_sizes[0];
  177       x_sizes[1] = b_t_sizes[1];
  178       loop_ub = b_t_sizes[0] * b_t_sizes[1] - 1;
  179       for (i0 = 0; i0 <= loop_ub; i0++) {
  180           x_data[i0] = b_t_data[i0];
  181       }
  182       for (i0 = 0; i0 < 2; i0++) {
  183           iv1[i0] = (int8_T)b_t_sizes[i0];
  184       }
  185       b_t_sizes[0] = (int32_T)iv1[0];
  186       b_t_sizes[1] = (int32_T)iv1[1];
  187       i0 = b_t_sizes[0] * b_t_sizes[1];
  188       k = 1;
  189       while (k <= i0) {
  190           emlrtDynamicBoundsCheck(1, 1, x_sizes[0] * x_sizes[1], &c_emlrtBCI);
  191           emlrtDynamicBoundsCheck(1, 1, b_t_sizes[0] * b_t_sizes[1], &b_emlrtBCI);
  192           b_t_data[0] = muDoubleScalarMin(x_data[0], 1.0);
  193           k = 2;
  194       }
  195       EMLRTPOPRTSTACK(&wd_emlrtRSI);
  196       EMLRTPOPRTSTACK(&vd_emlrtRSI);
  197       EMLRTPOPRTSTACK(&ud_emlrtRSI);
  198       EMLRTPOPRTSTACK(&i_emlrtRSI);
  199       /*  if isfield(spline,'tm') */
  200       /*      if t<spline.tm; */
  201       /*          spline.coefs = spline.coefs(:,1); */
  202       /*      else */
  203       /*          spline.coefs = spline.coefs(:,2); */
  204       /*      end */
  205       /*  end */
  206       for (i0 = 0; i0 < 3; i0++) {
  207           x[i0] = 0.0;
  208       }
  209       /*  [pos,vel,acc] */
  210       for (i = 0; i < 4; i++) {
  211           EMLRTPUSHRTSTACK(&j_emlrtRSI);
  212           mpower(b_t_data, b_t_sizes, (1.0 + (real_T)i) - 1.0, x_data, x_sizes);
  213           i0 = 0;
  214           while (i0 <= 0) {
  215               b_data[0] = iv2[i];
  216               i0 = 1;
  217           }
  218           EMLRTPUSHRTSTACK(&nj_emlrtRSI);
  219           if (!(x_sizes[1] == 1)) {
  220               EMLRTPUSHRTSTACK(&pj_emlrtRSI);
  221               y = NULL;
  222               m1 = mxCreateCharArray(2, iv3);
  223               emlrtInitCharArray(45, m1, cv1);
  224               emlrtAssign(&y, m1);
  225               error(message(y, &rb_emlrtMCI), &sb_emlrtMCI);
  226               EMLRTPOPRTSTACK(&pj_emlrtRSI);
  227           }
  228           EMLRTPOPRTSTACK(&nj_emlrtRSI);
  229           loop_ub = x_sizes[0] - 1;
  230           for (i0 = 0; i0 <= loop_ub; i0++) {
  231               C_data[i0] = 0.0;
  232               k = 0;
  233               while (k <= 0) {
  234                   C_data[i0] += x_data[i0] * (real_T)b_data[0];
  235                   k = 1;
  236               }
  237           }
  238           EMLRTPOPRTSTACK(&j_emlrtRSI);
  239           emlrtSizeEqCheck1D(1, x_sizes[0], &e_emlrtECI);
  240           emlrtDynamicBoundsCheck(1, 1, x_sizes[0], &j_emlrtBCI);
  241           x[0] += C_data[0];
  242           if (1 + i > 1) {
  243               EMLRTPUSHRTSTACK(&k_emlrtRSI);
  244               mpower(b_t_data, b_t_sizes, (1.0 + (real_T)i) - 2.0, x_data, x_sizes);
  245               loop_ub = x_sizes[0] * x_sizes[1] - 1;
  246               for (i0 = 0; i0 <= loop_ub; i0++) {
  247                   x_data[i0] *= (1.0 + (real_T)i) - 1.0;
  248               }
  249               i0 = 0;
  250               while (i0 <= 0) {
  251                   b_data[0] = iv2[i];
  252                   i0 = 1;
  253               }
  254               EMLRTPUSHRTSTACK(&nj_emlrtRSI);
  255               if (!(x_sizes[1] == 1)) {
  256                   EMLRTPUSHRTSTACK(&pj_emlrtRSI);
  257                   b_y = NULL;
  258                   m1 = mxCreateCharArray(2, iv4);
  259                   emlrtInitCharArray(45, m1, cv1);
  260                   emlrtAssign(&b_y, m1);
  261                   error(message(b_y, &rb_emlrtMCI), &sb_emlrtMCI);
  262                   EMLRTPOPRTSTACK(&pj_emlrtRSI);
  263               }
  264               EMLRTPOPRTSTACK(&nj_emlrtRSI);
  265               loop_ub = x_sizes[0] - 1;
  266               for (i0 = 0; i0 <= loop_ub; i0++) {
  267                   C_data[i0] = 0.0;
  268                   k = 0;
  269                   while (k <= 0) {
  270                       C_data[i0] += x_data[i0] * (real_T)b_data[0];
  271                       k = 1;
  272                   }
  273               }
  274               EMLRTPOPRTSTACK(&k_emlrtRSI);
  275               emlrtSizeEqCheck1D(1, x_sizes[0], &d_emlrtECI);
  276               emlrtDynamicBoundsCheck(1, 1, x_sizes[0], &i_emlrtBCI);
  277               x[1] += C_data[0];
  278           }
  279           if (1 + i > 2) {
  280               EMLRTPUSHRTSTACK(&l_emlrtRSI);
  281               k = (i - 1) * i;
  282               mpower(b_t_data, b_t_sizes, (1.0 + (real_T)i) - 3.0, x_data, x_sizes);
  283               loop_ub = x_sizes[0] * x_sizes[1] - 1;
  284               for (i0 = 0; i0 <= loop_ub; i0++) {
  285                   x_data[i0] *= (real_T)k;
  286               }
  287               i0 = 0;
  288               while (i0 <= 0) {
  289                   b_data[0] = iv2[i];
  290                   i0 = 1;
  291               }
  292               EMLRTPUSHRTSTACK(&nj_emlrtRSI);
  293               if (!(x_sizes[1] == 1)) {
  294                   EMLRTPUSHRTSTACK(&pj_emlrtRSI);
  295                   c_y = NULL;
  296                   m1 = mxCreateCharArray(2, iv5);
  297                   emlrtInitCharArray(45, m1, cv1);
  298                   emlrtAssign(&c_y, m1);
  299                   error(message(c_y, &rb_emlrtMCI), &sb_emlrtMCI);
  300                   EMLRTPOPRTSTACK(&pj_emlrtRSI);
  301               }
  302               EMLRTPOPRTSTACK(&nj_emlrtRSI);
  303               loop_ub = x_sizes[0] - 1;
  304               for (i0 = 0; i0 <= loop_ub; i0++) {
  305                   C_data[i0] = 0.0;
  306                   k = 0;
  307                   while (k <= 0) {
  308                       C_data[i0] += x_data[i0] * (real_T)b_data[0];
  309                       k = 1;
  310                   }
  311               }
  312               EMLRTPOPRTSTACK(&l_emlrtRSI);
  313               emlrtSizeEqCheck1D(1, x_sizes[0], &c_emlrtECI);
  314               emlrtDynamicBoundsCheck(1, 1, x_sizes[0], &h_emlrtBCI);
  315               x[2] += C_data[0];
  316           }
  317           emlrtBreakCheck();
  318       }
  319       /*  scale with duration */
  320       EMLRTPUSHRTSTACK(&m_emlrtRSI);
  321       b_tmp_sizes[0] = ii_sizes_idx_0;
  322       b_tmp_sizes[1] = ii_sizes_idx_1;
  323       loop_ub = ii_sizes_idx_0 * ii_sizes_idx_1 - 1;
  324       i0 = 0;
  325       while (i0 <= loop_ub) {
  326           tmp_data[0] = 1.0;
  327           i0 = 1;
  328       }
  329       c_mrdivide(x[1], tmp_data, b_tmp_sizes, t_data, tmp_sizes);
  330       EMLRTPOPRTSTACK(&m_emlrtRSI);
  331       emlrtSizeEqCheck1D(1, tmp_sizes[1], &b_emlrtECI);
  332       emlrtDynamicBoundsCheck(1, 1, tmp_sizes[1], &g_emlrtBCI);
  333       x[1] = t_data[0];
  334       EMLRTPUSHRTSTACK(&n_emlrtRSI);
  335       EMLRTPUSHRTSTACK(&ok_emlrtRSI);
  336       c_tmp_sizes[0] = ii_sizes_idx_0;
  337       c_tmp_sizes[1] = ii_sizes_idx_1;
  338       loop_ub = ii_sizes_idx_0 * ii_sizes_idx_1 - 1;
  339       i0 = 0;
  340       while (i0 <= loop_ub) {
  341           tmp_data[0] = 1.0;
  342           i0 = 1;
  343       }
  344       b_matrix_to_integer_power(tmp_data, c_tmp_sizes, x_data, x_sizes);
  345       EMLRTPOPRTSTACK(&ok_emlrtRSI);
  346       c_mrdivide(x[2], x_data, x_sizes, t_data, tmp_sizes);
  347       EMLRTPOPRTSTACK(&n_emlrtRSI);
  348       emlrtSizeEqCheck1D(1, tmp_sizes[1], &emlrtECI);
  349       emlrtDynamicBoundsCheck(1, 1, tmp_sizes[1], &f_emlrtBCI);
  350       x[2] = t_data[0];
  351   }
  352   
  353   void b_EvalSpline(real_T t, const real_T spline_startTime[2], const real_T spline_duration[2], const real_T spline_coefs[8], real_T x[3])
  354   {
  355       boolean_T b_x[2];
  356       int32_T i;
  357       int8_T ii_data[1];
  358       int32_T ii_sizes;
  359       int32_T ii;
  360       boolean_T exitg2;
  361       int32_T i19;
  362       int8_T n_data[1];
  363       real_T A_data[1];
  364       int32_T t_sizes;
  365       real_T e;
  366       int32_T lda;
  367       int32_T ldb;
  368       char_T SIDE;
  369       char_T UPLO;
  370       char_T TRANSA;
  371       char_T DIAGA;
  372       real_T Y_data[1];
  373       real_T t_data[1];
  374       int32_T spline_duration_sizes[2];
  375       boolean_T firstmult;
  376       int32_T exitg1;
  377       real_T ed2;
  378       /*  multipal knots splines */
  379       EMLRTPUSHRTSTACK(&rk_emlrtRSI);
  380       for (i = 0; i < 2; i++) {
  381           b_x[i] = (spline_startTime[i] < t);
  382       }
  383       EMLRTPUSHRTSTACK(&yk_emlrtRSI);
  384       i = 0;
  385       ii_sizes = 1;
  386       ii = 2;
  387       exitg2 = 0U;
  388       while ((exitg2 == 0U) && (ii > 0)) {
  389           if (b_x[ii - 1]) {
  390               i = 1;
  391               ii_data[0] = (int8_T)ii;
  392               exitg2 = 1U;
  393           } else {
  394               ii--;
  395           }
  396       }
  397       if (i == 0) {
  398           ii_sizes = 0;
  399       }
  400       EMLRTPOPRTSTACK(&yk_emlrtRSI);
  401       ii = ii_sizes - 1;
  402       i19 = 0;
  403       while (i19 <= ii) {
  404           n_data[0] = ii_data[0];
  405           i19 = 1;
  406       }
  407       EMLRTPOPRTSTACK(&rk_emlrtRSI);
  408       if (ii_sizes == 0) {
  409           ii_sizes = 1;
  410           n_data[0] = 1;
  411       }
  412       /*  shift and scale time */
  413       EMLRTPUSHRTSTACK(&sk_emlrtRSI);
  414       EMLRTPUSHRTSTACK(&al_emlrtRSI);
  415       ii = ii_sizes - 1;
  416       i19 = 0;
  417       while (i19 <= ii) {
  418           A_data[0] = spline_duration[n_data[0] - 1];
  419           i19 = 1;
  420       }
  421       EMLRTPUSHRTSTACK(&bl_emlrtRSI);
  422       EMLRTPUSHRTSTACK(&cl_emlrtRSI);
  423       EMLRTPUSHRTSTACK(&el_emlrtRSI);
  424       EMLRTPUSHRTSTACK(&fl_emlrtRSI);
  425       EMLRTPUSHRTSTACK(&gl_emlrtRSI);
  426       i = 1;
  427       t_sizes = 1;
  428       e = 1.0;
  429       lda = 1;
  430       ldb = 1;
  431       SIDE = 'L';
  432       UPLO = 'L';
  433       TRANSA = 'N';
  434       DIAGA = 'U';
  435       ii = ii_sizes - 1;
  436       i19 = 0;
  437       while (i19 <= ii) {
  438           Y_data[0] = t - spline_startTime[n_data[0] - 1];
  439           i19 = 1;
  440       }
  441       EMLRTPUSHRTSTACK(&il_emlrtRSI);
  442       dtrsm32(&SIDE, &UPLO, &TRANSA, &DIAGA, &i, &t_sizes, &e, &A_data[0], &lda, &Y_data[0], &ldb);
  443       EMLRTPOPRTSTACK(&il_emlrtRSI);
  444       EMLRTPOPRTSTACK(&gl_emlrtRSI);
  445       EMLRTPOPRTSTACK(&fl_emlrtRSI);
  446       EMLRTPOPRTSTACK(&el_emlrtRSI);
  447       EMLRTPUSHRTSTACK(&dl_emlrtRSI);
  448       EMLRTPUSHRTSTACK(&jl_emlrtRSI);
  449       EMLRTPUSHRTSTACK(&kl_emlrtRSI);
  450       i = 1;
  451       t_sizes = 1;
  452       e = 1.0;
  453       lda = 1;
  454       ldb = 1;
  455       SIDE = 'L';
  456       UPLO = 'U';
  457       TRANSA = 'N';
  458       DIAGA = 'N';
  459       EMLRTPUSHRTSTACK(&ml_emlrtRSI);
  460       dtrsm32(&SIDE, &UPLO, &TRANSA, &DIAGA, &i, &t_sizes, &e, &A_data[0], &lda, &Y_data[0], &ldb);
  461       EMLRTPOPRTSTACK(&ml_emlrtRSI);
  462       EMLRTPOPRTSTACK(&kl_emlrtRSI);
  463       EMLRTPOPRTSTACK(&jl_emlrtRSI);
  464       EMLRTPOPRTSTACK(&dl_emlrtRSI);
  465       EMLRTPOPRTSTACK(&cl_emlrtRSI);
  466       EMLRTPOPRTSTACK(&bl_emlrtRSI);
  467       EMLRTPOPRTSTACK(&al_emlrtRSI);
  468       EMLRTPOPRTSTACK(&sk_emlrtRSI);
  469       /*  clip t between 0 and 1 */
  470       i19 = 0;
  471       while (i19 <= 0) {
  472           A_data[0] = Y_data[0];
  473           i19 = 1;
  474       }
  475       t_data[0] = muDoubleScalarMax(A_data[0], 0.0);
  476       A_data[0] = t_data[0];
  477       t_sizes = 1;
  478       t_data[0] = muDoubleScalarMin(A_data[0], 1.0);
  479       /*  if isfield(spline,'tm') */
  480       /*      if t<spline.tm; */
  481       /*          spline.coefs = spline.coefs(:,1); */
  482       /*      else */
  483       /*          spline.coefs = spline.coefs(:,2); */
  484       /*      end */
  485       /*  end */
  486       for (i19 = 0; i19 < 3; i19++) {
  487           x[i19] = 0.0;
  488       }
  489       /*  [pos,vel,acc] */
  490       for (i = 0; i < 4; i++) {
  491           EMLRTPUSHRTSTACK(&tk_emlrtRSI);
  492           b_mpower(t_data, *(int32_T (*)[1])&t_sizes, (1.0 + (real_T)i) - 1.0, A_data, *(int32_T (*)[1])&ii);
  493           EMLRTPOPRTSTACK(&tk_emlrtRSI);
  494           x[0] += A_data[0] * spline_coefs[(n_data[0] + (i << 1)) - 1];
  495           if (1 + i > 1) {
  496               EMLRTPUSHRTSTACK(&uk_emlrtRSI);
  497               b_mpower(t_data, *(int32_T (*)[1])&t_sizes, (1.0 + (real_T)i) - 2.0, A_data, *(int32_T (*)[1])&ii);
  498               EMLRTPOPRTSTACK(&uk_emlrtRSI);
  499               x[1] += ((1.0 + (real_T)i) - 1.0) * A_data[0] * spline_coefs[(n_data[0] + (i << 1)) - 1];
  500           }
  501           if (1 + i > 2) {
  502               EMLRTPUSHRTSTACK(&vk_emlrtRSI);
  503               b_mpower(t_data, *(int32_T (*)[1])&t_sizes, (1.0 + (real_T)i) - 3.0, A_data, *(int32_T (*)[1])&ii);
  504               EMLRTPOPRTSTACK(&vk_emlrtRSI);
  505               x[2] += (real_T)((i - 1) * i) * A_data[0] * spline_coefs[(n_data[0] + (i << 1)) - 1];
  506           }
  507           emlrtBreakCheck();
  508       }
  509       /*  scale with duration */
  510       EMLRTPUSHRTSTACK(&wk_emlrtRSI);
  511       spline_duration_sizes[0] = 1;
  512       spline_duration_sizes[1] = 1;
  513       ii = ii_sizes - 1;
  514       i19 = 0;
  515       while (i19 <= ii) {
  516           A_data[0] = spline_duration[n_data[0] - 1];
  517           i19 = 1;
  518       }
  519       x[1] = d_mrdivide(x[1], A_data, spline_duration_sizes);
  520       EMLRTPOPRTSTACK(&wk_emlrtRSI);
  521       EMLRTPUSHRTSTACK(&xk_emlrtRSI);
  522       ii = ii_sizes - 1;
  523       i19 = 0;
  524       while (i19 <= ii) {
  525           Y_data[0] = spline_duration[n_data[0] - 1];
  526           i19 = 1;
  527       }
  528       for (i19 = 0; i19 < 2; i19++) {
  529           spline_duration_sizes[i19] = 1;
  530       }
  531       e = 2.0;
  532       firstmult = TRUE;
  533       do {
  534           exitg1 = 0U;
  535           ed2 = muDoubleScalarFloor(e / 2.0);
  536           if (2.0 * ed2 != e) {
  537               if (firstmult) {
  538                   t_data[0] = Y_data[0];
  539                   spline_duration_sizes[0] = 1;
  540                   spline_duration_sizes[1] = 1;
  541                   i19 = 0;
  542                   while (i19 <= 0) {
  543                       i19 = 0;
  544                       while (i19 <= 0) {
  545                           A_data[0] = t_data[0];
  546                           i19 = 1;
  547                       }
  548                       i19 = 1;
  549                   }
  550                   firstmult = FALSE;
  551               } else {
  552                   spline_duration_sizes[0] = 1;
  553                   spline_duration_sizes[1] = 1;
  554                   A_data[0] *= Y_data[0];
  555               }
  556           }
  557           if (ed2 == 0.0) {
  558               exitg1 = 1U;
  559           } else {
  560               e = ed2;
  561               Y_data[0] *= Y_data[0];
  562           }
  563       } while (exitg1 == 0U);
  564       x[2] = d_mrdivide(x[2], A_data, spline_duration_sizes);
  565       EMLRTPOPRTSTACK(&xk_emlrtRSI);
  566   }
  567   /* End of code generation (EvalSpline.c) */
  568