File: D:\TMTmethod\simulator\codegen\mex\testspline\EvalSpline.c1 /* 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 |