Esempio n. 1
0
/*
 * Arguments    : emxArray_real_T *x
 * Return Type  : void
 */
void d_sort(emxArray_real_T *x)
{
  emxArray_real_T *vwork;
  int i9;
  int k;
  int i10;
  emxArray_int32_T *b_vwork;
  emxInit_real_T2(&vwork, 1);
  i9 = x->size[1];
  k = x->size[1];
  i10 = vwork->size[0];
  vwork->size[0] = k;
  emxEnsureCapacity((emxArray__common *)vwork, i10, (int)sizeof(double));
  for (k = 0; k + 1 <= i9; k++) {
    vwork->data[k] = x->data[k];
  }

  emxInit_int32_T(&b_vwork, 1);
  sortIdx(vwork, b_vwork);
  k = 0;
  emxFree_int32_T(&b_vwork);
  while (k + 1 <= i9) {
    x->data[k] = vwork->data[k];
    k++;
  }

  emxFree_real_T(&vwork);
}
Esempio n. 2
0
/*
 * Arguments    : double x_data[]
 *                int x_size[1]
 * Return Type  : void
 */
void sort(double x_data[], int x_size[1])
{
  int dim;
  int i6;
  double vwork_data[252];
  int vwork_size_idx_0;
  int vstride;
  int k;
  int j;
  emxArray_real_T *vwork;
  emxArray_int32_T *b_vwork;
  dim = nonSingletonDim(x_size);
  if (dim <= 1) {
    i6 = x_size[0];
  } else {
    i6 = 1;
  }

  vwork_size_idx_0 = (unsigned char)i6;
  vstride = 1;
  k = 1;
  while (k <= dim - 1) {
    vstride *= x_size[0];
    k = 2;
  }

  j = 0;
  emxInit_real_T2(&vwork, 1);
  emxInit_int32_T(&b_vwork, 1);
  while (j + 1 <= vstride) {
    for (k = 0; k + 1 <= i6; k++) {
      vwork_data[k] = x_data[j + k * vstride];
    }

    dim = vwork->size[0];
    vwork->size[0] = vwork_size_idx_0;
    emxEnsureCapacity((emxArray__common *)vwork, dim, (int)sizeof(double));
    for (dim = 0; dim < vwork_size_idx_0; dim++) {
      vwork->data[dim] = vwork_data[dim];
    }

    sortIdx(vwork, b_vwork);
    vwork_size_idx_0 = vwork->size[0];
    k = vwork->size[0];
    for (dim = 0; dim < k; dim++) {
      vwork_data[dim] = vwork->data[dim];
    }

    for (k = 0; k + 1 <= i6; k++) {
      x_data[j + k * vstride] = vwork->data[k];
    }

    j++;
  }

  emxFree_int32_T(&b_vwork);
  emxFree_real_T(&vwork);
}
/* Function Definitions */
void compmat(const emlrtStack *sp, const emxArray_uint8_T *x, real_T dims,
             emxArray_real_T *y)
{
  int32_T i1;
  real_T d3;
  int32_T ii;
  int32_T i;
  emxArray_boolean_T *b_x;
  emxArray_int32_T *b_ii;
  int32_T nx;
  int32_T idx;
  boolean_T overflow;
  boolean_T exitg1;
  boolean_T guard1 = false;
  emlrtStack st;
  emlrtStack b_st;
  emlrtStack c_st;
  emlrtStack d_st;
  st.prev = sp;
  st.tls = sp->tls;
  b_st.prev = &st;
  b_st.tls = st.tls;
  c_st.prev = &b_st;
  c_st.tls = b_st.tls;
  d_st.prev = &c_st;
  d_st.tls = c_st.tls;
  emlrtHeapReferenceStackEnterFcnR2012b(sp);

  /* UNTITLED Summary of this function goes here */
  /*    Detailed explanation goes here */
  i1 = y->size[0] * y->size[1];
  y->size[0] = 1;
  if (!(dims >= 0.0)) {
    emlrtNonNegativeCheckR2012b(dims, (emlrtDCInfo *)&j_emlrtDCI, sp);
  }

  d3 = dims;
  if (d3 != (int32_T)muDoubleScalarFloor(d3)) {
    emlrtIntegerCheckR2012b(d3, (emlrtDCInfo *)&i_emlrtDCI, sp);
  }

  y->size[1] = (int32_T)d3;
  emxEnsureCapacity(sp, (emxArray__common *)y, i1, (int32_T)sizeof(real_T),
                    &f_emlrtRTEI);
  if (!(dims >= 0.0)) {
    emlrtNonNegativeCheckR2012b(dims, (emlrtDCInfo *)&j_emlrtDCI, sp);
  }

  if (d3 != (int32_T)muDoubleScalarFloor(d3)) {
    emlrtIntegerCheckR2012b(d3, (emlrtDCInfo *)&i_emlrtDCI, sp);
  }

  ii = (int32_T)d3;
  for (i1 = 0; i1 < ii; i1++) {
    y->data[i1] = 0.0;
  }

  emlrtForLoopVectorCheckR2012b(1.0, 1.0, dims, mxDOUBLE_CLASS, (int32_T)dims,
    (emlrtRTEInfo *)&n_emlrtRTEI, sp);
  i = 0;
  emxInit_boolean_T(sp, &b_x, 2, &f_emlrtRTEI, true);
  emxInit_int32_T(sp, &b_ii, 2, &g_emlrtRTEI, true);
  while (i <= (int32_T)dims - 1) {
    st.site = &k_emlrtRSI;
    i1 = b_x->size[0] * b_x->size[1];
    b_x->size[0] = 1;
    b_x->size[1] = x->size[1];
    emxEnsureCapacity(&st, (emxArray__common *)b_x, i1, (int32_T)sizeof
                      (boolean_T), &f_emlrtRTEI);
    ii = x->size[0] * x->size[1];
    for (i1 = 0; i1 < ii; i1++) {
      b_x->data[i1] = (x->data[i1] == 1.0 + (real_T)i);
    }

    b_st.site = &h_emlrtRSI;
    nx = b_x->size[1];
    idx = 0;
    i1 = b_ii->size[0] * b_ii->size[1];
    b_ii->size[0] = 1;
    b_ii->size[1] = b_x->size[1];
    emxEnsureCapacity(&b_st, (emxArray__common *)b_ii, i1, (int32_T)sizeof
                      (int32_T), &f_emlrtRTEI);
    c_st.site = &i_emlrtRSI;
    overflow = ((!(1 > b_x->size[1])) && (b_x->size[1] > 2147483646));
    if (overflow) {
      d_st.site = &j_emlrtRSI;
      check_forloop_overflow_error(&d_st);
    }

    ii = 1;
    exitg1 = false;
    while ((!exitg1) && (ii <= nx)) {
      guard1 = false;
      if (b_x->data[ii - 1]) {
        idx++;
        b_ii->data[idx - 1] = ii;
        if (idx >= nx) {
          exitg1 = true;
        } else {
          guard1 = true;
        }
      } else {
        guard1 = true;
      }

      if (guard1) {
        ii++;
      }
    }

    if (idx <= b_x->size[1]) {
    } else {
      emlrtErrorWithMessageIdR2012b(&b_st, &k_emlrtRTEI,
        "Coder:builtins:AssertionFailed", 0);
    }

    if (b_x->size[1] == 1) {
      if (idx == 0) {
        i1 = b_ii->size[0] * b_ii->size[1];
        b_ii->size[0] = 1;
        b_ii->size[1] = 0;
        emxEnsureCapacity(&b_st, (emxArray__common *)b_ii, i1, (int32_T)sizeof
                          (int32_T), &f_emlrtRTEI);
      }
    } else {
      i1 = b_ii->size[0] * b_ii->size[1];
      if (1 > idx) {
        b_ii->size[1] = 0;
      } else {
        b_ii->size[1] = idx;
      }

      emxEnsureCapacity(&b_st, (emxArray__common *)b_ii, i1, (int32_T)sizeof
                        (int32_T), &b_emlrtRTEI);
    }

    i1 = y->size[1];
    if (!((i + 1 >= 1) && (i + 1 <= i1))) {
      emlrtDynamicBoundsCheckR2012b(i + 1, 1, i1, (emlrtBCInfo *)&w_emlrtBCI, sp);
    }

    y->data[i] = b_ii->size[1];
    i++;
    if (*emlrtBreakCheckR2012bFlagVar != 0) {
      emlrtBreakCheckR2012b(sp);
    }
  }

  emxFree_int32_T(&b_ii);
  emxFree_boolean_T(&b_x);
  emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
Esempio n. 4
0
/* Function Definitions */
static void b_eml_lusolve(const emlrtStack *sp, const emxArray_real_T *A,
  emxArray_real_T *B)
{
  emxArray_real_T *b_A;
  int32_T i58;
  int32_T iy;
  emxArray_int32_T *ipiv;
  int32_T info;
  int32_T i59;
  int32_T b;
  int32_T j;
  int32_T mmj;
  int32_T c;
  ptrdiff_t n_t;
  ptrdiff_t incx_t;
  double * xix0_t;
  int32_T ix;
  boolean_T overflow;
  int32_T k;
  real_T temp;
  int32_T i60;
  boolean_T b_c;
  ptrdiff_t m_t;
  ptrdiff_t incy_t;
  ptrdiff_t lda_t;
  double * alpha1_t;
  double * Aia0_t;
  double * Aiy0_t;
  char_T DIAGA;
  char_T TRANSA;
  char_T UPLO;
  char_T SIDE;
  emlrtStack st;
  emlrtStack b_st;
  emlrtStack c_st;
  emlrtStack d_st;
  emlrtStack e_st;
  emlrtStack f_st;
  emlrtStack g_st;
  emlrtStack h_st;
  emlrtStack i_st;
  st.prev = sp;
  st.tls = sp->tls;
  b_st.prev = &st;
  b_st.tls = st.tls;
  c_st.prev = &b_st;
  c_st.tls = b_st.tls;
  d_st.prev = &c_st;
  d_st.tls = c_st.tls;
  e_st.prev = &d_st;
  e_st.tls = d_st.tls;
  f_st.prev = &e_st;
  f_st.tls = e_st.tls;
  g_st.prev = &f_st;
  g_st.tls = f_st.tls;
  h_st.prev = &g_st;
  h_st.tls = g_st.tls;
  i_st.prev = &h_st;
  i_st.tls = h_st.tls;
  emlrtHeapReferenceStackEnterFcnR2012b(sp);
  emxInit_real_T(sp, &b_A, 2, &ob_emlrtRTEI, true);
  st.site = &ib_emlrtRSI;
  b_st.site = &lb_emlrtRSI;
  c_st.site = &nb_emlrtRSI;
  d_st.site = &ob_emlrtRSI;
  i58 = b_A->size[0] * b_A->size[1];
  b_A->size[0] = A->size[0];
  b_A->size[1] = A->size[1];
  emxEnsureCapacity(&d_st, (emxArray__common *)b_A, i58, (int32_T)sizeof(real_T),
                    &ob_emlrtRTEI);
  iy = A->size[0] * A->size[1];
  for (i58 = 0; i58 < iy; i58++) {
    b_A->data[i58] = A->data[i58];
  }

  b_emxInit_int32_T(&d_st, &ipiv, 2, &ob_emlrtRTEI, true);
  e_st.site = &qb_emlrtRSI;
  f_st.site = &rb_emlrtRSI;
  g_st.site = &sb_emlrtRSI;
  h_st.site = &tb_emlrtRSI;
  eml_signed_integer_colon(&h_st, muIntScalarMin_sint32(A->size[1], A->size[1]),
    ipiv);
  info = 0;
  if (A->size[1] < 1) {
  } else {
    i59 = A->size[1] - 1;
    b = muIntScalarMin_sint32(i59, A->size[1]);
    e_st.site = &pb_emlrtRSI;
    for (j = 1; j <= b; j++) {
      mmj = A->size[1] - j;
      c = (j - 1) * (A->size[1] + 1) + 1;
      e_st.site = &if_emlrtRSI;
      f_st.site = &yb_emlrtRSI;
      if (mmj + 1 < 1) {
        iy = -1;
      } else {
        g_st.site = &ac_emlrtRSI;
        h_st.site = &ac_emlrtRSI;
        n_t = (ptrdiff_t)(mmj + 1);
        h_st.site = &ac_emlrtRSI;
        incx_t = (ptrdiff_t)(1);
        i58 = b_A->size[0] * b_A->size[1];
        xix0_t = (double *)(&b_A->data[emlrtDynamicBoundsCheckFastR2012b(c, 1,
          i58, &je_emlrtBCI, &g_st) - 1]);
        incx_t = idamax(&n_t, xix0_t, &incx_t);
        iy = (int32_T)incx_t - 1;
      }

      if (b_A->data[(c + iy) - 1] != 0.0) {
        if (iy != 0) {
          ipiv->data[j - 1] = j + iy;
          e_st.site = &jf_emlrtRSI;
          f_st.site = &bc_emlrtRSI;
          g_st.site = &cc_emlrtRSI;
          ix = j;
          iy += j;
          h_st.site = &dc_emlrtRSI;
          overflow = (A->size[1] > 2147483646);
          if (overflow) {
            i_st.site = &db_emlrtRSI;
            check_forloop_overflow_error(&i_st);
          }

          for (k = 1; k <= A->size[1]; k++) {
            i58 = b_A->size[0] * b_A->size[1];
            temp = b_A->data[emlrtDynamicBoundsCheckFastR2012b(ix, 1, i58,
              &le_emlrtBCI, &g_st) - 1];
            i58 = b_A->size[0] * b_A->size[1];
            i60 = b_A->size[0] * b_A->size[1];
            b_A->data[emlrtDynamicBoundsCheckFastR2012b(ix, 1, i58, &le_emlrtBCI,
              &g_st) - 1] = b_A->data[emlrtDynamicBoundsCheckFastR2012b(iy, 1,
              i60, &le_emlrtBCI, &g_st) - 1];
            i58 = b_A->size[0] * b_A->size[1];
            b_A->data[emlrtDynamicBoundsCheckFastR2012b(iy, 1, i58, &le_emlrtBCI,
              &g_st) - 1] = temp;
            ix += A->size[1];
            iy += A->size[1];
          }
        }

        iy = c + mmj;
        e_st.site = &kf_emlrtRSI;
        if (c + 1 > iy) {
          b_c = false;
        } else {
          b_c = (iy > 2147483646);
        }

        if (b_c) {
          f_st.site = &db_emlrtRSI;
          check_forloop_overflow_error(&f_st);
        }

        for (k = c; k + 1 <= iy; k++) {
          b_A->data[k] /= b_A->data[c - 1];
        }
      } else {
        info = j;
      }

      iy = A->size[1] - j;
      e_st.site = &lf_emlrtRSI;
      f_st.site = &ec_emlrtRSI;
      g_st.site = &fc_emlrtRSI;
      if ((mmj < 1) || (iy < 1)) {
      } else {
        h_st.site = &gc_emlrtRSI;
        temp = -1.0;
        m_t = (ptrdiff_t)(mmj);
        n_t = (ptrdiff_t)(iy);
        incx_t = (ptrdiff_t)(1);
        incy_t = (ptrdiff_t)(A->size[1]);
        lda_t = (ptrdiff_t)(A->size[1]);
        alpha1_t = (double *)(&temp);
        i58 = b_A->size[0] * b_A->size[1];
        i60 = (c + A->size[1]) + 1;
        Aia0_t = (double *)(&b_A->data[emlrtDynamicBoundsCheckFastR2012b(i60, 1,
          i58, &ke_emlrtBCI, &h_st) - 1]);
        i58 = b_A->size[0] * b_A->size[1];
        xix0_t = (double *)(&b_A->data[emlrtDynamicBoundsCheckFastR2012b(c + 1,
          1, i58, &ke_emlrtBCI, &h_st) - 1]);
        i58 = b_A->size[0] * b_A->size[1];
        i60 = c + A->size[1];
        Aiy0_t = (double *)(&b_A->data[emlrtDynamicBoundsCheckFastR2012b(i60, 1,
          i58, &ke_emlrtBCI, &h_st) - 1]);
        dger(&m_t, &n_t, alpha1_t, xix0_t, &incx_t, Aiy0_t, &incy_t, Aia0_t,
             &lda_t);
      }
    }

    if ((info == 0) && (!(b_A->data[(A->size[1] + b_A->size[0] * (A->size[1] - 1))
                          - 1] != 0.0))) {
      info = A->size[1];
    }
  }

  if (info > 0) {
    b_st.site = &mb_emlrtRSI;
    warn_singular(&b_st);
  }

  b_st.site = &yf_emlrtRSI;
  for (iy = 0; iy + 1 < A->size[1]; iy++) {
    if (ipiv->data[iy] != iy + 1) {
      temp = B->data[iy];
      B->data[iy] = B->data[ipiv->data[iy] - 1];
      B->data[ipiv->data[iy] - 1] = temp;
    }
  }

  emxFree_int32_T(&ipiv);
  b_st.site = &ag_emlrtRSI;
  c_st.site = &ic_emlrtRSI;
  if (A->size[1] < 1) {
  } else {
    d_st.site = &jc_emlrtRSI;
    temp = 1.0;
    DIAGA = 'U';
    TRANSA = 'N';
    UPLO = 'L';
    SIDE = 'L';
    e_st.site = &jc_emlrtRSI;
    m_t = (ptrdiff_t)(A->size[1]);
    e_st.site = &jc_emlrtRSI;
    n_t = (ptrdiff_t)(1);
    e_st.site = &jc_emlrtRSI;
    lda_t = (ptrdiff_t)(A->size[1]);
    e_st.site = &jc_emlrtRSI;
    incx_t = (ptrdiff_t)(A->size[1]);
    i58 = b_A->size[0] * b_A->size[1];
    emlrtDynamicBoundsCheckFastR2012b(1, 1, i58, &ie_emlrtBCI, &d_st);
    Aia0_t = (double *)(&b_A->data[0]);
    xix0_t = (double *)(&B->data[0]);
    alpha1_t = (double *)(&temp);
    dtrsm(&SIDE, &UPLO, &TRANSA, &DIAGA, &m_t, &n_t, alpha1_t, Aia0_t, &lda_t,
          xix0_t, &incx_t);
  }

  b_st.site = &bg_emlrtRSI;
  c_st.site = &ic_emlrtRSI;
  if (A->size[1] < 1) {
  } else {
    d_st.site = &jc_emlrtRSI;
    temp = 1.0;
    DIAGA = 'N';
    TRANSA = 'N';
    UPLO = 'U';
    SIDE = 'L';
    e_st.site = &jc_emlrtRSI;
    m_t = (ptrdiff_t)(A->size[1]);
    e_st.site = &jc_emlrtRSI;
    n_t = (ptrdiff_t)(1);
    e_st.site = &jc_emlrtRSI;
    lda_t = (ptrdiff_t)(A->size[1]);
    e_st.site = &jc_emlrtRSI;
    incx_t = (ptrdiff_t)(A->size[1]);
    i58 = b_A->size[0] * b_A->size[1];
    emlrtDynamicBoundsCheckFastR2012b(1, 1, i58, &ie_emlrtBCI, &d_st);
    Aia0_t = (double *)(&b_A->data[0]);
    xix0_t = (double *)(&B->data[0]);
    alpha1_t = (double *)(&temp);
    dtrsm(&SIDE, &UPLO, &TRANSA, &DIAGA, &m_t, &n_t, alpha1_t, Aia0_t, &lda_t,
          xix0_t, &incx_t);
  }

  emxFree_real_T(&b_A);
  emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
Esempio n. 5
0
static void c_eml_qrsolve(const emlrtStack *sp, const emxArray_real_T *A,
  emxArray_real_T *B, emxArray_real_T *Y)
{
  emxArray_real_T *b_A;
  emxArray_real_T *work;
  int32_T mn;
  int32_T i51;
  int32_T ix;
  emxArray_real_T *tau;
  emxArray_int32_T *jpvt;
  int32_T m;
  int32_T n;
  int32_T b_mn;
  emxArray_real_T *vn1;
  emxArray_real_T *vn2;
  int32_T k;
  boolean_T overflow;
  boolean_T b12;
  int32_T i;
  int32_T i_i;
  int32_T nmi;
  int32_T mmi;
  int32_T pvt;
  int32_T iy;
  boolean_T b13;
  real_T xnorm;
  int32_T i52;
  real_T atmp;
  real_T d16;
  boolean_T b14;
  boolean_T b_i;
  ptrdiff_t n_t;
  ptrdiff_t incx_t;
  double * xix0_t;
  boolean_T exitg1;
  const mxArray *y;
  static const int32_T iv78[2] = { 1, 8 };

  const mxArray *m14;
  char_T cv76[8];
  static const char_T cv77[8] = { '%', '%', '%', 'd', '.', '%', 'd', 'e' };

  char_T cv78[14];
  uint32_T unnamed_idx_0;
  emlrtStack st;
  emlrtStack b_st;
  emlrtStack c_st;
  emlrtStack d_st;
  emlrtStack e_st;
  emlrtStack f_st;
  emlrtStack g_st;
  emlrtStack h_st;
  st.prev = sp;
  st.tls = sp->tls;
  b_st.prev = &st;
  b_st.tls = st.tls;
  c_st.prev = &b_st;
  c_st.tls = b_st.tls;
  d_st.prev = &c_st;
  d_st.tls = c_st.tls;
  e_st.prev = &d_st;
  e_st.tls = d_st.tls;
  f_st.prev = &e_st;
  f_st.tls = e_st.tls;
  g_st.prev = &f_st;
  g_st.tls = f_st.tls;
  h_st.prev = &g_st;
  h_st.tls = g_st.tls;
  emlrtHeapReferenceStackEnterFcnR2012b(sp);
  emxInit_real_T(sp, &b_A, 2, &m_emlrtRTEI, true);
  b_emxInit_real_T(sp, &work, 1, &rb_emlrtRTEI, true);
  mn = (int32_T)muDoubleScalarMin(A->size[0], A->size[1]);
  st.site = &mc_emlrtRSI;
  b_st.site = &nc_emlrtRSI;
  c_st.site = &oc_emlrtRSI;
  i51 = b_A->size[0] * b_A->size[1];
  b_A->size[0] = A->size[0];
  b_A->size[1] = A->size[1];
  emxEnsureCapacity(&c_st, (emxArray__common *)b_A, i51, (int32_T)sizeof(real_T),
                    &m_emlrtRTEI);
  ix = A->size[0] * A->size[1];
  for (i51 = 0; i51 < ix; i51++) {
    b_A->data[i51] = A->data[i51];
  }

  b_emxInit_real_T(&c_st, &tau, 1, &m_emlrtRTEI, true);
  b_emxInit_int32_T(&c_st, &jpvt, 2, &m_emlrtRTEI, true);
  m = b_A->size[0];
  n = b_A->size[1];
  b_mn = muIntScalarMin_sint32(b_A->size[0], b_A->size[1]);
  i51 = tau->size[0];
  tau->size[0] = b_mn;
  emxEnsureCapacity(&c_st, (emxArray__common *)tau, i51, (int32_T)sizeof(real_T),
                    &n_emlrtRTEI);
  d_st.site = &mf_emlrtRSI;
  e_st.site = &rb_emlrtRSI;
  f_st.site = &sb_emlrtRSI;
  g_st.site = &tb_emlrtRSI;
  eml_signed_integer_colon(&g_st, b_A->size[1], jpvt);
  if ((b_A->size[0] == 0) || (b_A->size[1] == 0)) {
  } else {
    ix = b_A->size[1];
    i51 = work->size[0];
    work->size[0] = ix;
    emxEnsureCapacity(&c_st, (emxArray__common *)work, i51, (int32_T)sizeof
                      (real_T), &m_emlrtRTEI);
    for (i51 = 0; i51 < ix; i51++) {
      work->data[i51] = 0.0;
    }

    b_emxInit_real_T(&c_st, &vn1, 1, &pb_emlrtRTEI, true);
    b_emxInit_real_T(&c_st, &vn2, 1, &qb_emlrtRTEI, true);
    d_st.site = &tc_emlrtRSI;
    ix = b_A->size[1];
    i51 = vn1->size[0];
    vn1->size[0] = ix;
    emxEnsureCapacity(&c_st, (emxArray__common *)vn1, i51, (int32_T)sizeof
                      (real_T), &pb_emlrtRTEI);
    i51 = vn2->size[0];
    vn2->size[0] = ix;
    emxEnsureCapacity(&c_st, (emxArray__common *)vn2, i51, (int32_T)sizeof
                      (real_T), &qb_emlrtRTEI);
    k = 1;
    d_st.site = &nf_emlrtRSI;
    overflow = (b_A->size[1] > 2147483646);
    if (overflow) {
      e_st.site = &db_emlrtRSI;
      check_forloop_overflow_error(&e_st);
    }

    for (ix = 0; ix + 1 <= b_A->size[1]; ix++) {
      d_st.site = &sc_emlrtRSI;
      vn1->data[ix] = b_eml_xnrm2(&d_st, b_A->size[0], b_A, k);
      vn2->data[ix] = vn1->data[ix];
      k += b_A->size[0];
    }

    d_st.site = &rc_emlrtRSI;
    if (1 > b_mn) {
      b12 = false;
    } else {
      b12 = (b_mn > 2147483646);
    }

    if (b12) {
      e_st.site = &db_emlrtRSI;
      check_forloop_overflow_error(&e_st);
    }

    for (i = 1; i <= b_mn; i++) {
      i_i = (i + (i - 1) * m) - 1;
      nmi = n - i;
      mmi = m - i;
      d_st.site = &of_emlrtRSI;
      ix = eml_ixamax(&d_st, 1 + nmi, vn1, i);
      pvt = (i + ix) - 2;
      if (pvt + 1 != i) {
        d_st.site = &pf_emlrtRSI;
        e_st.site = &bc_emlrtRSI;
        f_st.site = &cc_emlrtRSI;
        ix = 1 + m * pvt;
        iy = 1 + m * (i - 1);
        g_st.site = &dc_emlrtRSI;
        if (1 > m) {
          b13 = false;
        } else {
          b13 = (m > 2147483646);
        }

        if (b13) {
          h_st.site = &db_emlrtRSI;
          check_forloop_overflow_error(&h_st);
        }

        for (k = 1; k <= m; k++) {
          i51 = b_A->size[0] * b_A->size[1];
          xnorm = b_A->data[emlrtDynamicBoundsCheckFastR2012b(ix, 1, i51,
            &le_emlrtBCI, &f_st) - 1];
          i51 = b_A->size[0] * b_A->size[1];
          i52 = b_A->size[0] * b_A->size[1];
          b_A->data[emlrtDynamicBoundsCheckFastR2012b(ix, 1, i51, &le_emlrtBCI,
            &f_st) - 1] = b_A->data[emlrtDynamicBoundsCheckFastR2012b(iy, 1, i52,
            &le_emlrtBCI, &f_st) - 1];
          i51 = b_A->size[0] * b_A->size[1];
          b_A->data[emlrtDynamicBoundsCheckFastR2012b(iy, 1, i51, &le_emlrtBCI,
            &f_st) - 1] = xnorm;
          ix++;
          iy++;
        }

        ix = jpvt->data[pvt];
        jpvt->data[pvt] = jpvt->data[i - 1];
        jpvt->data[i - 1] = ix;
        vn1->data[pvt] = vn1->data[i - 1];
        vn2->data[pvt] = vn2->data[i - 1];
      }

      if (i < m) {
        d_st.site = &qc_emlrtRSI;
        atmp = b_A->data[i_i];
        d16 = 0.0;
        if (1 + mmi <= 0) {
        } else {
          e_st.site = &wc_emlrtRSI;
          xnorm = b_eml_xnrm2(&e_st, mmi, b_A, i_i + 2);
          if (xnorm != 0.0) {
            xnorm = muDoubleScalarHypot(b_A->data[i_i], xnorm);
            if (b_A->data[i_i] >= 0.0) {
              xnorm = -xnorm;
            }

            if (muDoubleScalarAbs(xnorm) < 1.0020841800044864E-292) {
              ix = 0;
              do {
                ix++;
                e_st.site = &xc_emlrtRSI;
                b_eml_xscal(&e_st, mmi, 9.9792015476736E+291, b_A, i_i + 2);
                xnorm *= 9.9792015476736E+291;
                atmp *= 9.9792015476736E+291;
              } while (!(muDoubleScalarAbs(xnorm) >= 1.0020841800044864E-292));

              e_st.site = &yc_emlrtRSI;
              xnorm = b_eml_xnrm2(&e_st, mmi, b_A, i_i + 2);
              xnorm = muDoubleScalarHypot(atmp, xnorm);
              if (atmp >= 0.0) {
                xnorm = -xnorm;
              }

              d16 = (xnorm - atmp) / xnorm;
              e_st.site = &ad_emlrtRSI;
              b_eml_xscal(&e_st, mmi, 1.0 / (atmp - xnorm), b_A, i_i + 2);
              e_st.site = &bd_emlrtRSI;
              if (1 > ix) {
                b14 = false;
              } else {
                b14 = (ix > 2147483646);
              }

              if (b14) {
                f_st.site = &db_emlrtRSI;
                check_forloop_overflow_error(&f_st);
              }

              for (k = 1; k <= ix; k++) {
                xnorm *= 1.0020841800044864E-292;
              }

              atmp = xnorm;
            } else {
              d16 = (xnorm - b_A->data[i_i]) / xnorm;
              atmp = 1.0 / (b_A->data[i_i] - xnorm);
              e_st.site = &cd_emlrtRSI;
              b_eml_xscal(&e_st, mmi, atmp, b_A, i_i + 2);
              atmp = xnorm;
            }
          }
        }

        tau->data[i - 1] = d16;
      } else {
        atmp = b_A->data[i_i];
        d_st.site = &pc_emlrtRSI;
        tau->data[i - 1] = eml_matlab_zlarfg();
      }

      b_A->data[i_i] = atmp;
      if (i < n) {
        atmp = b_A->data[i_i];
        b_A->data[i_i] = 1.0;
        d_st.site = &qf_emlrtRSI;
        eml_matlab_zlarf(&d_st, mmi + 1, nmi, i_i + 1, tau->data[i - 1], b_A, i
                         + i * m, m, work);
        b_A->data[i_i] = atmp;
      }

      d_st.site = &rf_emlrtRSI;
      if (i + 1 > n) {
        b_i = false;
      } else {
        b_i = (n > 2147483646);
      }

      if (b_i) {
        e_st.site = &db_emlrtRSI;
        check_forloop_overflow_error(&e_st);
      }

      for (ix = i; ix + 1 <= n; ix++) {
        if (vn1->data[ix] != 0.0) {
          xnorm = muDoubleScalarAbs(b_A->data[(i + b_A->size[0] * ix) - 1]) /
            vn1->data[ix];
          xnorm = 1.0 - xnorm * xnorm;
          if (xnorm < 0.0) {
            xnorm = 0.0;
          }

          atmp = vn1->data[ix] / vn2->data[ix];
          atmp = xnorm * (atmp * atmp);
          if (atmp <= 1.4901161193847656E-8) {
            if (i < m) {
              d_st.site = &sf_emlrtRSI;
              e_st.site = &uc_emlrtRSI;
              if (mmi < 1) {
                xnorm = 0.0;
              } else {
                f_st.site = &vc_emlrtRSI;
                g_st.site = &vc_emlrtRSI;
                n_t = (ptrdiff_t)(mmi);
                g_st.site = &vc_emlrtRSI;
                incx_t = (ptrdiff_t)(1);
                i51 = b_A->size[0] * b_A->size[1];
                i52 = (i + m * ix) + 1;
                xix0_t = (double *)(&b_A->data[emlrtDynamicBoundsCheckFastR2012b
                                    (i52, 1, i51, &vb_emlrtBCI, &f_st) - 1]);
                xnorm = dnrm2(&n_t, xix0_t, &incx_t);
              }

              vn1->data[ix] = xnorm;
              vn2->data[ix] = vn1->data[ix];
            } else {
              vn1->data[ix] = 0.0;
              vn2->data[ix] = 0.0;
            }
          } else {
            d_st.site = &tf_emlrtRSI;
            vn1->data[ix] *= muDoubleScalarSqrt(xnorm);
          }
        }
      }
    }

    emxFree_real_T(&vn2);
    emxFree_real_T(&vn1);
  }

  atmp = 0.0;
  if (mn > 0) {
    xnorm = muDoubleScalarMax(A->size[0], A->size[1]) * muDoubleScalarAbs
      (b_A->data[0]) * 2.2204460492503131E-16;
    k = 0;
    exitg1 = false;
    while ((!exitg1) && (k <= mn - 1)) {
      if (muDoubleScalarAbs(b_A->data[k + b_A->size[0] * k]) <= xnorm) {
        st.site = &lc_emlrtRSI;
        y = NULL;
        m14 = emlrtCreateCharArray(2, iv78);
        for (i = 0; i < 8; i++) {
          cv76[i] = cv77[i];
        }

        emlrtInitCharArrayR2013a(&st, 8, m14, cv76);
        emlrtAssign(&y, m14);
        b_st.site = &tg_emlrtRSI;
        emlrt_marshallIn(&b_st, c_sprintf(&b_st, b_sprintf(&b_st, y,
          emlrt_marshallOut(14.0), emlrt_marshallOut(6.0), &o_emlrtMCI),
          emlrt_marshallOut(xnorm), &p_emlrtMCI), "sprintf", cv78);
        st.site = &kc_emlrtRSI;
        b_eml_warning(&st, atmp, cv78);
        exitg1 = true;
      } else {
        atmp++;
        k++;
      }
    }
  }

  unnamed_idx_0 = (uint32_T)A->size[1];
  i51 = Y->size[0];
  Y->size[0] = (int32_T)unnamed_idx_0;
  emxEnsureCapacity(sp, (emxArray__common *)Y, i51, (int32_T)sizeof(real_T),
                    &m_emlrtRTEI);
  ix = (int32_T)unnamed_idx_0;
  for (i51 = 0; i51 < ix; i51++) {
    Y->data[i51] = 0.0;
  }

  for (ix = 0; ix < mn; ix++) {
    if (tau->data[ix] != 0.0) {
      xnorm = B->data[ix];
      i51 = A->size[0] + (int32_T)(1.0 - ((1.0 + (real_T)ix) + 1.0));
      emlrtForLoopVectorCheckR2012b((1.0 + (real_T)ix) + 1.0, 1.0, A->size[0],
        mxDOUBLE_CLASS, i51, &ac_emlrtRTEI, sp);
      for (i = 0; i < i51; i++) {
        unnamed_idx_0 = ((uint32_T)ix + i) + 2U;
        xnorm += b_A->data[((int32_T)unnamed_idx_0 + b_A->size[0] * ix) - 1] *
          B->data[(int32_T)unnamed_idx_0 - 1];
      }

      xnorm *= tau->data[ix];
      if (xnorm != 0.0) {
        B->data[ix] -= xnorm;
        i51 = A->size[0] + (int32_T)(1.0 - ((1.0 + (real_T)ix) + 1.0));
        emlrtForLoopVectorCheckR2012b((1.0 + (real_T)ix) + 1.0, 1.0, A->size[0],
          mxDOUBLE_CLASS, i51, &yb_emlrtRTEI, sp);
        for (i = 0; i < i51; i++) {
          unnamed_idx_0 = ((uint32_T)ix + i) + 2U;
          B->data[(int32_T)unnamed_idx_0 - 1] -= b_A->data[((int32_T)
            unnamed_idx_0 + b_A->size[0] * ix) - 1] * xnorm;
        }
      }
    }
  }

  emxFree_real_T(&tau);
  emlrtForLoopVectorCheckR2012b(1.0, 1.0, atmp, mxDOUBLE_CLASS, (int32_T)atmp,
    &xb_emlrtRTEI, sp);
  for (i = 0; i < (int32_T)atmp; i++) {
    Y->data[jpvt->data[i] - 1] = B->data[i];
  }

  emlrtForLoopVectorCheckR2012b(atmp, -1.0, 1.0, mxDOUBLE_CLASS, (int32_T)-(1.0
    + (-1.0 - atmp)), &wb_emlrtRTEI, sp);
  for (ix = 0; ix < (int32_T)-(1.0 + (-1.0 - atmp)); ix++) {
    xnorm = atmp + -(real_T)ix;
    Y->data[jpvt->data[(int32_T)xnorm - 1] - 1] = eml_div(Y->data[jpvt->data
      [(int32_T)xnorm - 1] - 1], b_A->data[((int32_T)xnorm + b_A->size[0] *
      ((int32_T)xnorm - 1)) - 1]);
    for (i = 0; i < (int32_T)(xnorm - 1.0); i++) {
      Y->data[jpvt->data[i] - 1] -= Y->data[jpvt->data[(int32_T)xnorm - 1] - 1] *
        b_A->data[i + b_A->size[0] * ((int32_T)xnorm - 1)];
    }
  }

  emxFree_int32_T(&jpvt);
  emxFree_real_T(&work);
  emxFree_real_T(&b_A);
  emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
Esempio n. 6
0
/* Function Definitions */
void MechanicalPointForce(const emlrtStack *sp, const emxArray_real_T
  *particlePosition, const emxArray_real_T *pointSourcePosition, real_T
  forceDirection, real_T forceMagnitude, real_T cutoff, emxArray_real_T *force)
{
  uint32_T sz[2];
  int32_T ix;
  emxArray_real_T *forceTemp;
  int32_T loop_ub;
  emxArray_real_T *forceMag;
  int32_T vlen;
  int32_T sIdx;
  emxArray_real_T *forceDir;
  emxArray_real_T *distToSource;
  emxArray_int32_T *r0;
  emxArray_boolean_T *r1;
  emxArray_int32_T *r2;
  emxArray_real_T *x;
  emxArray_real_T *b_x;
  emxArray_real_T *r3;
  emxArray_real_T *r4;
  emxArray_real_T *b_pointSourcePosition;
  emxArray_real_T *b_forceDir;
  emxArray_real_T *c_forceDir;
  int32_T k;
  int32_T vstride;
  int32_T iy;
  int32_T ixstart;
  boolean_T overflow;
  real_T s;
  boolean_T b0;
  uint32_T varargin_2[2];
  boolean_T p;
  boolean_T exitg1;
  int32_T iv0[1];
  int32_T iv1[2];
  int32_T b_force[2];
  int32_T iv2[1];
  int32_T b_iy;
  int32_T c_iy;
  int32_T b_forceTemp[2];
  emlrtStack st;
  emlrtStack b_st;
  emlrtStack c_st;
  emlrtStack d_st;
  emlrtStack e_st;
  st.prev = sp;
  st.tls = sp->tls;
  b_st.prev = &st;
  b_st.tls = st.tls;
  c_st.prev = &b_st;
  c_st.tls = b_st.tls;
  d_st.prev = &c_st;
  d_st.tls = c_st.tls;
  e_st.prev = &d_st;
  e_st.tls = d_st.tls;
  emlrtHeapReferenceStackEnterFcnR2012b(sp);

  /*  apply mechanical (push or pull) force on particles */
  /*  mechanicalForce is a logical flag  */
  /*  particlPosition is a N by 3 vector of particle position */
  /*  pointSourcePosition is the position of force sources  */
  /*  forceDirection is either  -1 for 'in' or 1 for 'out' */
  /*  forceMagnitude is a positive number between 0 and 1 */
  /*  cutoff is the maximal direction the force operates on particle relative */
  /*  to the pointSourcePosition  */
  /*  the output is a vector of N by 3 of delta position to th */
  for (ix = 0; ix < 2; ix++) {
    sz[ix] = (uint32_T)particlePosition->size[ix];
  }

  emxInit_real_T(sp, &forceTemp, 2, &c_emlrtRTEI, true);
  ix = forceTemp->size[0] * forceTemp->size[1];
  forceTemp->size[0] = (int32_T)sz[0];
  emxEnsureCapacity(sp, (emxArray__common *)forceTemp, ix, (int32_T)sizeof
                    (real_T), &emlrtRTEI);
  ix = forceTemp->size[0] * forceTemp->size[1];
  forceTemp->size[1] = (int32_T)sz[1];
  emxEnsureCapacity(sp, (emxArray__common *)forceTemp, ix, (int32_T)sizeof
                    (real_T), &emlrtRTEI);
  loop_ub = (int32_T)sz[0] * (int32_T)sz[1];
  for (ix = 0; ix < loop_ub; ix++) {
    forceTemp->data[ix] = 0.0;
  }

  for (ix = 0; ix < 2; ix++) {
    sz[ix] = (uint32_T)particlePosition->size[ix];
  }

  ix = force->size[0] * force->size[1];
  force->size[0] = (int32_T)sz[0];
  emxEnsureCapacity(sp, (emxArray__common *)force, ix, (int32_T)sizeof(real_T),
                    &emlrtRTEI);
  ix = force->size[0] * force->size[1];
  force->size[1] = (int32_T)sz[1];
  emxEnsureCapacity(sp, (emxArray__common *)force, ix, (int32_T)sizeof(real_T),
                    &emlrtRTEI);
  loop_ub = (int32_T)sz[0] * (int32_T)sz[1];
  for (ix = 0; ix < loop_ub; ix++) {
    force->data[ix] = 0.0;
  }

  emxInit_real_T(sp, &forceMag, 2, &d_emlrtRTEI, true);
  vlen = particlePosition->size[0];
  ix = forceMag->size[0] * forceMag->size[1];
  forceMag->size[0] = vlen;
  emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof(real_T),
                    &emlrtRTEI);
  vlen = particlePosition->size[0];
  ix = forceMag->size[0] * forceMag->size[1];
  forceMag->size[1] = vlen;
  emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof(real_T),
                    &emlrtRTEI);
  loop_ub = particlePosition->size[0] * particlePosition->size[0];
  for (ix = 0; ix < loop_ub; ix++) {
    forceMag->data[ix] = 0.0;
  }

  sIdx = 0;
  emxInit_real_T(sp, &forceDir, 2, &e_emlrtRTEI, true);
  b_emxInit_real_T(sp, &distToSource, 1, &f_emlrtRTEI, true);
  emxInit_int32_T(sp, &r0, 1, &emlrtRTEI, true);
  emxInit_boolean_T(sp, &r1, 2, &emlrtRTEI, true);
  emxInit_int32_T(sp, &r2, 1, &emlrtRTEI, true);
  emxInit_real_T(sp, &x, 2, &emlrtRTEI, true);
  b_emxInit_real_T(sp, &b_x, 1, &emlrtRTEI, true);
  b_emxInit_real_T(sp, &r3, 1, &emlrtRTEI, true);
  b_emxInit_real_T(sp, &r4, 1, &emlrtRTEI, true);
  emxInit_real_T(sp, &b_pointSourcePosition, 2, &emlrtRTEI, true);
  b_emxInit_real_T(sp, &b_forceDir, 1, &emlrtRTEI, true);
  emxInit_real_T(sp, &c_forceDir, 2, &emlrtRTEI, true);
  while (sIdx <= pointSourcePosition->size[0] - 1) {
    loop_ub = pointSourcePosition->size[1];
    ix = pointSourcePosition->size[0];
    if ((sIdx + 1 >= 1) && (sIdx + 1 < ix)) {
      vlen = sIdx + 1;
    } else {
      vlen = emlrtDynamicBoundsCheckR2012b(sIdx + 1, 1, ix, (emlrtBCInfo *)
        &e_emlrtBCI, sp);
    }

    ix = b_pointSourcePosition->size[0] * b_pointSourcePosition->size[1];
    b_pointSourcePosition->size[0] = 1;
    b_pointSourcePosition->size[1] = loop_ub;
    emxEnsureCapacity(sp, (emxArray__common *)b_pointSourcePosition, ix,
                      (int32_T)sizeof(real_T), &emlrtRTEI);
    for (ix = 0; ix < loop_ub; ix++) {
      b_pointSourcePosition->data[b_pointSourcePosition->size[0] * ix] =
        pointSourcePosition->data[(vlen + pointSourcePosition->size[0] * ix) - 1];
    }

    st.site = &emlrtRSI;
    bsxfun(&st, particlePosition, b_pointSourcePosition, forceDir);

    /*  Find the distance between the particles and the source */
    st.site = &b_emlrtRSI;
    b_st.site = &h_emlrtRSI;
    c_st.site = &i_emlrtRSI;
    d_st.site = &j_emlrtRSI;
    for (ix = 0; ix < 2; ix++) {
      sz[ix] = (uint32_T)forceDir->size[ix];
    }

    ix = x->size[0] * x->size[1];
    x->size[0] = (int32_T)sz[0];
    x->size[1] = (int32_T)sz[1];
    emxEnsureCapacity(&d_st, (emxArray__common *)x, ix, (int32_T)sizeof(real_T),
                      &b_emlrtRTEI);
    if (dimagree(x, forceDir)) {
    } else {
      emlrtErrorWithMessageIdR2012b(&d_st, &b_emlrtRTEI, "MATLAB:dimagree", 0);
    }

    ix = (int32_T)sz[0] * (int32_T)sz[1];
    for (k = 0; k < ix; k++) {
      x->data[k] = forceDir->data[k] * forceDir->data[k];
    }

    st.site = &b_emlrtRSI;
    b_st.site = &k_emlrtRSI;
    c_st.site = &l_emlrtRSI;
    for (ix = 0; ix < 2; ix++) {
      sz[ix] = (uint32_T)x->size[ix];
    }

    ix = b_x->size[0];
    b_x->size[0] = (int32_T)sz[0];
    emxEnsureCapacity(&c_st, (emxArray__common *)b_x, ix, (int32_T)sizeof(real_T),
                      &emlrtRTEI);
    if ((x->size[0] == 0) || (x->size[1] == 0)) {
      ix = b_x->size[0];
      b_x->size[0] = (int32_T)sz[0];
      emxEnsureCapacity(&c_st, (emxArray__common *)b_x, ix, (int32_T)sizeof
                        (real_T), &emlrtRTEI);
      loop_ub = (int32_T)sz[0];
      for (ix = 0; ix < loop_ub; ix++) {
        b_x->data[ix] = 0.0;
      }
    } else {
      vlen = x->size[1];
      vstride = x->size[0];
      iy = -1;
      ixstart = -1;
      d_st.site = &m_emlrtRSI;
      overflow = (x->size[0] > 2147483646);
      if (overflow) {
        e_st.site = &g_emlrtRSI;
        check_forloop_overflow_error(&e_st);
      }

      for (loop_ub = 1; loop_ub <= vstride; loop_ub++) {
        ixstart++;
        ix = ixstart;
        s = x->data[ixstart];
        d_st.site = &n_emlrtRSI;
        if (2 > vlen) {
          b0 = false;
        } else {
          b0 = (vlen > 2147483646);
        }

        if (b0) {
          e_st.site = &g_emlrtRSI;
          check_forloop_overflow_error(&e_st);
        }

        for (k = 2; k <= vlen; k++) {
          ix += vstride;
          s += x->data[ix];
        }

        iy++;
        b_x->data[iy] = s;
      }
    }

    st.site = &b_emlrtRSI;
    ix = distToSource->size[0];
    distToSource->size[0] = b_x->size[0];
    emxEnsureCapacity(&st, (emxArray__common *)distToSource, ix, (int32_T)sizeof
                      (real_T), &emlrtRTEI);
    loop_ub = b_x->size[0];
    for (ix = 0; ix < loop_ub; ix++) {
      distToSource->data[ix] = b_x->data[ix];
    }

    for (k = 0; k < b_x->size[0]; k++) {
      if (b_x->data[k] < 0.0) {
        b_st.site = &o_emlrtRSI;
        eml_error(&b_st);
      }
    }

    for (k = 0; k < b_x->size[0]; k++) {
      distToSource->data[k] = muDoubleScalarSqrt(distToSource->data[k]);
    }

    /*  Normalize the forceDirection */
    iy = 0;
    while (iy < 3) {
      loop_ub = forceDir->size[0];
      ix = r2->size[0];
      r2->size[0] = loop_ub;
      emxEnsureCapacity(sp, (emxArray__common *)r2, ix, (int32_T)sizeof(int32_T),
                        &emlrtRTEI);
      for (ix = 0; ix < loop_ub; ix++) {
        r2->data[ix] = ix;
      }

      ix = forceDir->size[1];
      ixstart = 1 + iy;
      emlrtDynamicBoundsCheckR2012b(ixstart, 1, ix, (emlrtBCInfo *)&c_emlrtBCI,
        sp);
      st.site = &c_emlrtRSI;
      ix = forceDir->size[1];
      ixstart = 1 + iy;
      emlrtDynamicBoundsCheckR2012b(ixstart, 1, ix, (emlrtBCInfo *)&d_emlrtBCI,
        &st);
      ix = forceDir->size[0];
      sz[0] = (uint32_T)ix;
      sz[1] = 1U;
      varargin_2[0] = (uint32_T)distToSource->size[0];
      varargin_2[1] = 1U;
      overflow = false;
      p = true;
      k = 0;
      exitg1 = false;
      while ((!exitg1) && (k < 2)) {
        if (!((int32_T)sz[k] == (int32_T)varargin_2[k])) {
          p = false;
          exitg1 = true;
        } else {
          k++;
        }
      }

      if (!p) {
      } else {
        overflow = true;
      }

      if (overflow) {
      } else {
        emlrtErrorWithMessageIdR2012b(&st, &l_emlrtRTEI, "MATLAB:dimagree", 0);
      }

      loop_ub = forceDir->size[0];
      ix = b_x->size[0];
      b_x->size[0] = loop_ub;
      emxEnsureCapacity(&st, (emxArray__common *)b_x, ix, (int32_T)sizeof(real_T),
                        &emlrtRTEI);
      for (ix = 0; ix < loop_ub; ix++) {
        b_x->data[ix] = forceDir->data[ix + forceDir->size[0] * iy] /
          distToSource->data[ix];
      }

      iv0[0] = r2->size[0];
      emlrtSubAssignSizeCheckR2012b(iv0, 1, *(int32_T (*)[1])b_x->size, 1,
        (emlrtECInfo *)&d_emlrtECI, sp);
      loop_ub = b_x->size[0];
      for (ix = 0; ix < loop_ub; ix++) {
        forceDir->data[r2->data[ix] + forceDir->size[0] * iy] = b_x->data[ix];
      }

      /*  bsxfun(@rdivide,forceDir,distToSource); */
      iy++;
      if (*emlrtBreakCheckR2012bFlagVar != 0) {
        emlrtBreakCheckR2012b(sp);
      }
    }

    /*  Multiply the */
    if (forceDirection == -1.0) {
      ix = r4->size[0];
      r4->size[0] = distToSource->size[0];
      emxEnsureCapacity(sp, (emxArray__common *)r4, ix, (int32_T)sizeof(real_T),
                        &emlrtRTEI);
      loop_ub = distToSource->size[0];
      for (ix = 0; ix < loop_ub; ix++) {
        r4->data[ix] = 1.0 + distToSource->data[ix];
      }

      rdivide(sp, forceMagnitude, r4, b_x);
      vlen = b_x->size[0];
      ix = forceMag->size[0] * forceMag->size[1];
      forceMag->size[0] = vlen;
      emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof
                        (real_T), &emlrtRTEI);
      ix = forceMag->size[0] * forceMag->size[1];
      forceMag->size[1] = 1;
      emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof
                        (real_T), &emlrtRTEI);
      loop_ub = b_x->size[0];
      for (ix = 0; ix < loop_ub; ix++) {
        forceMag->data[ix] = 1.0 - b_x->data[ix];
      }
    } else {
      if (forceDirection == 1.0) {
        ix = r3->size[0];
        r3->size[0] = distToSource->size[0];
        emxEnsureCapacity(sp, (emxArray__common *)r3, ix, (int32_T)sizeof(real_T),
                          &emlrtRTEI);
        loop_ub = distToSource->size[0];
        for (ix = 0; ix < loop_ub; ix++) {
          r3->data[ix] = 1.0 + distToSource->data[ix];
        }

        rdivide(sp, forceMagnitude, r3, b_x);
        vlen = b_x->size[0];
        ix = forceMag->size[0] * forceMag->size[1];
        forceMag->size[0] = vlen;
        emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof
                          (real_T), &emlrtRTEI);
        ix = forceMag->size[0] * forceMag->size[1];
        forceMag->size[1] = 1;
        emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof
                          (real_T), &emlrtRTEI);
        loop_ub = b_x->size[0];
        for (ix = 0; ix < loop_ub; ix++) {
          forceMag->data[ix] = b_x->data[ix];
        }
      }
    }

    iy = 0;
    while (iy < 3) {
      ix = forceDir->size[1];
      ixstart = 1 + iy;
      emlrtDynamicBoundsCheckR2012b(ixstart, 1, ix, (emlrtBCInfo *)&b_emlrtBCI,
        sp);
      ix = forceDir->size[0];
      iv1[0] = ix;
      iv1[1] = 1;
      for (ix = 0; ix < 2; ix++) {
        b_force[ix] = forceMag->size[ix];
      }

      if ((iv1[0] != b_force[0]) || (1 != b_force[1])) {
        emlrtSizeEqCheckNDR2012b(iv1, b_force, (emlrtECInfo *)&c_emlrtECI, sp);
      }

      loop_ub = forceTemp->size[0];
      ix = r2->size[0];
      r2->size[0] = loop_ub;
      emxEnsureCapacity(sp, (emxArray__common *)r2, ix, (int32_T)sizeof(int32_T),
                        &emlrtRTEI);
      for (ix = 0; ix < loop_ub; ix++) {
        r2->data[ix] = ix;
      }

      ix = forceTemp->size[1];
      ixstart = 1 + iy;
      emlrtDynamicBoundsCheckR2012b(ixstart, 1, ix, (emlrtBCInfo *)&emlrtBCI, sp);
      loop_ub = forceDir->size[0];
      vlen = forceDir->size[0];
      vstride = forceDir->size[0];
      ix = b_forceDir->size[0];
      b_forceDir->size[0] = vstride;
      emxEnsureCapacity(sp, (emxArray__common *)b_forceDir, ix, (int32_T)sizeof
                        (real_T), &emlrtRTEI);
      for (ix = 0; ix < vstride; ix++) {
        b_forceDir->data[ix] = forceDir->data[ix + forceDir->size[0] * iy];
      }

      ix = c_forceDir->size[0] * c_forceDir->size[1];
      c_forceDir->size[0] = loop_ub;
      c_forceDir->size[1] = 1;
      emxEnsureCapacity(sp, (emxArray__common *)c_forceDir, ix, (int32_T)sizeof
                        (real_T), &emlrtRTEI);
      for (ix = 0; ix < loop_ub; ix++) {
        c_forceDir->data[ix] = b_forceDir->data[ix];
      }

      ix = b_x->size[0];
      b_x->size[0] = vlen;
      emxEnsureCapacity(sp, (emxArray__common *)b_x, ix, (int32_T)sizeof(real_T),
                        &emlrtRTEI);
      for (ix = 0; ix < vlen; ix++) {
        b_x->data[ix] = c_forceDir->data[ix] * forceMag->data[ix];
      }

      iv2[0] = r2->size[0];
      emlrtSubAssignSizeCheckR2012b(iv2, 1, *(int32_T (*)[1])b_x->size, 1,
        (emlrtECInfo *)&b_emlrtECI, sp);
      loop_ub = b_x->size[0];
      for (ix = 0; ix < loop_ub; ix++) {
        forceTemp->data[r2->data[ix] + forceTemp->size[0] * iy] = b_x->data[ix];
      }

      /*  bsxfun(@times,forceDir,forceTemp); */
      iy++;
      if (*emlrtBreakCheckR2012bFlagVar != 0) {
        emlrtBreakCheckR2012b(sp);
      }
    }

    iy = distToSource->size[0] - 1;
    vlen = 0;
    for (vstride = 0; vstride <= iy; vstride++) {
      if (distToSource->data[vstride] > cutoff) {
        vlen++;
      }
    }

    ix = r2->size[0];
    r2->size[0] = vlen;
    emxEnsureCapacity(sp, (emxArray__common *)r2, ix, (int32_T)sizeof(int32_T),
                      &emlrtRTEI);
    vlen = 0;
    for (vstride = 0; vstride <= iy; vstride++) {
      if (distToSource->data[vstride] > cutoff) {
        r2->data[vlen] = vstride + 1;
        vlen++;
      }
    }

    loop_ub = forceTemp->size[1];
    vstride = forceTemp->size[0];
    vlen = r2->size[0];
    for (ix = 0; ix < loop_ub; ix++) {
      for (ixstart = 0; ixstart < vlen; ixstart++) {
        iy = r2->data[ixstart];
        if ((iy >= 1) && (iy < vstride)) {
          b_iy = iy;
        } else {
          b_iy = emlrtDynamicBoundsCheckR2012b(iy, 1, vstride, (emlrtBCInfo *)
            &f_emlrtBCI, sp);
        }

        forceTemp->data[(b_iy + forceTemp->size[0] * ix) - 1] = 0.0;
      }
    }

    ix = r1->size[0] * r1->size[1];
    r1->size[0] = forceTemp->size[0];
    r1->size[1] = forceTemp->size[1];
    emxEnsureCapacity(sp, (emxArray__common *)r1, ix, (int32_T)sizeof(boolean_T),
                      &emlrtRTEI);
    loop_ub = forceTemp->size[0] * forceTemp->size[1];
    for (ix = 0; ix < loop_ub; ix++) {
      r1->data[ix] = muDoubleScalarIsNaN(forceTemp->data[ix]);
    }

    iy = r1->size[0] * r1->size[1] - 1;
    vlen = 0;
    for (vstride = 0; vstride <= iy; vstride++) {
      if (r1->data[vstride]) {
        vlen++;
      }
    }

    ix = r0->size[0];
    r0->size[0] = vlen;
    emxEnsureCapacity(sp, (emxArray__common *)r0, ix, (int32_T)sizeof(int32_T),
                      &emlrtRTEI);
    vlen = 0;
    for (vstride = 0; vstride <= iy; vstride++) {
      if (r1->data[vstride]) {
        r0->data[vlen] = vstride + 1;
        vlen++;
      }
    }

    vstride = forceTemp->size[0];
    vlen = forceTemp->size[1];
    loop_ub = r0->size[0];
    for (ix = 0; ix < loop_ub; ix++) {
      ixstart = vstride * vlen;
      iy = r0->data[ix];
      if ((iy >= 1) && (iy < ixstart)) {
        c_iy = iy;
      } else {
        c_iy = emlrtDynamicBoundsCheckR2012b(iy, 1, ixstart, (emlrtBCInfo *)
          &g_emlrtBCI, sp);
      }

      forceTemp->data[c_iy - 1] = 0.0;
    }

    for (ix = 0; ix < 2; ix++) {
      b_force[ix] = force->size[ix];
    }

    for (ix = 0; ix < 2; ix++) {
      b_forceTemp[ix] = forceTemp->size[ix];
    }

    if ((b_force[0] != b_forceTemp[0]) || (b_force[1] != b_forceTemp[1])) {
      emlrtSizeEqCheckNDR2012b(b_force, b_forceTemp, (emlrtECInfo *)&emlrtECI,
        sp);
    }

    ix = force->size[0] * force->size[1];
    emxEnsureCapacity(sp, (emxArray__common *)force, ix, (int32_T)sizeof(real_T),
                      &emlrtRTEI);
    vlen = force->size[0];
    vstride = force->size[1];
    loop_ub = vlen * vstride;
    for (ix = 0; ix < loop_ub; ix++) {
      force->data[ix] += forceTemp->data[ix];
    }

    sIdx++;
    if (*emlrtBreakCheckR2012bFlagVar != 0) {
      emlrtBreakCheckR2012b(sp);
    }
  }

  emxFree_real_T(&c_forceDir);
  emxFree_real_T(&b_forceDir);
  emxFree_real_T(&b_pointSourcePosition);
  emxFree_real_T(&r4);
  emxFree_real_T(&r3);
  emxFree_real_T(&b_x);
  emxFree_real_T(&x);
  emxFree_int32_T(&r2);
  emxFree_boolean_T(&r1);
  emxFree_int32_T(&r0);
  emxFree_real_T(&distToSource);
  emxFree_real_T(&forceDir);
  emxFree_real_T(&forceMag);
  emxFree_real_T(&forceTemp);
  emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
/*
 * function offsetcost = equateoffsetcost(pathqi)
 */
void equateoffsetcost(const emxArray_real_T *pathqi, emxArray_real_T *offsetcost)
{
  emxArray_real_T *b_pathqi;
  int32_T n;
  int32_T c_pathqi;
  int32_T ixstart;
  uint32_T uv0[2];
  int32_T k;
  emxArray_int32_T *r75;
  int32_T exitg3;
  real_T mtmp;
  real_T b_mtmp;
  boolean_T exitg2;
  boolean_T exitg1;
  b_emxInit_real_T(&b_pathqi, 2);

  /* UNTITLED2 Summary of this function goes here */
  /*    Detailed explanation goes here */
  /* 'equateoffsetcost:6' pathoffsetcost = abs(pathqi(end,:)); */
  n = pathqi->size[1];
  c_pathqi = pathqi->size[0];
  ixstart = b_pathqi->size[0] * b_pathqi->size[1];
  b_pathqi->size[0] = 1;
  b_pathqi->size[1] = n;
  emxEnsureCapacity((emxArray__common *)b_pathqi, ixstart, (int32_T)sizeof
                    (real_T));
  ixstart = n - 1;
  for (n = 0; n <= ixstart; n++) {
    b_pathqi->data[b_pathqi->size[0] * n] = pathqi->data[(c_pathqi +
      pathqi->size[0] * n) - 1];
  }

  for (n = 0; n < 2; n++) {
    uv0[n] = (uint32_T)b_pathqi->size[n];
  }

  emxFree_real_T(&b_pathqi);
  n = offsetcost->size[0] * offsetcost->size[1];
  offsetcost->size[0] = 1;
  offsetcost->size[1] = (int32_T)uv0[1];
  emxEnsureCapacity((emxArray__common *)offsetcost, n, (int32_T)sizeof(real_T));
  k = 0;
  b_emxInit_int32_T(&r75, 1);
  do {
    exitg3 = 0;
    n = pathqi->size[1];
    ixstart = r75->size[0];
    r75->size[0] = n;
    emxEnsureCapacity((emxArray__common *)r75, ixstart, (int32_T)sizeof(int32_T));
    ixstart = n - 1;
    for (n = 0; n <= ixstart; n++) {
      r75->data[n] = 1 + n;
    }

    if (k <= r75->size[0] - 1) {
      c_pathqi = pathqi->size[0];
      mtmp = pathqi->data[(c_pathqi + pathqi->size[0] * k) - 1];
      offsetcost->data[k] = fabs(mtmp);
      k++;
    } else {
      exitg3 = 1;
    }
  } while (exitg3 == 0U);

  emxFree_int32_T(&r75);

  /* 'equateoffsetcost:8' minoffsetcost = min(pathoffsetcost); */
  ixstart = 1;
  n = offsetcost->size[1];
  b_mtmp = offsetcost->data[0];
  if (n > 1) {
    if (rtIsNaN(offsetcost->data[0])) {
      c_pathqi = 2;
      exitg2 = FALSE;
      while ((exitg2 == 0U) && (c_pathqi <= n)) {
        ixstart = c_pathqi;
        if (!rtIsNaN(offsetcost->data[c_pathqi - 1])) {
          b_mtmp = offsetcost->data[c_pathqi - 1];
          exitg2 = TRUE;
        } else {
          c_pathqi++;
        }
      }
    }

    if (ixstart < n) {
      while (ixstart + 1 <= n) {
        if (offsetcost->data[ixstart] < b_mtmp) {
          b_mtmp = offsetcost->data[ixstart];
        }

        ixstart++;
      }
    }
  }

  /* 'equateoffsetcost:9' maxoffsetcost = max(pathoffsetcost); */
  ixstart = 1;
  n = offsetcost->size[1];
  mtmp = offsetcost->data[0];
  if (n > 1) {
    if (rtIsNaN(offsetcost->data[0])) {
      c_pathqi = 2;
      exitg1 = FALSE;
      while ((exitg1 == 0U) && (c_pathqi <= n)) {
        ixstart = c_pathqi;
        if (!rtIsNaN(offsetcost->data[c_pathqi - 1])) {
          mtmp = offsetcost->data[c_pathqi - 1];
          exitg1 = TRUE;
        } else {
          c_pathqi++;
        }
      }
    }

    if (ixstart < n) {
      while (ixstart + 1 <= n) {
        if (offsetcost->data[ixstart] > mtmp) {
          mtmp = offsetcost->data[ixstart];
        }

        ixstart++;
      }
    }
  }

  /* 'equateoffsetcost:10' offsetcost = (pathoffsetcost-minoffsetcost)*(1/(maxoffsetcost - minoffsetcost)); */
  mtmp = 1.0 / (mtmp - b_mtmp);
  n = offsetcost->size[0] * offsetcost->size[1];
  offsetcost->size[0] = 1;
  offsetcost->size[1] = offsetcost->size[1];
  emxEnsureCapacity((emxArray__common *)offsetcost, n, (int32_T)sizeof(real_T));
  ixstart = offsetcost->size[0];
  n = offsetcost->size[1];
  ixstart = ixstart * n - 1;
  for (n = 0; n <= ixstart; n++) {
    offsetcost->data[n] = (offsetcost->data[n] - b_mtmp) * mtmp;
  }
}
Esempio n. 8
0
/*
 * 计算肯德尔系数
 * data1;data2均为一列数据
 * Arguments    : const emxArray_real_T *data1
 *                const emxArray_real_T *data2
 *                double limit_min1
 *                double limit_min2
 * Return Type  : double
 */
double Kendall(const emxArray_real_T *data1, const emxArray_real_T *data2,
               double limit_min1, double limit_min2)
{
  double R;
  int row;
  emxArray_real_T *sample1;
  int i2;
  int loop_ub;
  unsigned int mm;
  int ii;
  emxArray_real_T *sample;
  int m;
  emxArray_int32_T *b_ii;
  emxArray_int32_T *iwork;
  int i;
  int j;
  int pEnd;
  int p;
  int q;
  int qEnd;
  int kEnd;
  emxArray_real_T *nozero;
  unsigned int sample_idx_0;
  double P;
  int aa;
  int bb;
  if (data1->size[0] <= data2->size[0]) {
    row = data1->size[0];
  } else {
    row = data2->size[0];
  }

  emxInit_real_T(&sample1, 2);
  i2 = sample1->size[0] * sample1->size[1];
  sample1->size[0] = row;
  sample1->size[1] = 2;
  emxEnsureCapacity((emxArray__common *)sample1, i2, (int)sizeof(double));
  loop_ub = row << 1;
  for (i2 = 0; i2 < loop_ub; i2++) {
    sample1->data[i2] = 0.0;
  }

  mm = 0U;
  for (ii = 0; ii < row; ii++) {
    if ((data1->data[ii] < limit_min1) || (data2->data[ii] < limit_min2) ||
        rtIsNaN(data1->data[ii]) || rtIsNaN(data2->data[ii])) {
    } else {
      mm++;
      sample1->data[(int)mm - 1] = data1->data[ii];
      sample1->data[((int)mm + sample1->size[0]) - 1] = data2->data[ii];
    }
  }

  if (1 > (int)mm) {
    loop_ub = 0;
  } else {
    loop_ub = (int)mm;
  }

  emxInit_real_T(&sample, 2);

  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
  /* 计算Kendall(肯德尔)秩相关系数,用于二维最大熵函数的拟合 */
  i2 = sample->size[0] * sample->size[1];
  sample->size[0] = loop_ub;
  sample->size[1] = 2;
  emxEnsureCapacity((emxArray__common *)sample, i2, (int)sizeof(double));
  for (i2 = 0; i2 < 2; i2++) {
    for (m = 0; m < loop_ub; m++) {
      sample->data[m + sample->size[0] * i2] = sample1->data[m + sample1->size[0]
        * i2];
    }
  }

  emxFree_real_T(&sample1);
  emxInit_int32_T(&b_ii, 1);
  i2 = b_ii->size[0];
  b_ii->size[0] = loop_ub;
  emxEnsureCapacity((emxArray__common *)b_ii, i2, (int)sizeof(int));
  for (i2 = 0; i2 < loop_ub; i2++) {
    b_ii->data[i2] = 0;
  }

  if (loop_ub == 0) {
  } else {
    emxInit_int32_T(&iwork, 1);
    i2 = iwork->size[0];
    iwork->size[0] = loop_ub;
    emxEnsureCapacity((emxArray__common *)iwork, i2, (int)sizeof(int));
    for (m = 1; m <= loop_ub - 1; m += 2) {
      if (sortLE(sample, m, m + 1)) {
        b_ii->data[m - 1] = m;
        b_ii->data[m] = m + 1;
      } else {
        b_ii->data[m - 1] = m + 1;
        b_ii->data[m] = m;
      }
    }

    if ((loop_ub & 1) != 0) {
      b_ii->data[loop_ub - 1] = loop_ub;
    }

    i = 2;
    while (i < loop_ub) {
      i2 = i << 1;
      j = 1;
      for (pEnd = 1 + i; pEnd < loop_ub + 1; pEnd = qEnd + i) {
        p = j;
        q = pEnd;
        qEnd = j + i2;
        if (qEnd > loop_ub + 1) {
          qEnd = loop_ub + 1;
        }

        m = 0;
        kEnd = qEnd - j;
        while (m + 1 <= kEnd) {
          if (sortLE(sample, b_ii->data[p - 1], b_ii->data[q - 1])) {
            iwork->data[m] = b_ii->data[p - 1];
            p++;
            if (p == pEnd) {
              while (q < qEnd) {
                m++;
                iwork->data[m] = b_ii->data[q - 1];
                q++;
              }
            }
          } else {
            iwork->data[m] = b_ii->data[q - 1];
            q++;
            if (q == qEnd) {
              while (p < pEnd) {
                m++;
                iwork->data[m] = b_ii->data[p - 1];
                p++;
              }
            }
          }

          m++;
        }

        for (m = 0; m + 1 <= kEnd; m++) {
          b_ii->data[(j + m) - 1] = iwork->data[m];
        }

        j = qEnd;
      }

      i = i2;
    }

    emxFree_int32_T(&iwork);
  }

  emxInit_real_T1(&nozero, 1);
  m = sample->size[0];
  sample_idx_0 = (unsigned int)sample->size[0];
  i2 = nozero->size[0];
  nozero->size[0] = (int)sample_idx_0;
  emxEnsureCapacity((emxArray__common *)nozero, i2, (int)sizeof(double));
  for (j = 0; j < 2; j++) {
    for (i = 0; i + 1 <= m; i++) {
      nozero->data[i] = sample->data[(b_ii->data[i] + sample->size[0] * j) - 1];
    }

    for (i = 0; i + 1 <= m; i++) {
      sample->data[i + sample->size[0] * j] = nozero->data[i];
    }
  }

  emxFree_int32_T(&b_ii);
  emxFree_real_T(&nozero);

  /* 按波高为关键值,对波高和周期的组合进行排序 */
  P = 0.0;
  for (aa = 0; aa < loop_ub; aa++) {
    i2 = loop_ub + (int)(1.0 - ((1.0 + (double)aa) + 1.0));
    for (bb = 0; bb < i2; bb++) {
      if (sample->data[aa + sample->size[0]] <= sample->data[((int)((unsigned
             int)aa + bb) + sample->size[0]) + 1]) {
        P++;
      }
    }
  }

  emxFree_real_T(&sample);
  R = 4.0 * P / ((double)loop_ub * ((double)loop_ub - 1.0)) - 1.0;

  /* Kendall(肯德尔)秩相关系数 */
  return R;
}
Esempio n. 9
0
/* Function Definitions */
void features_bta(const emxArray_real_T *tap, double ft[2])
{
  int i12;
  emxArray_real_T *t;
  int loop_ub;
  double tapdt;
  emxArray_real_T *tapx;
  emxArray_real_T *dx;
  emxArray_boolean_T *i;
  emxArray_int32_T *r12;
  double tapiqr;

  /*  Computes basic tapping test features. */
  /*  Inputs: */
  /*   tap - tapping data vector: tap(:,1) - time points, */
  /*         tap(:,2:3) - X,Y touch screen coordinates */
  /*  */
  /*  (CC BY-SA 3.0) Max Little, 2014 */
  /*  Output feature vector */
  for (i12 = 0; i12 < 2; i12++) {
    ft[i12] = rtNaN;
  }

  /*  Ignore zero-length inputs */
  if (tap->size[0] == 0) {
  } else {
    b_emxInit_real_T(&t, 1);

    /*  Calculate relative time */
    loop_ub = tap->size[0];
    tapdt = tap->data[0];
    i12 = t->size[0];
    t->size[0] = loop_ub;
    emxEnsureCapacity((emxArray__common *)t, i12, (int)sizeof(double));
    for (i12 = 0; i12 < loop_ub; i12++) {
      t->data[i12] = tap->data[i12] - tapdt;
    }

    b_emxInit_real_T(&tapx, 1);

    /*  X,Y offset */
    loop_ub = tap->size[0];
    i12 = tapx->size[0];
    tapx->size[0] = loop_ub;
    emxEnsureCapacity((emxArray__common *)tapx, i12, (int)sizeof(double));
    for (i12 = 0; i12 < loop_ub; i12++) {
      tapx->data[i12] = tap->data[i12 + tap->size[0]];
    }

    tapdt = mean(tapx);
    i12 = tapx->size[0];
    emxEnsureCapacity((emxArray__common *)tapx, i12, (int)sizeof(double));
    loop_ub = tapx->size[0];
    for (i12 = 0; i12 < loop_ub; i12++) {
      tapx->data[i12] -= tapdt;
    }

    b_emxInit_real_T(&dx, 1);
    emxInit_boolean_T(&i, 1);

    /*  Find left/right finger 'depress' events */
    diff(tapx, dx);
    b_abs(dx, tapx);
    i12 = i->size[0];
    i->size[0] = tapx->size[0];
    emxEnsureCapacity((emxArray__common *)i, i12, (int)sizeof(boolean_T));
    loop_ub = tapx->size[0];
    for (i12 = 0; i12 < loop_ub; i12++) {
      i->data[i12] = (tapx->data[i12] > 20.0);
    }

    emxInit_int32_T(&r12, 1);
    eml_li_find(i, r12);
    i12 = dx->size[0];
    dx->size[0] = r12->size[0];
    emxEnsureCapacity((emxArray__common *)dx, i12, (int)sizeof(double));
    loop_ub = r12->size[0];
    emxFree_boolean_T(&i);
    for (i12 = 0; i12 < loop_ub; i12++) {
      dx->data[i12] = t->data[r12->data[i12] - 1];
    }

    emxFree_int32_T(&r12);
    emxFree_real_T(&t);

    /*  Find depress event intervals */
    diff(dx, tapx);

    /*  Median and spread of tapping rate */
    emxFree_real_T(&dx);
    if (tapx->size[0] == 0) {
      tapdt = rtNaN;
    } else {
      tapdt = vectormedian(tapx);
    }

    tapiqr = iqr(tapx);

    /*  Output tapping test feature vector */
    ft[0] = tapdt;
    ft[1] = tapiqr;
    emxFree_real_T(&tapx);
  }
}
Esempio n. 10
0
void mldivide(const emxArray_real_T *A, emxArray_real_T *B)
{
  emxArray_real_T *Y;
  emxArray_real_T *b_B;
  int32_T n;
  int32_T i17;
  int32_T jy;
  emxArray_int32_T *ipiv;
  int32_T ldap1;
  int32_T u0;
  int32_T j;
  int32_T mmj;
  int32_T jj;
  int32_T jp1j;
  int32_T c;
  int32_T ix;
  real_T temp;
  int32_T k;
  real_T s;
  int32_T jA;
  emxArray_real_T *r77;
  emxInit_real_T(&Y, 1);
  if (A->size[0] == A->size[1]) {
    b_emxInit_real_T(&b_B, 2);
    n = A->size[1];
    i17 = b_B->size[0] * b_B->size[1];
    b_B->size[0] = A->size[0];
    b_B->size[1] = A->size[1];
    emxEnsureCapacity((emxArray__common *)b_B, i17, (int32_T)sizeof(real_T));
    jy = A->size[0] * A->size[1] - 1;
    for (i17 = 0; i17 <= jy; i17++) {
      b_B->data[i17] = A->data[i17];
    }

    emxInit_int32_T(&ipiv, 2);
    eml_signed_integer_colon(n, ipiv);
    ldap1 = n + 1;
    u0 = n - 1;
    if (u0 <= n) {
    } else {
      u0 = n;
    }

    for (j = 1; j <= u0; j++) {
      mmj = n - j;
      jj = (j - 1) * ldap1;
      jp1j = jj + 2;
      c = mmj + 1;
      if (c < 1) {
        jy = 0;
      } else {
        jy = 1;
        if (c > 1) {
          ix = jj;
          temp = fabs(b_B->data[jj]);
          for (k = 2; k <= c; k++) {
            ix++;
            s = fabs(b_B->data[ix]);
            if (s > temp) {
              jy = k;
              temp = s;
            }
          }
        }
      }

      if (b_B->data[(jj + jy) - 1] != 0.0) {
        if (jy - 1 != 0) {
          ipiv->data[j - 1] = (j + jy) - 1;
          eml_xswap(n, b_B, j, n, (j + jy) - 1, n);
        }

        i17 = (jp1j + mmj) - 1;
        for (jy = jp1j; jy <= i17; jy++) {
          b_B->data[jy - 1] /= b_B->data[jj];
        }
      }

      c = n - j;
      jA = jj + ldap1;
      jy = jj + n;
      for (k = 1; k <= c; k++) {
        temp = b_B->data[jy];
        if (b_B->data[jy] != 0.0) {
          ix = jp1j;
          i17 = mmj + jA;
          for (jj = jA; jj + 1 <= i17; jj++) {
            b_B->data[jj] += b_B->data[ix - 1] * -temp;
            ix++;
          }
        }

        jy += n;
        jA += n;
      }
    }

    for (jy = 0; jy + 1 <= n; jy++) {
      if (ipiv->data[jy] != jy + 1) {
        temp = B->data[jy];
        B->data[jy] = B->data[ipiv->data[jy] - 1];
        B->data[ipiv->data[jy] - 1] = temp;
      }
    }

    emxFree_int32_T(&ipiv);
    for (k = 0; k + 1 <= n; k++) {
      c = n * k;
      if (B->data[k] != 0.0) {
        for (jy = k + 2; jy <= n; jy++) {
          B->data[jy - 1] -= B->data[k] * b_B->data[(jy + c) - 1];
        }
      }
    }

    for (k = n - 1; k + 1 > 0; k--) {
      c = n * k;
      if (B->data[k] != 0.0) {
        B->data[k] /= b_B->data[k + c];
        for (jy = 1; jy <= k; jy++) {
          B->data[jy - 1] -= B->data[k] * b_B->data[(jy + c) - 1];
        }
      }
    }

    emxFree_real_T(&b_B);
  } else {
    emxInit_real_T(&b_B, 1);
    i17 = b_B->size[0];
    b_B->size[0] = B->size[0];
    emxEnsureCapacity((emxArray__common *)b_B, i17, (int32_T)sizeof(real_T));
    jy = B->size[0] - 1;
    for (i17 = 0; i17 <= jy; i17++) {
      b_B->data[i17] = B->data[i17];
    }

    emxInit_real_T(&r77, 1);
    eml_qrsolve(A, b_B, r77);
    i17 = B->size[0];
    B->size[0] = r77->size[0];
    emxEnsureCapacity((emxArray__common *)B, i17, (int32_T)sizeof(real_T));
    emxFree_real_T(&b_B);
    jy = r77->size[0] - 1;
    for (i17 = 0; i17 <= jy; i17++) {
      B->data[i17] = r77->data[i17];
    }

    emxFree_real_T(&r77);
  }

  emxFree_real_T(&Y);
}
// 
// % Erased all parts not wanted for codegeneration /Martin %% 
// Arguments    : emxArray_real_T *s 
//                emxArray_boolean_T *outliers 
//                emxArray_boolean_T *outliers2 
// Return Type  : void 
//
void locateOutliers(emxArray_real_T *s, emxArray_boolean_T *outliers, emxArray_boolean_T *outliers2)
{
    int ia;
    int ic;
    emxArray_real_T *b_y1;
    int orderForDim;
    int iyLead;
    double work_data_idx_0;
    int m;
    double tmp1;
    double tmp2;
    emxArray_real_T *SigmaMat;
    emxArray_int32_T *r0;
    emxArray_int32_T *r1;
    emxArray_real_T *a;
    unsigned int s_idx_0;
    emxArray_real_T *C;
    int br;
    double mu;
    emxArray_real_T *b_s;
    /* locateOutliers: locates artifacts/outliers from data series */
    /*  */
    /*   Inputs:  s = array containg data series */
    /*            method = artifact removal method to use. */
    /*   methods: 'percent' = percentage filter: locates data > x percent diff than previous data point.                */
    /*            'sd' = standard deviation filter: locates data > x stdev away from mean. */
    /*            'above' = Threshold filter: locates data > threshold value */
    /*            'below' = Threshold filter: locates data < threshold value */
    /*            'median' = median filter. Outliers are located. */
    /* Outputs:   outliers = logical array of whether s is artifact/outlier or not */
    /*                        eg. - [0 0 0 1 0], 1=artifact, 0=normal */
    /*                         */
    /* Examples: */
    /*    Locate outliers with 20% percentage filter: */
    /*        outliers = locateOutlers(s,'percent',0.2) */
    /*    Locate outliers that are above a threshold of 0.5: */
    /*        outliers = locateOutlers(s,'thresh','above',0.5) */
    /*    Locate outliers with median filter: */
    /*        outliers = locateOutlers(s,'median',4,5) */
    /*  */
    /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
    /*  Copyright (C) 2010, John T. Ramshur, [email protected] */
    /*   */
    /*  This file is part of HRVAS */
    /*  */
    /*  HRVAS is free software: you can redistribute it and/or modify */
    /*  it under the terms of the GNU General Public License as published by */
    /*  the Free Software Foundation, either version 3 of the License, or */
    /*  (at your option) any later version. */
    /*   */
    /*  HRVAS is distributed in the hope that it will be useful, */
    /*  but WITHOUT ANY WARRANTY; without even the implied warranty of */
    /*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the */
    /*  GNU General Public License for more details. */
    /*   */
    /*  You should have received a copy of the GNU General Public License */
    /*  along with Foobar.  If not, see <http://www.gnu.org/licenses/>. */
    /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
    ia = outliers->size[0];
    outliers->size[0] = s->size[0];
    emxEnsureCapacity((emxArray__common *)outliers, ia, (int)sizeof(boolean_T));
    ic = s->size[0];
    for (ia = 0; ia < ic; ia++) {
        outliers->data[ia] = false;
    }
    /* preallocate         */
    if (1 > s->size[0] - 1) {
        ic = 0;
    } else {
        ic = s->size[0] - 1;
    }
    emxInit_real_T(&b_y1, 1);
    if (s->size[0] == 0) {
        ia = b_y1->size[0];
        b_y1->size[0] = 0;
        emxEnsureCapacity((emxArray__common *)b_y1, ia, (int)sizeof(double));
    } else {
        if (s->size[0] - 1 <= 1) {
            orderForDim = s->size[0] - 1;
        } else {
            orderForDim = 1;
        }
        if (orderForDim < 1) {
            ia = b_y1->size[0];
            b_y1->size[0] = 0;
            emxEnsureCapacity((emxArray__common *)b_y1, ia, (int)sizeof(double));
        } else {
            orderForDim = s->size[0] - 1;
            ia = b_y1->size[0];
            b_y1->size[0] = orderForDim;
            emxEnsureCapacity((emxArray__common *)b_y1, ia, (int)sizeof(double));
            if (!(b_y1->size[0] == 0)) {
                orderForDim = 1;
                iyLead = 0;
                work_data_idx_0 = s->data[0];
                for (m = 2; m <= s->size[0]; m++) {
                    tmp1 = s->data[orderForDim];
                    tmp2 = work_data_idx_0;
                    work_data_idx_0 = tmp1;
                    tmp1 -= tmp2;
                    orderForDim++;
                    b_y1->data[iyLead] = tmp1;
                    iyLead++;
                }
            }
        }
    }
    emxInit_real_T(&SigmaMat, 1);
    b_abs(b_y1, SigmaMat);
    /* percent chage from previous */
    /* find index of values where pChange > perLimit */
    orderForDim = s->size[0];
    if (2 > orderForDim) {
        ia = 0;
        orderForDim = 0;
    } else {
        ia = 1;
        orderForDim = s->size[0];
    }
    emxInit_int32_T(&r0, 2);
    iyLead = r0->size[0] * r0->size[1];
    r0->size[0] = 1;
    r0->size[1] = orderForDim - ia;
    emxEnsureCapacity((emxArray__common *)r0, iyLead, (int)sizeof(int));
    orderForDim -= ia;
    for (iyLead = 0; iyLead < orderForDim; iyLead++) {
        r0->data[r0->size[0] * iyLead] = ia + iyLead;
    }
    emxInit_int32_T1(&r1, 1);
    ia = r1->size[0];
    r1->size[0] = ic;
    emxEnsureCapacity((emxArray__common *)r1, ia, (int)sizeof(int));
    for (ia = 0; ia < ic; ia++) {
        r1->data[ia] = 1 + ia;
    }
    ic = r0->size[0] * r0->size[1];
    for (ia = 0; ia < ic; ia++) {
        outliers->data[r0->data[ia]] = (SigmaMat->data[ia] / s->data[r1->data[ia] - 1] > 0.2);
    }
    emxFree_int32_T(&r1);
    emxFree_int32_T(&r0);
    emxInit_real_T1(&a, 2);
    /*  Reference:  */
    /*  Clifford, G. (2002). "Characterizing Artefact in the Normal  */
    /*  Human 24-Hour RR Time Series to Aid Identification and Artificial  */
    /*  Replication of Circadian Variations in Human Beat to Beat Heart Rate  */
    /*  using a Simple Threshold." */
    /*  */
    /*  Aubert, A. E., D. Ramaekers, et al. (1999). "The analysis of heart  */
    /*  rate variability in unrestrained rats. Validation of method and  */
    /*  results." Comput Methods Programs Biomed 60(3): 197-213. */
    /* convert to logical array */
    /* preallocate   */
    iyLead = s->size[0];
    s_idx_0 = (unsigned int)s->size[0];
    ia = a->size[0] * a->size[1];
    a->size[0] = (int)s_idx_0;
    a->size[1] = 2;
    emxEnsureCapacity((emxArray__common *)a, ia, (int)sizeof(double));
    for (orderForDim = 1; orderForDim <= iyLead; orderForDim++) {
        a->data[orderForDim - 1] = (double)orderForDim / (double)iyLead;
        a->data[(orderForDim + a->size[0]) - 1] = 1.0;
    }
    mldivide(a, s, b_y1);
    emxInit_real_T(&C, 1);
    if (b_y1->size[0] == 1) {
        ia = C->size[0];
        C->size[0] = a->size[0];
        emxEnsureCapacity((emxArray__common *)C, ia, (int)sizeof(double));
        ic = a->size[0];
        for (ia = 0; ia < ic; ia++) {
            C->data[ia] = 0.0;
            for (iyLead = 0; iyLead < 2; iyLead++) {
                C->data[ia] += a->data[ia + a->size[0] * iyLead] * b_y1->data[iyLead];
            }
        }
    } else {
        s_idx_0 = (unsigned int)a->size[0];
        ia = C->size[0];
        C->size[0] = (int)s_idx_0;
        emxEnsureCapacity((emxArray__common *)C, ia, (int)sizeof(double));
        m = a->size[0];
        orderForDim = C->size[0];
        ia = C->size[0];
        C->size[0] = orderForDim;
        emxEnsureCapacity((emxArray__common *)C, ia, (int)sizeof(double));
        for (ia = 0; ia < orderForDim; ia++) {
            C->data[ia] = 0.0;
        }
        if (a->size[0] == 0) {
        } else {
            orderForDim = 0;
            while ((m > 0) && (orderForDim <= 0)) {
                for (ic = 1; ic <= m; ic++) {
                    C->data[ic - 1] = 0.0;
                }
                orderForDim = m;
            }
            br = 0;
            orderForDim = 0;
            while ((m > 0) && (orderForDim <= 0)) {
                orderForDim = 0;
                for (iyLead = br; iyLead + 1 <= br + 2; iyLead++) {
                    if (b_y1->data[iyLead] != 0.0) {
                        ia = orderForDim;
                        for (ic = 0; ic + 1 <= m; ic++) {
                            ia++;
                            C->data[ic] += b_y1->data[iyLead] * a->data[ia - 1];
                        }
                    }
                    orderForDim += m;
                }
                br += 2;
                orderForDim = m;
            }
        }
    }
    emxFree_real_T(&a);
    ia = s->size[0];
    emxEnsureCapacity((emxArray__common *)s, ia, (int)sizeof(double));
    ic = s->size[0];
    for (ia = 0; ia < ic; ia++) {
        s->data[ia] -= C->data[ia];
    }
    emxFree_real_T(&C);
    mu = mean(s);
    /* mean */
    orderForDim = s->size[0];
    if (s->size[0] > 1) {
        iyLead = s->size[0] - 1;
    } else {
        iyLead = s->size[0];
    }
    if (s->size[0] == 0) {
        tmp2 = 0.0;
    } else {
        ia = 0;
        work_data_idx_0 = s->data[0];
        for (ic = 2; ic <= orderForDim; ic++) {
            ia++;
            work_data_idx_0 += s->data[ia];
        }
        work_data_idx_0 /= (double)s->size[0];
        ia = 0;
        tmp1 = s->data[0] - work_data_idx_0;
        tmp2 = tmp1 * tmp1;
        for (ic = 2; ic <= orderForDim; ic++) {
            ia++;
            tmp1 = s->data[ia] - work_data_idx_0;
            tmp2 += tmp1 * tmp1;
        }
        tmp2 /= (double)iyLead;
    }
    emxInit_real_T(&b_s, 1);
    /* standard deviation */
    /*  Create a matrix of mean values by replicating the mu vector for n rows */
    repmat(mu, (double)s->size[0], b_y1);
    /*  Create a matrix of standard deviation values by replicating the sigma vector for n rows */
    repmat(std::sqrt(tmp2), (double)s->size[0], SigmaMat);
    /*  Create a matrix of zeros and ones, where ones indicate the location of outliers */
    ia = b_s->size[0];
    b_s->size[0] = s->size[0];
    emxEnsureCapacity((emxArray__common *)b_s, ia, (int)sizeof(double));
    ic = s->size[0];
    for (ia = 0; ia < ic; ia++) {
        b_s->data[ia] = s->data[ia] - b_y1->data[ia];
    }
    b_abs(b_s, b_y1);
    ia = outliers2->size[0];
    outliers2->size[0] = b_y1->size[0];
    emxEnsureCapacity((emxArray__common *)outliers2, ia, (int)sizeof(boolean_T));
    ic = b_y1->size[0];
    emxFree_real_T(&b_s);
    for (ia = 0; ia < ic; ia++) {
        outliers2->data[ia] = (b_y1->data[ia] > 3.0 * SigmaMat->data[ia]);
    }
    emxFree_real_T(&b_y1);
    emxFree_real_T(&SigmaMat);
    /*  Reference:  */
    /*  Aubert, A. E., D. Ramaekers, et al. (1999). "The analysis of heart  */
    /*  rate variability in unrestrained rats. Validation of method and  */
    /*  results." Comput Methods Programs Biomed 60(3): 197-213. */
    /* convert to logical array */
}
Esempio n. 12
0
static void eml_qrsolve(const emxArray_real_T *A, emxArray_real_T *B,
  emxArray_real_T *Y)
{
  emxArray_real_T *b_A;
  real_T wj;
  real_T s;
  int32_T mn;
  int32_T nmip1;
  int32_T itemp;
  emxArray_real_T *tau;
  emxArray_int32_T *jpvt;
  emxArray_real_T *work;
  int32_T m;
  int32_T n;
  int32_T b_mn;
  emxArray_real_T *vn1;
  emxArray_real_T *vn2;
  int32_T k;
  int32_T ix;
  int32_T i;
  int32_T i_i;
  int32_T nmi;
  int32_T mmi;
  real_T rankR;
  real_T y;
  boolean_T exitg1;
  uint32_T unnamed_idx_0;
  b_emxInit_real_T(&b_A, 2);
  wj = (real_T)A->size[0];
  s = (real_T)A->size[1];
  if (wj <= s) {
    s = wj;
  }

  mn = (int32_T)s - 1;
  nmip1 = b_A->size[0] * b_A->size[1];
  b_A->size[0] = A->size[0];
  b_A->size[1] = A->size[1];
  emxEnsureCapacity((emxArray__common *)b_A, nmip1, (int32_T)sizeof(real_T));
  itemp = A->size[0] * A->size[1] - 1;
  for (nmip1 = 0; nmip1 <= itemp; nmip1++) {
    b_A->data[nmip1] = A->data[nmip1];
  }

  emxInit_real_T(&tau, 1);
  emxInit_int32_T(&jpvt, 2);
  emxInit_real_T(&work, 1);
  m = A->size[0];
  n = A->size[1];
  if (m <= n) {
    b_mn = m;
  } else {
    b_mn = n;
  }

  nmip1 = tau->size[0];
  tau->size[0] = b_mn;
  emxEnsureCapacity((emxArray__common *)tau, nmip1, (int32_T)sizeof(real_T));
  eml_signed_integer_colon(n, jpvt);
  nmip1 = work->size[0];
  work->size[0] = n;
  emxEnsureCapacity((emxArray__common *)work, nmip1, (int32_T)sizeof(real_T));
  itemp = n - 1;
  for (nmip1 = 0; nmip1 <= itemp; nmip1++) {
    work->data[nmip1] = 0.0;
  }

  emxInit_real_T(&vn1, 1);
  emxInit_real_T(&vn2, 1);
  nmip1 = vn1->size[0];
  vn1->size[0] = n;
  emxEnsureCapacity((emxArray__common *)vn1, nmip1, (int32_T)sizeof(real_T));
  nmip1 = vn2->size[0];
  vn2->size[0] = vn1->size[0];
  emxEnsureCapacity((emxArray__common *)vn2, nmip1, (int32_T)sizeof(real_T));
  k = 1;
  for (ix = 0; ix + 1 <= n; ix++) {
    vn1->data[ix] = eml_xnrm2(m, A, k);
    vn2->data[ix] = vn1->data[ix];
    k += m;
  }

  for (i = 0; i + 1 <= b_mn; i++) {
    i_i = i + i * m;
    nmi = (n - i) - 1;
    mmi = (m - i) - 1;
    nmip1 = 1 + nmi;
    if (nmip1 < 1) {
      itemp = -1;
    } else {
      itemp = 0;
      if (nmip1 > 1) {
        ix = i;
        wj = fabs(vn1->data[i]);
        for (k = 2; k <= nmip1; k++) {
          ix++;
          s = fabs(vn1->data[ix]);
          if (s > wj) {
            itemp = k - 1;
            wj = s;
          }
        }
      }
    }

    nmip1 = i + itemp;
    if (nmip1 + 1 != i + 1) {
      eml_xswap(m, b_A, m * nmip1 + 1, 1, m * i + 1, 1);
      itemp = jpvt->data[nmip1];
      jpvt->data[nmip1] = jpvt->data[i];
      jpvt->data[i] = itemp;
      vn1->data[nmip1] = vn1->data[i];
      vn2->data[nmip1] = vn2->data[i];
    }

    if (i + 1 < m) {
      k = i_i + 2;
      rankR = b_A->data[i_i];
      y = 0.0;
      if (mmi + 1 <= 0) {
      } else {
        wj = eml_xnrm2(mmi, b_A, k);
        if (wj != 0.0) {
          s = rt_hypotd_snf(fabs(b_A->data[i_i]), wj);
          if (b_A->data[i_i] >= 0.0) {
            s = -s;
          }

          if (fabs(s) < 1.0020841800044864E-292) {
            nmip1 = 0;
            do {
              nmip1++;
              eml_xscal(mmi, 9.9792015476736E+291, b_A, k);
              s *= 9.9792015476736E+291;
              rankR *= 9.9792015476736E+291;
            } while (!(fabs(s) >= 1.0020841800044864E-292));

            wj = eml_xnrm2(mmi, b_A, k);
            s = rt_hypotd_snf(fabs(rankR), wj);
            if (rankR >= 0.0) {
              s = -s;
            }

            y = (s - rankR) / s;
            eml_xscal(mmi, 1.0 / (rankR - s), b_A, k);
            for (k = 1; k <= nmip1; k++) {
              s *= 1.0020841800044864E-292;
            }

            rankR = s;
          } else {
            y = (s - b_A->data[i_i]) / s;
            wj = 1.0 / (b_A->data[i_i] - s);
            eml_xscal(mmi, wj, b_A, k);
            rankR = s;
          }
        }
      }

      tau->data[i] = y;
    } else {
      wj = b_A->data[i_i];
      rankR = b_A->data[i_i];
      b_A->data[i_i] = wj;
      tau->data[i] = 0.0;
    }

    b_A->data[i_i] = rankR;
    if (i + 1 < n) {
      rankR = b_A->data[i_i];
      b_A->data[i_i] = 1.0;
      eml_matlab_zlarf(mmi + 1, nmi, i_i + 1, tau->data[i], b_A, (i + (i + 1) *
        m) + 1, m, work);
      b_A->data[i_i] = rankR;
    }

    for (ix = i + 1; ix + 1 <= n; ix++) {
      if (vn1->data[ix] != 0.0) {
        s = fabs(b_A->data[i + b_A->size[0] * ix]) / vn1->data[ix];
        y = s * s;
        s = 1.0 - s * s;
        if (1.0 - y < 0.0) {
          s = 0.0;
        }

        wj = vn1->data[ix] / vn2->data[ix];
        if (s * (wj * wj) <= 1.4901161193847656E-8) {
          if (i + 1 < m) {
            k = (i + m * ix) + 1;
            y = 0.0;
            if (mmi < 1) {
            } else if (mmi == 1) {
              y = fabs(b_A->data[k]);
            } else {
              wj = 2.2250738585072014E-308;
              itemp = k + mmi;
              while (k + 1 <= itemp) {
                s = fabs(b_A->data[k]);
                if (s > wj) {
                  rankR = wj / s;
                  y = 1.0 + y * rankR * rankR;
                  wj = s;
                } else {
                  rankR = s / wj;
                  y += rankR * rankR;
                }

                k++;
              }

              y = wj * sqrt(y);
            }

            vn1->data[ix] = y;
            vn2->data[ix] = vn1->data[ix];
          } else {
            vn1->data[ix] = 0.0;
            vn2->data[ix] = 0.0;
          }
        } else {
          vn1->data[ix] *= sqrt(s);
        }
      }
    }
  }

  emxFree_real_T(&vn2);
  emxFree_real_T(&vn1);
  emxFree_real_T(&work);
  rankR = 0.0;
  k = 0;
  exitg1 = FALSE;
  while ((exitg1 == 0U) && (k <= mn)) {
    wj = (real_T)A->size[0];
    s = (real_T)A->size[1];
    if (wj >= s) {
      s = wj;
    }

    if (fabs(b_A->data[k + b_A->size[0] * k]) <= s * fabs(b_A->data[0]) *
        2.2204460492503131E-16) {
      exitg1 = TRUE;
    } else {
      rankR++;
      k++;
    }
  }

  unnamed_idx_0 = (uint32_T)A->size[1];
  nmip1 = Y->size[0];
  Y->size[0] = (int32_T)unnamed_idx_0;
  emxEnsureCapacity((emxArray__common *)Y, nmip1, (int32_T)sizeof(real_T));
  itemp = (int32_T)unnamed_idx_0 - 1;
  for (nmip1 = 0; nmip1 <= itemp; nmip1++) {
    Y->data[nmip1] = 0.0;
  }

  for (ix = 0; ix <= mn; ix++) {
    if (tau->data[ix] != 0.0) {
      wj = B->data[ix];
      nmip1 = A->size[0] + (int32_T)(1.0 - ((1.0 + (real_T)ix) + 1.0));
      for (i = 0; i <= nmip1 - 1; i++) {
        unnamed_idx_0 = ((uint32_T)ix + (uint32_T)i) + 2U;
        wj += b_A->data[((int32_T)unnamed_idx_0 + b_A->size[0] * ix) - 1] *
          B->data[(int32_T)unnamed_idx_0 - 1];
      }

      wj *= tau->data[ix];
      if (wj != 0.0) {
        B->data[ix] -= wj;
        nmip1 = A->size[0] + (int32_T)(1.0 - ((1.0 + (real_T)ix) + 1.0));
        for (i = 0; i <= nmip1 - 1; i++) {
          unnamed_idx_0 = ((uint32_T)ix + (uint32_T)i) + 2U;
          B->data[(int32_T)unnamed_idx_0 - 1] -= b_A->data[((int32_T)
            unnamed_idx_0 + b_A->size[0] * ix) - 1] * wj;
        }
      }
    }
  }

  emxFree_real_T(&tau);
  for (i = 0; i <= (int32_T)rankR - 1; i++) {
    Y->data[jpvt->data[(int32_T)(1.0 + (real_T)i) - 1] - 1] = B->data[(int32_T)
      (1.0 + (real_T)i) - 1];
  }

  for (ix = 0; ix <= (int32_T)-(1.0 + (-1.0 - rankR)) - 1; ix++) {
    wj = rankR + -(real_T)ix;
    Y->data[jpvt->data[(int32_T)wj - 1] - 1] = eml_div(Y->data[jpvt->data
      [(int32_T)wj - 1] - 1], b_A->data[((int32_T)wj + b_A->size[0] * ((int32_T)
      wj - 1)) - 1]);
    for (i = 0; i <= (int32_T)wj - 2; i++) {
      Y->data[jpvt->data[(int32_T)(1.0 + (real_T)i) - 1] - 1] -= Y->data
        [jpvt->data[(int32_T)wj - 1] - 1] * b_A->data[((int32_T)(1.0 + (real_T)i)
        + b_A->size[0] * ((int32_T)wj - 1)) - 1];
    }
  }

  emxFree_int32_T(&jpvt);
  emxFree_real_T(&b_A);
}
Esempio n. 13
0
/* Function Definitions */
void occflow(const emlrtStack *sp, const emxArray_real_T *cgridvec,
             emxArray_real_T *cgridvecprev, emxArray_real_T *context, const
             emxArray_real_T *nei_idx, const emxArray_real_T *nei_weight, real_T
             nei_filter_n, const emxArray_real_T *nei4u_idx, const
             emxArray_real_T *nei4u_weight, real_T nei4u_filter_n, real_T occval,
             real_T minthreshold, real_T maxthreshold, real_T reinitval, real_T
             intensifyrate, real_T nocc_attenuaterate, real_T
             unknown_attenuaterate, real_T sigm_coef, real_T
             do_attenuation_first, emxArray_real_T *predvec, emxArray_real_T
             *maxvec)
{
  emxArray_boolean_T *x;
  int32_T ix;
  int32_T idx;
  emxArray_boolean_T *r0;
  int32_T nx;
  emxArray_int32_T *ii;
  boolean_T overflow;
  int32_T iy;
  boolean_T exitg6;
  boolean_T guard3 = false;
  boolean_T guard4 = false;
  emxArray_real_T *newlyoccidx;
  boolean_T exitg5;
  boolean_T guard2 = false;
  boolean_T b_guard3 = false;
  emxArray_real_T *occidx;
  boolean_T exitg4;
  boolean_T guard1 = false;
  boolean_T b_guard2 = false;
  emxArray_real_T *noccidx;
  int32_T nrnocc;
  int32_T j;
  emxArray_real_T *curr_col;
  emxArray_real_T *updt_col;
  emxArray_real_T *z;
  int32_T coccidx;
  boolean_T b_guard1 = false;
  int32_T ixstart;
  int32_T n;
  real_T mtmp;
  boolean_T exitg3;
  int32_T varargin_1[2];
  int32_T k;
  int32_T iv3[2];
  int32_T iv4[2];
  real_T d0;
  emxArray_real_T *tempcontext;
  emxArray_real_T *b_nei4u_weight;
  real_T sumval;
  int32_T m;
  int32_T iv5[2];
  boolean_T b_ix;
  boolean_T exitg2;
  boolean_T b_ixstart;
  int32_T varargin_2[2];
  boolean_T p;
  boolean_T exitg1;
  emlrtStack st;
  emlrtStack b_st;
  emlrtStack c_st;
  emlrtStack d_st;
  emlrtStack e_st;
  emlrtStack f_st;
  (void)unknown_attenuaterate;
  st.prev = sp;
  st.tls = sp->tls;
  b_st.prev = &st;
  b_st.tls = st.tls;
  c_st.prev = &b_st;
  c_st.tls = b_st.tls;
  d_st.prev = &c_st;
  d_st.tls = c_st.tls;
  e_st.prev = &d_st;
  e_st.tls = d_st.tls;
  f_st.prev = &e_st;
  f_st.tls = e_st.tls;
  emlrtHeapReferenceStackEnterFcnR2012b(sp);
  emxInit_boolean_T(sp, &x, 1, &emlrtRTEI, true);

  /*  */
  /*  Occupancy flow with vector input  */
  /*  */
  /*  Compute indices first  */
  ix = x->size[0];
  x->size[0] = cgridvec->size[0];
  emxEnsureCapacity(sp, (emxArray__common *)x, ix, (int32_T)sizeof(boolean_T),
                    &emlrtRTEI);
  idx = cgridvec->size[0];
  for (ix = 0; ix < idx; ix++) {
    x->data[ix] = (cgridvec->data[ix] == occval);
  }

  emxInit_boolean_T(sp, &r0, 1, &emlrtRTEI, true);
  ix = r0->size[0];
  r0->size[0] = cgridvecprev->size[0];
  emxEnsureCapacity(sp, (emxArray__common *)r0, ix, (int32_T)sizeof(boolean_T),
                    &emlrtRTEI);
  idx = cgridvecprev->size[0];
  for (ix = 0; ix < idx; ix++) {
    r0->data[ix] = (cgridvecprev->data[ix] != occval);
  }

  ix = x->size[0];
  nx = r0->size[0];
  if (ix != nx) {
    emlrtSizeEqCheck1DR2012b(ix, nx, &emlrtECI, sp);
  }

  st.site = &emlrtRSI;
  ix = x->size[0];
  emxEnsureCapacity(&st, (emxArray__common *)x, ix, (int32_T)sizeof(boolean_T),
                    &emlrtRTEI);
  idx = x->size[0];
  for (ix = 0; ix < idx; ix++) {
    x->data[ix] = (x->data[ix] && r0->data[ix]);
  }

  emxFree_boolean_T(&r0);
  emxInit_int32_T(&st, &ii, 1, &l_emlrtRTEI, true);
  b_st.site = &i_emlrtRSI;
  nx = x->size[0];
  idx = 0;
  ix = ii->size[0];
  ii->size[0] = x->size[0];
  emxEnsureCapacity(&b_st, (emxArray__common *)ii, ix, (int32_T)sizeof(int32_T),
                    &emlrtRTEI);
  c_st.site = &j_emlrtRSI;
  if (1 > x->size[0]) {
    overflow = false;
  } else {
    overflow = (x->size[0] > 2147483646);
  }

  if (overflow) {
    d_st.site = &l_emlrtRSI;
    check_forloop_overflow_error(&d_st);
  }

  iy = 1;
  exitg6 = false;
  while ((!exitg6) && (iy <= nx)) {
    guard3 = false;
    if (x->data[iy - 1]) {
      idx++;
      ii->data[idx - 1] = iy;
      if (idx >= nx) {
        exitg6 = true;
      } else {
        guard3 = true;
      }
    } else {
      guard3 = true;
    }

    if (guard3) {
      iy++;
    }
  }

  if (idx <= x->size[0]) {
  } else {
    emlrtErrorWithMessageIdR2012b(&b_st, &s_emlrtRTEI,
      "Coder:builtins:AssertionFailed", 0);
  }

  if (x->size[0] == 1) {
    if (idx == 0) {
      ix = ii->size[0];
      ii->size[0] = 0;
      emxEnsureCapacity(&b_st, (emxArray__common *)ii, ix, (int32_T)sizeof
                        (int32_T), &emlrtRTEI);
    }
  } else {
    if (1 > idx) {
      ix = 0;
    } else {
      ix = idx;
    }

    c_st.site = &k_emlrtRSI;
    overflow = !(ii->size[0] != 1);
    guard4 = false;
    if (overflow) {
      overflow = false;
      if (ix != 1) {
        overflow = true;
      }

      if (overflow) {
        overflow = true;
      } else {
        guard4 = true;
      }
    } else {
      guard4 = true;
    }

    if (guard4) {
      overflow = false;
    }

    d_st.site = &m_emlrtRSI;
    if (!overflow) {
    } else {
      emlrtErrorWithMessageIdR2012b(&d_st, &t_emlrtRTEI,
        "Coder:FE:PotentialVectorVector", 0);
    }

    nx = ii->size[0];
    ii->size[0] = ix;
    emxEnsureCapacity(&b_st, (emxArray__common *)ii, nx, (int32_T)sizeof(int32_T),
                      &c_emlrtRTEI);
  }

  emxInit_real_T(&b_st, &newlyoccidx, 1, &f_emlrtRTEI, true);
  ix = newlyoccidx->size[0];
  newlyoccidx->size[0] = ii->size[0];
  emxEnsureCapacity(&st, (emxArray__common *)newlyoccidx, ix, (int32_T)sizeof
                    (real_T), &emlrtRTEI);
  idx = ii->size[0];
  for (ix = 0; ix < idx; ix++) {
    newlyoccidx->data[ix] = ii->data[ix];
  }

  st.site = &b_emlrtRSI;
  ix = x->size[0];
  x->size[0] = cgridvec->size[0];
  emxEnsureCapacity(&st, (emxArray__common *)x, ix, (int32_T)sizeof(boolean_T),
                    &emlrtRTEI);
  idx = cgridvec->size[0];
  for (ix = 0; ix < idx; ix++) {
    x->data[ix] = (cgridvec->data[ix] == occval);
  }

  b_st.site = &i_emlrtRSI;
  nx = x->size[0];
  idx = 0;
  ix = ii->size[0];
  ii->size[0] = x->size[0];
  emxEnsureCapacity(&b_st, (emxArray__common *)ii, ix, (int32_T)sizeof(int32_T),
                    &emlrtRTEI);
  c_st.site = &j_emlrtRSI;
  if (1 > x->size[0]) {
    overflow = false;
  } else {
    overflow = (x->size[0] > 2147483646);
  }

  if (overflow) {
    d_st.site = &l_emlrtRSI;
    check_forloop_overflow_error(&d_st);
  }

  iy = 1;
  exitg5 = false;
  while ((!exitg5) && (iy <= nx)) {
    guard2 = false;
    if (x->data[iy - 1]) {
      idx++;
      ii->data[idx - 1] = iy;
      if (idx >= nx) {
        exitg5 = true;
      } else {
        guard2 = true;
      }
    } else {
      guard2 = true;
    }

    if (guard2) {
      iy++;
    }
  }

  if (idx <= x->size[0]) {
  } else {
    emlrtErrorWithMessageIdR2012b(&b_st, &s_emlrtRTEI,
      "Coder:builtins:AssertionFailed", 0);
  }

  if (x->size[0] == 1) {
    if (idx == 0) {
      ix = ii->size[0];
      ii->size[0] = 0;
      emxEnsureCapacity(&b_st, (emxArray__common *)ii, ix, (int32_T)sizeof
                        (int32_T), &emlrtRTEI);
    }
  } else {
    if (1 > idx) {
      ix = 0;
    } else {
      ix = idx;
    }

    c_st.site = &k_emlrtRSI;
    overflow = !(ii->size[0] != 1);
    b_guard3 = false;
    if (overflow) {
      overflow = false;
      if (ix != 1) {
        overflow = true;
      }

      if (overflow) {
        overflow = true;
      } else {
        b_guard3 = true;
      }
    } else {
      b_guard3 = true;
    }

    if (b_guard3) {
      overflow = false;
    }

    d_st.site = &m_emlrtRSI;
    if (!overflow) {
    } else {
      emlrtErrorWithMessageIdR2012b(&d_st, &t_emlrtRTEI,
        "Coder:FE:PotentialVectorVector", 0);
    }

    nx = ii->size[0];
    ii->size[0] = ix;
    emxEnsureCapacity(&b_st, (emxArray__common *)ii, nx, (int32_T)sizeof(int32_T),
                      &c_emlrtRTEI);
  }

  emxInit_real_T(&b_st, &occidx, 1, &g_emlrtRTEI, true);
  ix = occidx->size[0];
  occidx->size[0] = ii->size[0];
  emxEnsureCapacity(&st, (emxArray__common *)occidx, ix, (int32_T)sizeof(real_T),
                    &emlrtRTEI);
  idx = ii->size[0];
  for (ix = 0; ix < idx; ix++) {
    occidx->data[ix] = ii->data[ix];
  }

  st.site = &c_emlrtRSI;
  ix = x->size[0];
  x->size[0] = cgridvec->size[0];
  emxEnsureCapacity(&st, (emxArray__common *)x, ix, (int32_T)sizeof(boolean_T),
                    &emlrtRTEI);
  idx = cgridvec->size[0];
  for (ix = 0; ix < idx; ix++) {
    x->data[ix] = (cgridvec->data[ix] != occval);
  }

  b_st.site = &i_emlrtRSI;
  nx = x->size[0];
  idx = 0;
  ix = ii->size[0];
  ii->size[0] = x->size[0];
  emxEnsureCapacity(&b_st, (emxArray__common *)ii, ix, (int32_T)sizeof(int32_T),
                    &emlrtRTEI);
  c_st.site = &j_emlrtRSI;
  if (1 > x->size[0]) {
    overflow = false;
  } else {
    overflow = (x->size[0] > 2147483646);
  }

  if (overflow) {
    d_st.site = &l_emlrtRSI;
    check_forloop_overflow_error(&d_st);
  }

  iy = 1;
  exitg4 = false;
  while ((!exitg4) && (iy <= nx)) {
    guard1 = false;
    if (x->data[iy - 1]) {
      idx++;
      ii->data[idx - 1] = iy;
      if (idx >= nx) {
        exitg4 = true;
      } else {
        guard1 = true;
      }
    } else {
      guard1 = true;
    }

    if (guard1) {
      iy++;
    }
  }

  if (idx <= x->size[0]) {
  } else {
    emlrtErrorWithMessageIdR2012b(&b_st, &s_emlrtRTEI,
      "Coder:builtins:AssertionFailed", 0);
  }

  if (x->size[0] == 1) {
    if (idx == 0) {
      ix = ii->size[0];
      ii->size[0] = 0;
      emxEnsureCapacity(&b_st, (emxArray__common *)ii, ix, (int32_T)sizeof
                        (int32_T), &emlrtRTEI);
    }
  } else {
    if (1 > idx) {
      ix = 0;
    } else {
      ix = idx;
    }

    c_st.site = &k_emlrtRSI;
    overflow = !(ii->size[0] != 1);
    b_guard2 = false;
    if (overflow) {
      overflow = false;
      if (ix != 1) {
        overflow = true;
      }

      if (overflow) {
        overflow = true;
      } else {
        b_guard2 = true;
      }
    } else {
      b_guard2 = true;
    }

    if (b_guard2) {
      overflow = false;
    }

    d_st.site = &m_emlrtRSI;
    if (!overflow) {
    } else {
      emlrtErrorWithMessageIdR2012b(&d_st, &t_emlrtRTEI,
        "Coder:FE:PotentialVectorVector", 0);
    }

    nx = ii->size[0];
    ii->size[0] = ix;
    emxEnsureCapacity(&b_st, (emxArray__common *)ii, nx, (int32_T)sizeof(int32_T),
                      &c_emlrtRTEI);
  }

  emxFree_boolean_T(&x);
  emxInit_real_T(&b_st, &noccidx, 1, &h_emlrtRTEI, true);
  ix = noccidx->size[0];
  noccidx->size[0] = ii->size[0];
  emxEnsureCapacity(&st, (emxArray__common *)noccidx, ix, (int32_T)sizeof(real_T),
                    &emlrtRTEI);
  idx = ii->size[0];
  for (ix = 0; ix < idx; ix++) {
    noccidx->data[ix] = ii->data[ix];
  }

  nrnocc = noccidx->size[0] - 1;

  /*  1 Intensify newly occupied cells  */
  j = 0;
  emxInit_real_T1(sp, &curr_col, 2, &i_emlrtRTEI, true);
  emxInit_real_T1(sp, &updt_col, 2, &j_emlrtRTEI, true);
  emxInit_real_T1(sp, &z, 2, &emlrtRTEI, true);
  while (j <= newlyoccidx->size[0] - 1) {
    /*  For newly occupied cells  */
    ix = newlyoccidx->size[0];
    if (!((j + 1 >= 1) && (j + 1 <= ix))) {
      emlrtDynamicBoundsCheckR2012b(j + 1, 1, ix, &eb_emlrtBCI, sp);
    }

    coccidx = (int32_T)newlyoccidx->data[j] - 1;
    ix = context->size[0];
    nx = (int32_T)newlyoccidx->data[j];
    if (!((nx >= 1) && (nx <= ix))) {
      emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &emlrtBCI, sp);
    }

    st.site = &d_emlrtRSI;
    b_st.site = &n_emlrtRSI;
    c_st.site = &o_emlrtRSI;
    ix = context->size[1];
    b_guard1 = false;
    if (ix == 1) {
      b_guard1 = true;
    } else {
      ix = context->size[1];
      if (ix != 1) {
        b_guard1 = true;
      } else {
        overflow = false;
      }
    }

    if (b_guard1) {
      overflow = true;
    }

    if (overflow) {
    } else {
      emlrtErrorWithMessageIdR2012b(&c_st, &u_emlrtRTEI,
        "Coder:toolbox:autoDimIncompatibility", 0);
    }

    ix = context->size[1];
    if (ix > 0) {
    } else {
      emlrtErrorWithMessageIdR2012b(&c_st, &v_emlrtRTEI,
        "Coder:toolbox:eml_min_or_max_varDimZero", 0);
    }

    d_st.site = &p_emlrtRSI;
    ixstart = 1;
    n = context->size[1];
    nx = (int32_T)newlyoccidx->data[j];
    mtmp = context->data[nx - 1];
    ix = context->size[1];
    if (ix > 1) {
      if (muDoubleScalarIsNaN(mtmp)) {
        e_st.site = &r_emlrtRSI;
        ix = context->size[1];
        if (2 > ix) {
          overflow = false;
        } else {
          ix = context->size[1];
          overflow = (ix > 2147483646);
        }

        if (overflow) {
          f_st.site = &l_emlrtRSI;
          check_forloop_overflow_error(&f_st);
        }

        ix = 2;
        exitg3 = false;
        while ((!exitg3) && (ix <= n)) {
          ixstart = ix;
          if (!muDoubleScalarIsNaN(context->data[coccidx + context->size[0] *
               (ix - 1)])) {
            mtmp = context->data[coccidx + context->size[0] * (ix - 1)];
            exitg3 = true;
          } else {
            ix++;
          }
        }
      }

      ix = context->size[1];
      if (ixstart < ix) {
        e_st.site = &q_emlrtRSI;
        ix = context->size[1];
        if (ixstart + 1 > ix) {
          overflow = false;
        } else {
          ix = context->size[1];
          overflow = (ix > 2147483646);
        }

        if (overflow) {
          f_st.site = &l_emlrtRSI;
          check_forloop_overflow_error(&f_st);
        }

        for (ix = ixstart + 1; ix <= n; ix++) {
          if (context->data[coccidx + context->size[0] * (ix - 1)] > mtmp) {
            mtmp = context->data[coccidx + context->size[0] * (ix - 1)];
          }
        }
      }
    }

    if (mtmp < minthreshold) {
      idx = context->size[1];
      iy = context->size[0];
      nx = (int32_T)newlyoccidx->data[j];
      if (!((nx >= 1) && (nx <= iy))) {
        emlrtDynamicBoundsCheckR2012b(nx, 1, iy, &b_emlrtBCI, sp);
      }

      for (ix = 0; ix < idx; ix++) {
        context->data[(nx + context->size[0] * ix) - 1] = reinitval;
      }

      /*  Reinitialize */
    } else {
      idx = context->size[1];
      nx = (int32_T)newlyoccidx->data[j];
      ix = updt_col->size[0] * updt_col->size[1];
      updt_col->size[0] = 1;
      updt_col->size[1] = idx;
      emxEnsureCapacity(sp, (emxArray__common *)updt_col, ix, (int32_T)sizeof
                        (real_T), &emlrtRTEI);
      for (ix = 0; ix < idx; ix++) {
        updt_col->data[updt_col->size[0] * ix] = intensifyrate * context->data
          [(nx + context->size[0] * ix) - 1];
      }

      /*  Intensify */
      st.site = &e_emlrtRSI;
      b_st.site = &s_emlrtRSI;
      c_st.site = &o_emlrtRSI;
      d_st.site = &t_emlrtRSI;
      ix = curr_col->size[0] * curr_col->size[1];
      curr_col->size[0] = 1;
      curr_col->size[1] = updt_col->size[1];
      emxEnsureCapacity(&d_st, (emxArray__common *)curr_col, ix, (int32_T)sizeof
                        (real_T), &emlrtRTEI);
      idx = updt_col->size[0] * updt_col->size[1];
      for (ix = 0; ix < idx; ix++) {
        curr_col->data[ix] = updt_col->data[ix];
      }

      e_st.site = &u_emlrtRSI;
      for (ix = 0; ix < 2; ix++) {
        varargin_1[ix] = updt_col->size[ix];
      }

      ix = z->size[0] * z->size[1];
      z->size[0] = 1;
      z->size[1] = updt_col->size[1];
      emxEnsureCapacity(&e_st, (emxArray__common *)z, ix, (int32_T)sizeof(real_T),
                        &d_emlrtRTEI);
      iy = updt_col->size[1];
      ix = updt_col->size[0] * updt_col->size[1];
      updt_col->size[0] = 1;
      updt_col->size[1] = varargin_1[1];
      emxEnsureCapacity(&e_st, (emxArray__common *)updt_col, ix, (int32_T)sizeof
                        (real_T), &e_emlrtRTEI);
      if (dimagree(updt_col, curr_col)) {
      } else {
        emlrtErrorWithMessageIdR2012b(&e_st, &x_emlrtRTEI, "MATLAB:dimagree", 0);
      }

      e_st.site = &v_emlrtRSI;
      if (1 > z->size[1]) {
        overflow = false;
      } else {
        overflow = (z->size[1] > 2147483646);
      }

      if (overflow) {
        f_st.site = &l_emlrtRSI;
        check_forloop_overflow_error(&f_st);
      }

      for (k = 0; k + 1 <= iy; k++) {
        updt_col->data[k] = muDoubleScalarMin(curr_col->data[k], maxthreshold);
      }

      /*  Max-thesholding */
      ix = context->size[0];
      nx = (int32_T)newlyoccidx->data[j];
      if (!((nx >= 1) && (nx <= ix))) {
        emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &c_emlrtBCI, sp);
      }

      idx = context->size[1];
      ix = ii->size[0];
      ii->size[0] = idx;
      emxEnsureCapacity(sp, (emxArray__common *)ii, ix, (int32_T)sizeof(int32_T),
                        &emlrtRTEI);
      for (ix = 0; ix < idx; ix++) {
        ii->data[ix] = ix;
      }

      iv3[0] = 1;
      iv3[1] = ii->size[0];
      emlrtSubAssignSizeCheckR2012b(iv3, 2, *(int32_T (*)[2])updt_col->size, 2,
        &b_emlrtECI, sp);
      nx = (int32_T)newlyoccidx->data[j];
      idx = updt_col->size[1];
      for (ix = 0; ix < idx; ix++) {
        context->data[(nx + context->size[0] * ii->data[ix]) - 1] =
          updt_col->data[updt_col->size[0] * ix];
      }
    }

    j++;
    if (*emlrtBreakCheckR2012bFlagVar != 0) {
      emlrtBreakCheckR2012b(sp);
    }
  }

  emxFree_real_T(&z);

  /*  2 Attenuate unoccupied cells */
  if (do_attenuation_first == 1.0) {
    j = 0;
    while (j <= nrnocc) {
      /*  For unoccupied cells */
      ix = noccidx->size[0];
      nx = j + 1;
      if (!((nx >= 1) && (nx <= ix))) {
        emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &d_emlrtBCI, sp);
      }

      ix = context->size[0];
      nx = (int32_T)noccidx->data[j];
      if (!((nx >= 1) && (nx <= ix))) {
        emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &e_emlrtBCI, sp);
      }

      idx = context->size[1];
      iy = (int32_T)noccidx->data[j];
      ix = updt_col->size[0] * updt_col->size[1];
      updt_col->size[0] = 1;
      updt_col->size[1] = idx;
      emxEnsureCapacity(sp, (emxArray__common *)updt_col, ix, (int32_T)sizeof
                        (real_T), &emlrtRTEI);
      for (ix = 0; ix < idx; ix++) {
        updt_col->data[updt_col->size[0] * ix] = context->data[(iy +
          context->size[0] * ix) - 1] * nocc_attenuaterate;
      }

      /*  Attenuate */
      ix = context->size[0];
      nx = (int32_T)noccidx->data[j];
      if (!((nx >= 1) && (nx <= ix))) {
        emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &f_emlrtBCI, sp);
      }

      idx = context->size[1];
      ix = ii->size[0];
      ii->size[0] = idx;
      emxEnsureCapacity(sp, (emxArray__common *)ii, ix, (int32_T)sizeof(int32_T),
                        &emlrtRTEI);
      for (ix = 0; ix < idx; ix++) {
        ii->data[ix] = ix;
      }

      iv4[0] = 1;
      iv4[1] = ii->size[0];
      emlrtSubAssignSizeCheckR2012b(iv4, 2, *(int32_T (*)[2])updt_col->size, 2,
        &c_emlrtECI, sp);
      iy = (int32_T)noccidx->data[j];
      idx = updt_col->size[1];
      for (ix = 0; ix < idx; ix++) {
        context->data[(iy + context->size[0] * ii->data[ix]) - 1] =
          updt_col->data[updt_col->size[0] * ix];
      }

      j++;
      if (*emlrtBreakCheckR2012bFlagVar != 0) {
        emlrtBreakCheckR2012b(sp);
      }
    }
  }

  /*  4 Propagation  */
  j = 0;
  while (j <= occidx->size[0] - 1) {
    /*  For occupied cells  */
    ix = occidx->size[0];
    if (!((j + 1 >= 1) && (j + 1 <= ix))) {
      emlrtDynamicBoundsCheckR2012b(j + 1, 1, ix, &bb_emlrtBCI, sp);
    }

    idx = context->size[1];
    ix = context->size[0];
    iy = (int32_T)occidx->data[j];
    if (!((iy >= 1) && (iy <= ix))) {
      emlrtDynamicBoundsCheckR2012b(iy, 1, ix, &g_emlrtBCI, sp);
    }

    ix = curr_col->size[0] * curr_col->size[1];
    curr_col->size[0] = 1;
    curr_col->size[1] = idx;
    emxEnsureCapacity(sp, (emxArray__common *)curr_col, ix, (int32_T)sizeof
                      (real_T), &emlrtRTEI);
    for (ix = 0; ix < idx; ix++) {
      curr_col->data[curr_col->size[0] * ix] = context->data[(iy + context->
        size[0] * ix) - 1];
    }

    ix = nei_idx->size[0];
    nx = (int32_T)occidx->data[j];
    if (!((nx >= 1) && (nx <= ix))) {
      emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &h_emlrtBCI, sp);
    }

    ix = nei_weight->size[0];
    nx = (int32_T)occidx->data[j];
    if (!((nx >= 1) && (nx <= ix))) {
      emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &i_emlrtBCI, sp);
    }

    emlrtForLoopVectorCheckR2012b(1.0, 1.0, nei_filter_n, mxDOUBLE_CLASS,
      (int32_T)nei_filter_n, &p_emlrtRTEI, sp);
    k = 0;
    while (k <= (int32_T)nei_filter_n - 1) {
      /*  For all neighbor cells  */
      ix = curr_col->size[1];
      nx = k + 1;
      if (!((nx >= 1) && (nx <= ix))) {
        emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &j_emlrtBCI, sp);
      }

      ix = nei_idx->size[1];
      nx = k + 1;
      if (!((nx >= 1) && (nx <= ix))) {
        emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &k_emlrtBCI, sp);
      }

      ix = nei_weight->size[1];
      nx = k + 1;
      if (!((nx >= 1) && (nx <= ix))) {
        emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &l_emlrtBCI, sp);
      }

      iy = (int32_T)occidx->data[j];
      if (nei_idx->data[(iy + nei_idx->size[0] * k) - 1] != 0.0) {
        /*  If properly connected, propagate */
        iy = (int32_T)occidx->data[j];
        d0 = nei_idx->data[(iy + nei_idx->size[0] * k) - 1];
        if (d0 != (int32_T)muDoubleScalarFloor(d0)) {
          emlrtIntegerCheckR2012b(d0, &emlrtDCI, sp);
        }

        ix = context->size[0];
        nx = (int32_T)d0;
        if (!((nx >= 1) && (nx <= ix))) {
          emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &m_emlrtBCI, sp);
        }

        ix = context->size[1];
        nx = k + 1;
        if (!((nx >= 1) && (nx <= ix))) {
          emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &n_emlrtBCI, sp);
        }

        /*  Maximum value thresholding  */
        iy = (int32_T)occidx->data[j];
        idx = (int32_T)occidx->data[j];
        nx = (int32_T)occidx->data[j];
        ix = context->size[0];
        nx = (int32_T)nei_idx->data[(nx + nei_idx->size[0] * k) - 1];
        if (!((nx >= 1) && (nx <= ix))) {
          emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &cb_emlrtBCI, sp);
        }

        ix = context->size[1];
        if (!((k + 1 >= 1) && (k + 1 <= ix))) {
          emlrtDynamicBoundsCheckR2012b(k + 1, 1, ix, &db_emlrtBCI, sp);
        }

        context->data[(nx + context->size[0] * k) - 1] = muDoubleScalarMax
          (context->data[((int32_T)nei_idx->data[(iy + nei_idx->size[0] * k) - 1]
                          + context->size[0] * k) - 1], muDoubleScalarMin
           (nei_weight->data[(idx + nei_weight->size[0] * k) - 1] *
            curr_col->data[k], maxthreshold));

        /*  Make sure current context propagation does not weaken the flow information */
      }

      k++;
      if (*emlrtBreakCheckR2012bFlagVar != 0) {
        emlrtBreakCheckR2012b(sp);
      }
    }

    j++;
    if (*emlrtBreakCheckR2012bFlagVar != 0) {
      emlrtBreakCheckR2012b(sp);
    }
  }

  emxFree_real_T(&occidx);
  emxInit_real_T1(sp, &tempcontext, 2, &k_emlrtRTEI, true);

  /*  5 Uncertainty in acceleration */
  ix = tempcontext->size[0] * tempcontext->size[1];
  tempcontext->size[0] = context->size[0];
  tempcontext->size[1] = context->size[1];
  emxEnsureCapacity(sp, (emxArray__common *)tempcontext, ix, (int32_T)sizeof
                    (real_T), &emlrtRTEI);
  idx = context->size[0] * context->size[1];
  for (ix = 0; ix < idx; ix++) {
    tempcontext->data[ix] = context->data[ix];
  }

  emlrtForLoopVectorCheckR2012b(1.0, 1.0, nei_filter_n, mxDOUBLE_CLASS, (int32_T)
    nei_filter_n, &q_emlrtRTEI, sp);
  j = 0;
  emxInit_real_T1(sp, &b_nei4u_weight, 2, &emlrtRTEI, true);
  while (j <= (int32_T)nei_filter_n - 1) {
    /*  For all context level */
    k = 0;
    while (k <= nei_idx->size[0] - 1) {
      /*  For all cells */
      sumval = 0.0;
      emlrtForLoopVectorCheckR2012b(1.0, 1.0, nei4u_filter_n, mxDOUBLE_CLASS,
        (int32_T)nei4u_filter_n, &r_emlrtRTEI, sp);
      m = 0;
      while (m <= (int32_T)nei4u_filter_n - 1) {
        ix = nei4u_idx->size[0];
        nx = k + 1;
        if (!((nx >= 1) && (nx <= ix))) {
          emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &o_emlrtBCI, sp);
        }

        ix = nei4u_idx->size[1];
        nx = m + 1;
        if (!((nx >= 1) && (nx <= ix))) {
          emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &p_emlrtBCI, sp);
        }

        ix = nei4u_weight->size[0];
        nx = k + 1;
        if (!((nx >= 1) && (nx <= ix))) {
          emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &q_emlrtBCI, sp);
        }

        ix = nei4u_weight->size[1];
        nx = m + 1;
        if (!((nx >= 1) && (nx <= ix))) {
          emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &r_emlrtBCI, sp);
        }

        idx = nei4u_weight->size[1];
        ix = nei4u_weight->size[0];
        nx = 1 + k;
        if (!((nx >= 1) && (nx <= ix))) {
          emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &s_emlrtBCI, sp);
        }

        ix = b_nei4u_weight->size[0] * b_nei4u_weight->size[1];
        b_nei4u_weight->size[0] = 1;
        b_nei4u_weight->size[1] = idx;
        emxEnsureCapacity(sp, (emxArray__common *)b_nei4u_weight, ix, (int32_T)
                          sizeof(real_T), &emlrtRTEI);
        for (ix = 0; ix < idx; ix++) {
          b_nei4u_weight->data[b_nei4u_weight->size[0] * ix] =
            nei4u_weight->data[(nx + nei4u_weight->size[0] * ix) - 1];
        }

        st.site = &f_emlrtRSI;
        mtmp = sum(&st, b_nei4u_weight);
        if (nei4u_idx->data[k + nei4u_idx->size[0] * m] != 0.0) {
          d0 = nei4u_idx->data[k + nei4u_idx->size[0] * m];
          if (d0 != (int32_T)muDoubleScalarFloor(d0)) {
            emlrtIntegerCheckR2012b(d0, &b_emlrtDCI, sp);
          }

          ix = context->size[0];
          nx = (int32_T)d0;
          if (!((nx >= 1) && (nx <= ix))) {
            emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &t_emlrtBCI, sp);
          }

          ix = context->size[1];
          nx = j + 1;
          if (!((nx >= 1) && (nx <= ix))) {
            emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &u_emlrtBCI, sp);
          }

          sumval += nei4u_weight->data[k + nei4u_weight->size[0] * m] / mtmp *
            context->data[((int32_T)nei4u_idx->data[k + nei4u_idx->size[0] * m]
                           + context->size[0] * j) - 1];
        }

        m++;
        if (*emlrtBreakCheckR2012bFlagVar != 0) {
          emlrtBreakCheckR2012b(sp);
        }
      }

      ix = tempcontext->size[0];
      nx = 1 + k;
      if (!((nx >= 1) && (nx <= ix))) {
        emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &y_emlrtBCI, sp);
      }

      ix = tempcontext->size[1];
      if (!((j + 1 >= 1) && (j + 1 <= ix))) {
        emlrtDynamicBoundsCheckR2012b(j + 1, 1, ix, &ab_emlrtBCI, sp);
      }

      tempcontext->data[(nx + tempcontext->size[0] * j) - 1] = sumval;
      k++;
      if (*emlrtBreakCheckR2012bFlagVar != 0) {
        emlrtBreakCheckR2012b(sp);
      }
    }

    j++;
    if (*emlrtBreakCheckR2012bFlagVar != 0) {
      emlrtBreakCheckR2012b(sp);
    }
  }

  emxFree_real_T(&b_nei4u_weight);
  ix = context->size[0] * context->size[1];
  context->size[0] = tempcontext->size[0];
  context->size[1] = tempcontext->size[1];
  emxEnsureCapacity(sp, (emxArray__common *)context, ix, (int32_T)sizeof(real_T),
                    &emlrtRTEI);
  idx = tempcontext->size[1];
  for (ix = 0; ix < idx; ix++) {
    iy = tempcontext->size[0];
    for (nx = 0; nx < iy; nx++) {
      context->data[nx + context->size[0] * ix] = tempcontext->data[nx +
        tempcontext->size[0] * ix];
    }
  }

  if (do_attenuation_first == 0.0) {
    /*  2 Attenuate unoccupied cells */
    j = 0;
    while (j <= nrnocc) {
      /*  For unoccupied cells, attenuate */
      ix = noccidx->size[0];
      nx = j + 1;
      if (!((nx >= 1) && (nx <= ix))) {
        emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &v_emlrtBCI, sp);
      }

      ix = context->size[0];
      nx = (int32_T)noccidx->data[j];
      if (!((nx >= 1) && (nx <= ix))) {
        emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &w_emlrtBCI, sp);
      }

      idx = context->size[1];
      iy = (int32_T)noccidx->data[j];
      ix = updt_col->size[0] * updt_col->size[1];
      updt_col->size[0] = 1;
      updt_col->size[1] = idx;
      emxEnsureCapacity(sp, (emxArray__common *)updt_col, ix, (int32_T)sizeof
                        (real_T), &emlrtRTEI);
      for (ix = 0; ix < idx; ix++) {
        updt_col->data[updt_col->size[0] * ix] = context->data[(iy +
          context->size[0] * ix) - 1] * nocc_attenuaterate;
      }

      ix = context->size[0];
      nx = (int32_T)noccidx->data[j];
      if (!((nx >= 1) && (nx <= ix))) {
        emlrtDynamicBoundsCheckR2012b(nx, 1, ix, &x_emlrtBCI, sp);
      }

      idx = context->size[1];
      ix = ii->size[0];
      ii->size[0] = idx;
      emxEnsureCapacity(sp, (emxArray__common *)ii, ix, (int32_T)sizeof(int32_T),
                        &emlrtRTEI);
      for (ix = 0; ix < idx; ix++) {
        ii->data[ix] = ix;
      }

      iv5[0] = 1;
      iv5[1] = ii->size[0];
      emlrtSubAssignSizeCheckR2012b(iv5, 2, *(int32_T (*)[2])updt_col->size, 2,
        &d_emlrtECI, sp);
      iy = (int32_T)noccidx->data[j];
      idx = updt_col->size[1];
      for (ix = 0; ix < idx; ix++) {
        context->data[(iy + context->size[0] * ii->data[ix]) - 1] =
          updt_col->data[updt_col->size[0] * ix];
      }

      j++;
      if (*emlrtBreakCheckR2012bFlagVar != 0) {
        emlrtBreakCheckR2012b(sp);
      }
    }
  }

  emxFree_int32_T(&ii);
  emxFree_real_T(&updt_col);
  emxFree_real_T(&noccidx);

  /*  6 Prediction */
  st.site = &g_emlrtRSI;
  ix = tempcontext->size[0] * tempcontext->size[1];
  tempcontext->size[0] = context->size[1];
  tempcontext->size[1] = context->size[0];
  emxEnsureCapacity(&st, (emxArray__common *)tempcontext, ix, (int32_T)sizeof
                    (real_T), &emlrtRTEI);
  idx = context->size[0];
  for (ix = 0; ix < idx; ix++) {
    iy = context->size[1];
    for (nx = 0; nx < iy; nx++) {
      tempcontext->data[nx + tempcontext->size[0] * ix] = context->data[ix +
        context->size[0] * nx];
    }
  }

  b_st.site = &n_emlrtRSI;
  c_st.site = &o_emlrtRSI;
  if (((tempcontext->size[0] == 1) && (tempcontext->size[1] == 1)) ||
      (tempcontext->size[0] != 1)) {
    overflow = true;
  } else {
    overflow = false;
  }

  if (overflow) {
  } else {
    emlrtErrorWithMessageIdR2012b(&c_st, &u_emlrtRTEI,
      "Coder:toolbox:autoDimIncompatibility", 0);
  }

  if (tempcontext->size[0] > 0) {
  } else {
    emlrtErrorWithMessageIdR2012b(&c_st, &v_emlrtRTEI,
      "Coder:toolbox:eml_min_or_max_varDimZero", 0);
  }

  ix = curr_col->size[0] * curr_col->size[1];
  curr_col->size[0] = 1;
  curr_col->size[1] = tempcontext->size[1];
  emxEnsureCapacity(&c_st, (emxArray__common *)curr_col, ix, (int32_T)sizeof
                    (real_T), &emlrtRTEI);
  n = tempcontext->size[0];
  ix = 0;
  iy = -1;
  d_st.site = &ab_emlrtRSI;
  if (1 > tempcontext->size[1]) {
    overflow = false;
  } else {
    overflow = (tempcontext->size[1] > 2147483646);
  }

  if (overflow) {
    e_st.site = &l_emlrtRSI;
    check_forloop_overflow_error(&e_st);
  }

  for (nx = 1; nx <= tempcontext->size[1]; nx++) {
    d_st.site = &bb_emlrtRSI;
    ixstart = ix;
    idx = ix + n;
    mtmp = tempcontext->data[ix];
    if (n > 1) {
      if (muDoubleScalarIsNaN(tempcontext->data[ix])) {
        e_st.site = &r_emlrtRSI;
        if (ix + 2 > idx) {
          b_ix = false;
        } else {
          b_ix = (idx > 2147483646);
        }

        if (b_ix) {
          f_st.site = &l_emlrtRSI;
          check_forloop_overflow_error(&f_st);
        }

        k = ix + 1;
        exitg2 = false;
        while ((!exitg2) && (k + 1 <= idx)) {
          ixstart = k;
          if (!muDoubleScalarIsNaN(tempcontext->data[k])) {
            mtmp = tempcontext->data[k];
            exitg2 = true;
          } else {
            k++;
          }
        }
      }

      if (ixstart + 1 < idx) {
        e_st.site = &q_emlrtRSI;
        if (ixstart + 2 > idx) {
          b_ixstart = false;
        } else {
          b_ixstart = (idx > 2147483646);
        }

        if (b_ixstart) {
          f_st.site = &l_emlrtRSI;
          check_forloop_overflow_error(&f_st);
        }

        for (k = ixstart + 1; k + 1 <= idx; k++) {
          if (tempcontext->data[k] > mtmp) {
            mtmp = tempcontext->data[k];
          }
        }
      }
    }

    iy++;
    curr_col->data[iy] = mtmp;
    ix += n;
  }

  emxFree_real_T(&tempcontext);
  ix = maxvec->size[0];
  maxvec->size[0] = curr_col->size[1];
  emxEnsureCapacity(sp, (emxArray__common *)maxvec, ix, (int32_T)sizeof(real_T),
                    &emlrtRTEI);
  idx = curr_col->size[1];
  for (ix = 0; ix < idx; ix++) {
    maxvec->data[ix] = curr_col->data[curr_col->size[0] * ix];
  }

  emxFree_real_T(&curr_col);
  st.site = &h_emlrtRSI;

  /*  sigm_a  <= if we increase the value, than the sigm function gets peaky! */
  b_st.site = &cb_emlrtRSI;
  ix = predvec->size[0];
  predvec->size[0] = maxvec->size[0];
  emxEnsureCapacity(&b_st, (emxArray__common *)predvec, ix, (int32_T)sizeof
                    (real_T), &emlrtRTEI);
  idx = maxvec->size[0];
  for (ix = 0; ix < idx; ix++) {
    predvec->data[ix] = -sigm_coef * maxvec->data[ix];
  }

  c_st.site = &cb_emlrtRSI;
  b_exp(&c_st, predvec);
  ix = predvec->size[0];
  emxEnsureCapacity(&b_st, (emxArray__common *)predvec, ix, (int32_T)sizeof
                    (real_T), &emlrtRTEI);
  idx = predvec->size[0];
  for (ix = 0; ix < idx; ix++) {
    predvec->data[ix] = 1.0 - predvec->data[ix];
  }

  ix = newlyoccidx->size[0];
  newlyoccidx->size[0] = maxvec->size[0];
  emxEnsureCapacity(&b_st, (emxArray__common *)newlyoccidx, ix, (int32_T)sizeof
                    (real_T), &emlrtRTEI);
  idx = maxvec->size[0];
  for (ix = 0; ix < idx; ix++) {
    newlyoccidx->data[ix] = -sigm_coef * maxvec->data[ix];
  }

  c_st.site = &cb_emlrtRSI;
  b_exp(&c_st, newlyoccidx);
  ix = newlyoccidx->size[0];
  emxEnsureCapacity(&b_st, (emxArray__common *)newlyoccidx, ix, (int32_T)sizeof
                    (real_T), &emlrtRTEI);
  idx = newlyoccidx->size[0];
  for (ix = 0; ix < idx; ix++) {
    newlyoccidx->data[ix]++;
  }

  varargin_1[0] = predvec->size[0];
  varargin_1[1] = 1;
  varargin_2[0] = newlyoccidx->size[0];
  varargin_2[1] = 1;
  overflow = false;
  p = true;
  k = 0;
  exitg1 = false;
  while ((!exitg1) && (k < 2)) {
    if (!(varargin_1[k] == varargin_2[k])) {
      p = false;
      exitg1 = true;
    } else {
      k++;
    }
  }

  if (!p) {
  } else {
    overflow = true;
  }

  if (overflow) {
  } else {
    emlrtErrorWithMessageIdR2012b(&b_st, &w_emlrtRTEI, "MATLAB:dimagree", 0);
  }

  ix = predvec->size[0];
  emxEnsureCapacity(&b_st, (emxArray__common *)predvec, ix, (int32_T)sizeof
                    (real_T), &emlrtRTEI);
  idx = predvec->size[0];
  for (ix = 0; ix < idx; ix++) {
    predvec->data[ix] /= newlyoccidx->data[ix];
  }

  emxFree_real_T(&newlyoccidx);

  /*  7 Save previous grid */
  ix = cgridvecprev->size[0];
  cgridvecprev->size[0] = cgridvec->size[0];
  emxEnsureCapacity(sp, (emxArray__common *)cgridvecprev, ix, (int32_T)sizeof
                    (real_T), &emlrtRTEI);
  idx = cgridvec->size[0];
  for (ix = 0; ix < idx; ix++) {
    cgridvecprev->data[ix] = cgridvec->data[ix];
  }

  emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
Esempio n. 14
0
/* Function Definitions */
void updateCompass2(states_T *x, real32_T P[400], const real32_T y_Compass[3],
                    const real32_T b_N[3], const real32_T acc[3], boolean_T
                    constWind, boolean_T constQFF, boolean_T constK)
{
  real32_T r;
  real32_T b_r[3];
  real32_T R_Compass[9];
  real32_T C_NS[9];
  int32_T i13;
  real32_T y_Compass_N[3];
  int32_T i14;
  real32_T b_p_N[3];
  static const int8_T b[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 0 };

  real32_T y_p_N[3];
  real32_T X[9];
  real32_T b_X[9];
  real32_T y;
  int32_T ar;
  real32_T N[9];
  real32_T dtheta_half_norm;
  int32_T ib;
  int32_T i;
  real32_T c_X[9];
  static const int8_T a[3] = { 0, 0, 1 };

  real32_T b_a[3];
  real32_T E_z[3];
  real32_T d_X[9];
  real32_T e_X[9];
  real32_T K[20];
  real32_T e;
  static const int8_T iv4[3] = { 0, 0, 1 };

  real32_T H[20];
  real32_T b_y[3];
  boolean_T selector[20];
  emxArray_int32_T *r28;
  boolean_T b_selector[20];
  emxArray_int32_T *r29;
  emxArray_real32_T *c_a;
  emxArray_int32_T *r30;
  emxArray_int32_T *r31;
  int32_T br;
  emxArray_int32_T *r32;
  emxArray_int32_T *r33;
  emxArray_int32_T *r34;
  emxArray_int32_T *r35;
  emxArray_real32_T *d_a;
  emxArray_real32_T *b_b;
  emxArray_real32_T *c_y;
  uint32_T unnamed_idx_1;
  int32_T ic;
  int32_T ia;
  emxArray_real32_T *e_a;
  emxArray_real32_T *KH;
  emxArray_real32_T *r36;
  emxArray_real32_T *C;
  uint32_T unnamed_idx_0;
  int32_T c;
  emxArray_int32_T *r37;
  emxArray_int32_T *r38;
  real32_T dq[4];
  real32_T Q[16];

  /*  */
  /*  [x,P] = UPDATECOMPASS2(x,P,y_Compass, b_N, e_acc, constWind, constQFF,constK) */
  /*  */
  /*  Kalman filter update with 3-axis compass measurements. */
  /*  */
  /*  Author: lestefan */
  /*  date:  02/2013 */
  /*  */
  /*  Inputs: */
  /*    x:            states [p(lat*1e7,lon*1e7,h),q_NS,v_N,b_g,b_a,QFF,w,K] */
  /*    P:            covariance */
  /*    y_Compass:    magnetic field measurement 3x1 (in sensor frame S) [uT] */
  /*    b_N:          true magnetic flux density 3x1 (nav frame N) [uT] */
  /*    constWind:    keep wind constant [true/false] (use true on GPS out) */
  /*    constQFF:     keep QFF constant [true/false] (use true on GPS out) */
  /*    constK:       K (sideslip parameter) const. [true/false] (use true) */
  /*  */
  /*  Outputs: */
  /*    x:            updated states (see inputs) */
  /*    P:            updated covariance (see inputs) */
  /*  */
  /*  Needed globals: */
  /*    global configuration; */
  /*    configuration.tau:         accelerometer autocorrelation time [s] */
  /*    configuration.sigma_a_c:   accelerometer noise [m/s^2/sqrt(Hz)] */
  /*    configuration.sigma_a_d:   accelerometer standard diviation [m/s^2] */
  /*    configuration.sigma_aw_c:  accelerometer bias noise [m/s^3/sqrt(Hz)] */
  /*    configuration.sigma_g_c:   gyro noise [rad/s/sqrt(Hz)] */
  /*    configuration.sigma_gw_c:  gyro bias noise [rad/s^2/sqrt(Hz)] */
  /*    configuration.sigma_pw_c:  static pressure bias noise [kPa/s/sqrt(Hz)] */
  /*    configuration.sigma_w_c:   wind drift noise [m/s^2/sqrt(Hz)] */
  /*    configuration.sigma_psmd:  static pressure measurement variance [kPa] */
  /*    configuration.sigma_pdmd:  dynamic pressure measurement variance [kPa] */
  /*    configuration.sigma_Td=1:  temperature measurement variance [°K] */
  /*    configuration.sigma_md=1:  magnetometer noise [uT] */
  /*  */
  /*  See also UPDATEPOSITION, UPDATEVELNED, UPDATEPRESSURES,  */
  /*  UPDATEPRESSURES2, UPDATEPRESSURES3, PROPATATE, GETINITIALQ */
  /*  */
  r = rt_powf_snf(configuration.sigma_md, 2.0F);

  /* R_Compass=single(diag(configuration.sigma_md^2*single([1 1 1]))); */
  b_r[0] = r;
  b_r[1] = r;
  b_r[2] = r;
  b_diag(b_r, R_Compass);

  /* *configuration.sigma_md; */
  /*  TODO: update in the past, re-propagate. This might not be fast enough... */
  /*  set float type */
  C_NS[0] = ((x->q_NS[0] * x->q_NS[0] - x->q_NS[1] * x->q_NS[1]) - x->q_NS[2] *
             x->q_NS[2]) + x->q_NS[3] * x->q_NS[3];
  C_NS[3] = x->q_NS[0] * x->q_NS[1] * 2.0F + x->q_NS[2] * x->q_NS[3] * 2.0F;
  C_NS[6] = x->q_NS[0] * x->q_NS[2] * 2.0F - x->q_NS[1] * x->q_NS[3] * 2.0F;
  C_NS[1] = x->q_NS[0] * x->q_NS[1] * 2.0F - x->q_NS[2] * x->q_NS[3] * 2.0F;
  C_NS[4] = ((-x->q_NS[0] * x->q_NS[0] + x->q_NS[1] * x->q_NS[1]) - x->q_NS[2] *
             x->q_NS[2]) + x->q_NS[3] * x->q_NS[3];
  C_NS[7] = x->q_NS[0] * x->q_NS[3] * 2.0F + x->q_NS[1] * x->q_NS[2] * 2.0F;
  C_NS[2] = x->q_NS[0] * x->q_NS[2] * 2.0F + x->q_NS[1] * x->q_NS[3] * 2.0F;
  C_NS[5] = x->q_NS[0] * x->q_NS[3] * -2.0F + x->q_NS[1] * x->q_NS[2] * 2.0F;
  C_NS[8] = ((-x->q_NS[0] * x->q_NS[0] - x->q_NS[1] * x->q_NS[1]) + x->q_NS[2] *
             x->q_NS[2]) + x->q_NS[3] * x->q_NS[3];

  /*  error term: */
  for (i13 = 0; i13 < 3; i13++) {
    y_Compass_N[i13] = 0.0F;
    for (i14 = 0; i14 < 3; i14++) {
      y_Compass_N[i13] += C_NS[i13 + 3 * i14] * y_Compass[i14];
    }

    /* b_S=C_NS'*b_N; */
    /*  error and Jacobians: */
    /* [e,J]=autogen_angleJacobian2d(y_Compass_N,b_N); */
    /* dz=1e-3; */
    /* y_Compass_N_1p=C_NS*(y_Compass+[dz;0;0]); */
    /* y_Compass_N_2p=C_NS*(y_Compass+[0;dz;0]); */
    /* y_Compass_N_3p=C_NS*(y_Compass+[0;0;dz]); */
    /* y_Compass_N_1m=C_NS*(y_Compass-[dz;0;0]); */
    /* y_Compass_N_2m=C_NS*(y_Compass-[0;dz;0]); */
    /* y_Compass_N_3m=C_NS*(y_Compass-[0;0;dz]); */
    /* [e1,J]=autogen_angleJacobian2d(y_Compass_N_1p,b_N); */
    /* [e2,J]=autogen_angleJacobian2d(y_Compass_N_2p,b_N); */
    /* [e3,J]=autogen_angleJacobian2d(y_Compass_N_3p,b_N); */
    /* [e1,J]=autogen_angleJacobian2d(y_Compass_N_1m,b_N); */
    /* [e2,J]=autogen_angleJacobian2d(y_Compass_N_2m,b_N); */
    /* [e3,J]=autogen_angleJacobian2d(y_Compass_N_3m,b_N); */
    /* E_x=[single(zeros(1,3)),J*crossMx(y_Compass_N),single(zeros(1,14))]; */
    /* E_z=J*C_NS; */
    b_p_N[i13] = 0.0F;
    for (i14 = 0; i14 < 3; i14++) {
      b_p_N[i13] += (real32_T)b[i13 + 3 * i14] * b_N[i14];
    }
  }

  for (i13 = 0; i13 < 3; i13++) {
    y_p_N[i13] = 0.0F;
    for (i14 = 0; i14 < 3; i14++) {
      y_p_N[i13] += (real32_T)b[i13 + 3 * i14] * y_Compass_N[i14];
    }
  }

  /*  set float type */
  X[0] = 0.0F;
  X[3] = -y_p_N[2];
  X[6] = y_p_N[1];
  X[1] = y_p_N[2];
  X[4] = 0.0F;
  X[7] = -y_p_N[0];
  X[2] = -y_p_N[1];
  X[5] = y_p_N[0];
  X[8] = 0.0F;

  /*  set float type */
  b_X[0] = 0.0F;
  b_X[3] = -y_p_N[2];
  b_X[6] = y_p_N[1];
  b_X[1] = y_p_N[2];
  b_X[4] = 0.0F;
  b_X[7] = -y_p_N[0];
  b_X[2] = -y_p_N[1];
  b_X[5] = y_p_N[0];
  b_X[8] = 0.0F;
  y = 0.0F;
  for (ar = 0; ar < 3; ar++) {
    y += y_p_N[ar] * y_p_N[ar];
  }

  r = rt_powf_snf((real32_T)sqrt(y), 3.0F);
  for (i13 = 0; i13 < 3; i13++) {
    for (i14 = 0; i14 < 3; i14++) {
      dtheta_half_norm = 0.0F;
      for (ib = 0; ib < 3; ib++) {
        dtheta_half_norm += X[i13 + 3 * ib] * b_X[i14 + 3 * ib];
      }

      N[i13 + 3 * i14] = dtheta_half_norm / r;
    }
  }

  y = 0.0F;
  for (ar = 0; ar < 3; ar++) {
    y += b_p_N[ar] * b_p_N[ar];
  }

  r = (real32_T)sqrt(y);
  for (i = 0; i < 3; i++) {
    b_r[i] = -b_p_N[i] / r;
  }

  /*  set float type */
  c_X[0] = 0.0F;
  c_X[3] = -b_r[2];
  c_X[6] = b_r[1];
  c_X[1] = b_r[2];
  c_X[4] = 0.0F;
  c_X[7] = -b_r[0];
  c_X[2] = -b_r[1];
  c_X[5] = b_r[0];
  c_X[8] = 0.0F;
  for (i13 = 0; i13 < 3; i13++) {
    b_r[i13] = 0.0F;
    for (i14 = 0; i14 < 3; i14++) {
      b_r[i13] += (real32_T)a[i14] * c_X[i14 + 3 * i13];
    }
  }

  for (i13 = 0; i13 < 3; i13++) {
    b_a[i13] = 0.0F;
    for (i14 = 0; i14 < 3; i14++) {
      b_a[i13] += b_r[i14] * N[i14 + 3 * i13];
    }
  }

  for (i13 = 0; i13 < 3; i13++) {
    b_r[i13] = 0.0F;
    for (i14 = 0; i14 < 3; i14++) {
      b_r[i13] += b_a[i14] * (real32_T)b[i14 + 3 * i13];
    }
  }

  y = 0.0F;
  for (i13 = 0; i13 < 3; i13++) {
    E_z[i13] = 0.0F;
    for (i14 = 0; i14 < 3; i14++) {
      E_z[i13] += b_r[i14] * C_NS[i14 + 3 * i13];
    }

    y += b_p_N[i13] * b_p_N[i13];
  }

  r = (real32_T)sqrt(y);
  for (i = 0; i < 3; i++) {
    b_r[i] = -b_p_N[i] / r;
  }

  /*  set float type */
  d_X[0] = 0.0F;
  d_X[3] = -b_r[2];
  d_X[6] = b_r[1];
  d_X[1] = b_r[2];
  d_X[4] = 0.0F;
  d_X[7] = -b_r[0];
  d_X[2] = -b_r[1];
  d_X[5] = b_r[0];
  d_X[8] = 0.0F;

  /*  set float type */
  e_X[0] = 0.0F;
  e_X[3] = -y_Compass_N[2];
  e_X[6] = y_Compass_N[1];
  e_X[1] = y_Compass_N[2];
  e_X[4] = 0.0F;
  e_X[7] = -y_Compass_N[0];
  e_X[2] = -y_Compass_N[1];
  e_X[5] = y_Compass_N[0];
  e_X[8] = 0.0F;
  for (i13 = 0; i13 < 3; i13++) {
    b_r[i13] = 0.0F;
    for (i14 = 0; i14 < 3; i14++) {
      b_r[i13] += (real32_T)a[i14] * d_X[i14 + 3 * i13];
    }
  }

  for (i13 = 0; i13 < 3; i13++) {
    b_a[i13] = 0.0F;
    for (i14 = 0; i14 < 3; i14++) {
      b_a[i13] += b_r[i14] * N[i14 + 3 * i13];
    }
  }

  for (i13 = 0; i13 < 3; i13++) {
    b_r[i13] = 0.0F;
    for (i14 = 0; i14 < 3; i14++) {
      b_r[i13] += b_a[i14] * (real32_T)b[i14 + 3 * i13];
    }
  }

  for (i13 = 0; i13 < 3; i13++) {
    b_a[i13] = 0.0F;
    for (i14 = 0; i14 < 3; i14++) {
      b_a[i13] += b_r[i14] * e_X[i14 + 3 * i13];
    }

    K[i13] = 0.0F;
  }

  for (i13 = 0; i13 < 3; i13++) {
    K[i13 + 3] = b_a[i13];
  }

  for (i13 = 0; i13 < 14; i13++) {
    K[i13 + 6] = 0.0F;
  }

  y = 0.0F;
  for (ar = 0; ar < 3; ar++) {
    y += y_p_N[ar] * y_p_N[ar];
  }

  r = (real32_T)sqrt(y);
  y = 0.0F;
  for (ar = 0; ar < 3; ar++) {
    y += b_p_N[ar] * b_p_N[ar];
    b_a[ar] = y_p_N[ar] / r;
  }

  r = (real32_T)sqrt(y);
  for (i = 0; i < 3; i++) {
    b_r[i] = b_p_N[i] / r;
  }

  cross(b_a, b_r, y_Compass_N);
  e = 0.0F;
  for (ar = 0; ar < 3; ar++) {
    e += (real32_T)iv4[ar] * y_Compass_N[ar];
  }

  /* e1p=Pi_z*cross(Pi_xy0*y_Compass_N_1p/sqrt(y_Compass_N_1p'*y_Compass_N_1p),Pi_xy0*e_N); */
  /* e2p=Pi_z*cross(Pi_xy0*y_Compass_N_2p/sqrt(y_Compass_N_2p'*y_Compass_N_2p),Pi_xy0*e_N); */
  /* e3p=Pi_z*cross(Pi_xy0*y_Compass_N_3p/sqrt(y_Compass_N_3p'*y_Compass_N_3p),Pi_xy0*e_N); */
  /* e1m=Pi_z*cross(Pi_xy0*y_Compass_N_1m/sqrt(y_Compass_N_1m'*y_Compass_N_1m),Pi_xy0*e_N); */
  /* e2m=Pi_z*cross(Pi_xy0*y_Compass_N_2m/sqrt(y_Compass_N_2m'*y_Compass_N_2m),Pi_xy0*e_N); */
  /* e3m=Pi_z*cross(Pi_xy0*y_Compass_N_3m/sqrt(y_Compass_N_3m'*y_Compass_N_3m),Pi_xy0*e_N); */
  /* num_diff_Ez = [(e1p-e1m)/(2*dz) (e2p-e2m)/(2*dz) (e3p-e3m)/(2*dz)]; */
  /*  e_acc=single([0;0;1]); % rather random, but if mounted as advised, it's ok */
  /*  norm_acc_sq=acc'*acc; */
  /*  if norm_acc_sq>1 */
  /*      e_acc=acc/sqrt(norm_acc_sq); */
  /*  end */
  /*  [e,E_b_S,E_z]=autogen_angleJacobian2dAccDir(y_Compass,b_S,e_acc); */
  /*  E_x=[single(zeros(1,3)),-E_b_S*C_NS'*crossMx(b_N),single(zeros(1,14))]; */
  for (i13 = 0; i13 < 20; i13++) {
    H[i13] = -K[i13];
  }

  for (i13 = 0; i13 < 3; i13++) {
    b_y[i13] = 0.0F;
    for (i14 = 0; i14 < 3; i14++) {
      y = b_y[i13] + K[3 + i14] * P[(i14 + 20 * (3 + i13)) + 3];
      b_y[i13] = y;
    }
  }

  y = 0.0F;
  r = 0.0F;
  for (ar = 0; ar < 3; ar++) {
    y += b_y[ar] * K[3 + ar];
    b_y[ar] = 0.0F;
    for (i13 = 0; i13 < 3; i13++) {
      b_y[ar] += E_z[i13] * R_Compass[i13 + 3 * ar];
    }

    r += b_y[ar] * E_z[ar];
  }

  r += y;
  if (e * rt_powf_snf(r, -1.0F) * e > 100.0F) {
    /* disp(['magnetometer chi2 ' num2str(chi2)]); */
  } else {
    /* K=(S\(H(1:3,4:6)*P(4:6,:)))'; % gain */
    for (i = 0; i < 20; i++) {
      K[i] = 0.0F;

      /*  set float type */
      selector[i] = TRUE;
    }

    if (constK) {
      selector[19] = FALSE;
    }

    if (constQFF) {
      selector[15] = FALSE;
    }

    emxInit_int32_T(&r28, 1);

    /*  remove correlations of fixed states with active states */
    eml_li_find(selector, r28);
    i13 = r28->size[0];
    r28->size[0] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)r28, i13, (int32_T)sizeof(int32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r28->data[i13]--;
    }

    for (i = 0; i < 20; i++) {
      b_selector[i] = !selector[i];
    }

    emxInit_int32_T(&r29, 1);
    eml_li_find(b_selector, r29);
    i13 = r29->size[0];
    r29->size[0] = r29->size[0];
    emxEnsureCapacity((emxArray__common *)r29, i13, (int32_T)sizeof(int32_T));
    i = r29->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r29->data[i13]--;
    }

    for (i = 0; i < 20; i++) {
      b_selector[i] = !selector[i];
    }

    emxInit_real32_T(&c_a, 2);
    emxInit_int32_T(&r30, 1);
    emxInit_int32_T(&r31, 1);
    eml_li_find(b_selector, r30);
    eml_li_find(selector, r31);
    i13 = c_a->size[0] * c_a->size[1];
    c_a->size[0] = r31->size[0];
    c_a->size[1] = r30->size[0];
    emxEnsureCapacity((emxArray__common *)c_a, i13, (int32_T)sizeof(real32_T));
    i = r30->size[0];
    for (i13 = 0; i13 < i; i13++) {
      br = r31->size[0];
      for (i14 = 0; i14 < br; i14++) {
        c_a->data[i14 + c_a->size[0] * i13] = P[(r31->data[i14] + 20 *
          (r30->data[i13] - 1)) - 1];
      }
    }

    emxInit_int32_T(&r32, 1);
    i13 = r32->size[0];
    r32->size[0] = r29->size[0];
    emxEnsureCapacity((emxArray__common *)r32, i13, (int32_T)sizeof(int32_T));
    i = r29->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r32->data[i13] = r29->data[i13];
    }

    emxInit_int32_T(&r33, 1);
    i13 = r33->size[0];
    r33->size[0] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)r33, i13, (int32_T)sizeof(int32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r33->data[i13] = r28->data[i13];
    }

    i = c_a->size[1];
    for (i13 = 0; i13 < i; i13++) {
      br = c_a->size[0];
      for (i14 = 0; i14 < br; i14++) {
        P[r33->data[i14] + 20 * r32->data[i13]] = c_a->data[i14 + c_a->size[0] *
          i13] * 0.0F;
      }
    }

    emxFree_int32_T(&r33);
    emxFree_int32_T(&r32);
    for (i = 0; i < 20; i++) {
      b_selector[i] = !selector[i];
    }

    eml_li_find(b_selector, r28);
    i13 = r28->size[0];
    r28->size[0] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)r28, i13, (int32_T)sizeof(int32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r28->data[i13]--;
    }

    eml_li_find(selector, r29);
    i13 = r29->size[0];
    r29->size[0] = r29->size[0];
    emxEnsureCapacity((emxArray__common *)r29, i13, (int32_T)sizeof(int32_T));
    i = r29->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r29->data[i13]--;
    }

    eml_li_find(selector, r30);
    for (i = 0; i < 20; i++) {
      b_selector[i] = !selector[i];
    }

    eml_li_find(b_selector, r31);
    i13 = c_a->size[0] * c_a->size[1];
    c_a->size[0] = r31->size[0];
    c_a->size[1] = r30->size[0];
    emxEnsureCapacity((emxArray__common *)c_a, i13, (int32_T)sizeof(real32_T));
    i = r30->size[0];
    for (i13 = 0; i13 < i; i13++) {
      br = r31->size[0];
      for (i14 = 0; i14 < br; i14++) {
        c_a->data[i14 + c_a->size[0] * i13] = P[(r31->data[i14] + 20 *
          (r30->data[i13] - 1)) - 1];
      }
    }

    emxFree_int32_T(&r31);
    emxInit_int32_T(&r34, 1);
    i13 = r34->size[0];
    r34->size[0] = r29->size[0];
    emxEnsureCapacity((emxArray__common *)r34, i13, (int32_T)sizeof(int32_T));
    i = r29->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r34->data[i13] = r29->data[i13];
    }

    emxInit_int32_T(&r35, 1);
    i13 = r35->size[0];
    r35->size[0] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)r35, i13, (int32_T)sizeof(int32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r35->data[i13] = r28->data[i13];
    }

    i = c_a->size[1];
    for (i13 = 0; i13 < i; i13++) {
      br = c_a->size[0];
      for (i14 = 0; i14 < br; i14++) {
        P[r35->data[i14] + 20 * r34->data[i13]] = c_a->data[i14 + c_a->size[0] *
          i13] * 0.0F;
      }
    }

    emxFree_int32_T(&r35);
    emxFree_int32_T(&r34);

    /* P(~selector,~selector)=P(~selector,~selector)*0; */
    eml_li_find(selector, r28);
    i13 = r28->size[0];
    r28->size[0] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)r28, i13, (int32_T)sizeof(int32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r28->data[i13]--;
    }

    emxInit_real32_T(&d_a, 2);
    eml_li_find(selector, r29);
    i13 = d_a->size[0] * d_a->size[1];
    d_a->size[0] = 1;
    d_a->size[1] = r29->size[0];
    emxEnsureCapacity((emxArray__common *)d_a, i13, (int32_T)sizeof(real32_T));
    i = r29->size[0];
    for (i13 = 0; i13 < i; i13++) {
      d_a->data[d_a->size[0] * i13] = H[r29->data[i13] - 1];
    }

    emxInit_real32_T(&b_b, 2);
    eml_li_find(selector, r29);
    eml_li_find(selector, r30);
    i13 = b_b->size[0] * b_b->size[1];
    b_b->size[0] = r30->size[0];
    b_b->size[1] = r29->size[0];
    emxEnsureCapacity((emxArray__common *)b_b, i13, (int32_T)sizeof(real32_T));
    i = r29->size[0];
    for (i13 = 0; i13 < i; i13++) {
      br = r30->size[0];
      for (i14 = 0; i14 < br; i14++) {
        b_b->data[i14 + b_b->size[0] * i13] = P[(r30->data[i14] + 20 *
          (r29->data[i13] - 1)) - 1];
      }
    }

    emxFree_int32_T(&r30);
    emxInit_real32_T(&c_y, 2);
    if ((d_a->size[1] == 1) || (b_b->size[0] == 1)) {
      i13 = c_y->size[0] * c_y->size[1];
      c_y->size[0] = 1;
      c_y->size[1] = b_b->size[1];
      emxEnsureCapacity((emxArray__common *)c_y, i13, (int32_T)sizeof(real32_T));
      i = b_b->size[1];
      for (i13 = 0; i13 < i; i13++) {
        c_y->data[c_y->size[0] * i13] = 0.0F;
        br = d_a->size[1];
        for (i14 = 0; i14 < br; i14++) {
          c_y->data[c_y->size[0] * i13] += d_a->data[d_a->size[0] * i14] *
            b_b->data[i14 + b_b->size[0] * i13];
        }
      }
    } else {
      unnamed_idx_1 = (uint32_T)b_b->size[1];
      i13 = c_y->size[0] * c_y->size[1];
      c_y->size[0] = 1;
      emxEnsureCapacity((emxArray__common *)c_y, i13, (int32_T)sizeof(real32_T));
      i13 = c_y->size[0] * c_y->size[1];
      c_y->size[1] = (int32_T)unnamed_idx_1;
      emxEnsureCapacity((emxArray__common *)c_y, i13, (int32_T)sizeof(real32_T));
      i = (int32_T)unnamed_idx_1;
      for (i13 = 0; i13 < i; i13++) {
        c_y->data[i13] = 0.0F;
      }

      if (b_b->size[1] == 0) {
      } else {
        for (i = 0; i < b_b->size[1]; i++) {
          for (ic = i; ic + 1 <= i + 1; ic++) {
            c_y->data[ic] = 0.0F;
          }
        }

        br = 0;
        for (i = 0; i < b_b->size[1]; i++) {
          ar = 0;
          i13 = br + d_a->size[1];
          for (ib = br; ib + 1 <= i13; ib++) {
            if (b_b->data[ib] != 0.0F) {
              ia = ar;
              for (ic = i; ic + 1 <= i + 1; ic++) {
                ia++;
                c_y->data[ic] += b_b->data[ib] * d_a->data[ia - 1];
              }
            }

            ar++;
          }

          br += d_a->size[1];
        }
      }
    }

    i = c_y->size[1];
    for (i13 = 0; i13 < i; i13++) {
      K[r28->data[i13]] = c_y->data[i13] / r;
    }

    emxFree_real32_T(&c_y);
    b_emxInit_real32_T(&e_a, 1);

    /*  check Jacobian */
    /*  delta=1e-12; */
    /*  db=[-C_SN*crossMx(b_N)*[delta;0;0],-C_SN*crossMx(b_N)*[0;delta;0],-C_SN*crossMx(b_N)*[0;0;delta]]; */
    /*  q1=quatUpdate(x(4:7),[delta;0;0]); */
    /*  q2=quatUpdate(x(4:7),[0;delta;0]); */
    /*  q3=quatUpdate(x(4:7),[0;0;delta]); */
    /*  db_finiteDiff=[quat2r(q1)'*b_N-C_SN*b_N, quat2r(q2)'*b_N-C_SN*b_N, quat2r(q3)'*b_N-C_SN*b_N]; */
    /*  db-db_finiteDiff */
    /*  update: */
    eml_li_find(selector, r28);
    i13 = e_a->size[0];
    e_a->size[0] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)e_a, i13, (int32_T)sizeof(real32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      e_a->data[i13] = K[r28->data[i13] - 1];
    }

    eml_li_find(selector, r28);
    i13 = d_a->size[0] * d_a->size[1];
    d_a->size[0] = 1;
    d_a->size[1] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)d_a, i13, (int32_T)sizeof(real32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      d_a->data[d_a->size[0] * i13] = H[r28->data[i13] - 1];
    }

    emxInit_real32_T(&KH, 2);
    i13 = KH->size[0] * KH->size[1];
    KH->size[0] = e_a->size[0];
    KH->size[1] = d_a->size[1];
    emxEnsureCapacity((emxArray__common *)KH, i13, (int32_T)sizeof(real32_T));
    i = e_a->size[0];
    for (i13 = 0; i13 < i; i13++) {
      br = d_a->size[1];
      for (i14 = 0; i14 < br; i14++) {
        KH->data[i13 + KH->size[0] * i14] = e_a->data[i13] * d_a->data[d_a->
          size[0] * i14];
      }
    }

    emxInit_real32_T(&r36, 2);
    eml_li_find(selector, r28);
    eml_li_find(selector, r29);
    i13 = r36->size[0] * r36->size[1];
    r36->size[0] = r29->size[0];
    r36->size[1] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)r36, i13, (int32_T)sizeof(real32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      br = r29->size[0];
      for (i14 = 0; i14 < br; i14++) {
        r36->data[i14 + r36->size[0] * i13] = P[(r29->data[i14] + 20 *
          (r28->data[i13] - 1)) - 1];
      }
    }

    eml_li_find(selector, r28);
    eml_li_find(selector, r29);
    i13 = b_b->size[0] * b_b->size[1];
    b_b->size[0] = r29->size[0];
    b_b->size[1] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)b_b, i13, (int32_T)sizeof(real32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      br = r29->size[0];
      for (i14 = 0; i14 < br; i14++) {
        b_b->data[i14 + b_b->size[0] * i13] = P[(r29->data[i14] + 20 *
          (r28->data[i13] - 1)) - 1];
      }
    }

    emxInit_real32_T(&C, 2);
    if ((KH->size[1] == 1) || (b_b->size[0] == 1)) {
      i13 = C->size[0] * C->size[1];
      C->size[0] = KH->size[0];
      C->size[1] = b_b->size[1];
      emxEnsureCapacity((emxArray__common *)C, i13, (int32_T)sizeof(real32_T));
      i = KH->size[0];
      for (i13 = 0; i13 < i; i13++) {
        br = b_b->size[1];
        for (i14 = 0; i14 < br; i14++) {
          C->data[i13 + C->size[0] * i14] = 0.0F;
          ar = KH->size[1];
          for (ib = 0; ib < ar; ib++) {
            C->data[i13 + C->size[0] * i14] += KH->data[i13 + KH->size[0] * ib] *
              b_b->data[ib + b_b->size[0] * i14];
          }
        }
      }
    } else {
      unnamed_idx_0 = (uint32_T)KH->size[0];
      unnamed_idx_1 = (uint32_T)b_b->size[1];
      i13 = C->size[0] * C->size[1];
      C->size[0] = (int32_T)unnamed_idx_0;
      emxEnsureCapacity((emxArray__common *)C, i13, (int32_T)sizeof(real32_T));
      i13 = C->size[0] * C->size[1];
      C->size[1] = (int32_T)unnamed_idx_1;
      emxEnsureCapacity((emxArray__common *)C, i13, (int32_T)sizeof(real32_T));
      i = (int32_T)unnamed_idx_0 * (int32_T)unnamed_idx_1;
      for (i13 = 0; i13 < i; i13++) {
        C->data[i13] = 0.0F;
      }

      if ((KH->size[0] == 0) || (b_b->size[1] == 0)) {
      } else {
        c = KH->size[0] * (b_b->size[1] - 1);
        for (i = 0; i <= c; i += KH->size[0]) {
          i13 = i + KH->size[0];
          for (ic = i; ic + 1 <= i13; ic++) {
            C->data[ic] = 0.0F;
          }
        }

        br = 0;
        for (i = 0; i <= c; i += KH->size[0]) {
          ar = 0;
          i13 = br + KH->size[1];
          for (ib = br; ib + 1 <= i13; ib++) {
            if (b_b->data[ib] != 0.0F) {
              ia = ar;
              i14 = i + KH->size[0];
              for (ic = i; ic + 1 <= i14; ic++) {
                ia++;
                C->data[ic] += b_b->data[ib] * KH->data[ia - 1];
              }
            }

            ar += KH->size[0];
          }

          br += KH->size[1];
        }
      }
    }

    eml_li_find(selector, r28);
    eml_li_find(selector, r29);
    i13 = c_a->size[0] * c_a->size[1];
    c_a->size[0] = r29->size[0];
    c_a->size[1] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)c_a, i13, (int32_T)sizeof(real32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      br = r29->size[0];
      for (i14 = 0; i14 < br; i14++) {
        c_a->data[i14 + c_a->size[0] * i13] = P[(r29->data[i14] + 20 *
          (r28->data[i13] - 1)) - 1];
      }
    }

    i13 = b_b->size[0] * b_b->size[1];
    b_b->size[0] = KH->size[1];
    b_b->size[1] = KH->size[0];
    emxEnsureCapacity((emxArray__common *)b_b, i13, (int32_T)sizeof(real32_T));
    i = KH->size[0];
    for (i13 = 0; i13 < i; i13++) {
      br = KH->size[1];
      for (i14 = 0; i14 < br; i14++) {
        b_b->data[i14 + b_b->size[0] * i13] = KH->data[i13 + KH->size[0] * i14];
      }
    }

    if ((c_a->size[1] == 1) || (b_b->size[0] == 1)) {
      i13 = KH->size[0] * KH->size[1];
      KH->size[0] = c_a->size[0];
      KH->size[1] = b_b->size[1];
      emxEnsureCapacity((emxArray__common *)KH, i13, (int32_T)sizeof(real32_T));
      i = c_a->size[0];
      for (i13 = 0; i13 < i; i13++) {
        br = b_b->size[1];
        for (i14 = 0; i14 < br; i14++) {
          KH->data[i13 + KH->size[0] * i14] = 0.0F;
          ar = c_a->size[1];
          for (ib = 0; ib < ar; ib++) {
            KH->data[i13 + KH->size[0] * i14] += c_a->data[i13 + c_a->size[0] *
              ib] * b_b->data[ib + b_b->size[0] * i14];
          }
        }
      }
    } else {
      unnamed_idx_0 = (uint32_T)c_a->size[0];
      unnamed_idx_1 = (uint32_T)b_b->size[1];
      i13 = KH->size[0] * KH->size[1];
      KH->size[0] = (int32_T)unnamed_idx_0;
      emxEnsureCapacity((emxArray__common *)KH, i13, (int32_T)sizeof(real32_T));
      i13 = KH->size[0] * KH->size[1];
      KH->size[1] = (int32_T)unnamed_idx_1;
      emxEnsureCapacity((emxArray__common *)KH, i13, (int32_T)sizeof(real32_T));
      i = (int32_T)unnamed_idx_0 * (int32_T)unnamed_idx_1;
      for (i13 = 0; i13 < i; i13++) {
        KH->data[i13] = 0.0F;
      }

      if ((c_a->size[0] == 0) || (b_b->size[1] == 0)) {
      } else {
        c = c_a->size[0] * (b_b->size[1] - 1);
        for (i = 0; i <= c; i += c_a->size[0]) {
          i13 = i + c_a->size[0];
          for (ic = i; ic + 1 <= i13; ic++) {
            KH->data[ic] = 0.0F;
          }
        }

        br = 0;
        for (i = 0; i <= c; i += c_a->size[0]) {
          ar = 0;
          i13 = br + c_a->size[1];
          for (ib = br; ib + 1 <= i13; ib++) {
            if (b_b->data[ib] != 0.0F) {
              ia = ar;
              i14 = i + c_a->size[0];
              for (ic = i; ic + 1 <= i14; ic++) {
                ia++;
                KH->data[ic] += b_b->data[ib] * c_a->data[ia - 1];
              }
            }

            ar += c_a->size[0];
          }

          br += c_a->size[1];
        }
      }
    }

    emxFree_real32_T(&b_b);
    emxFree_real32_T(&c_a);
    eml_li_find(selector, r28);
    i13 = e_a->size[0];
    e_a->size[0] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)e_a, i13, (int32_T)sizeof(real32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      e_a->data[i13] = K[r28->data[i13] - 1];
    }

    eml_li_find(selector, r28);
    i13 = d_a->size[0] * d_a->size[1];
    d_a->size[0] = 1;
    d_a->size[1] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)d_a, i13, (int32_T)sizeof(real32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      d_a->data[d_a->size[0] * i13] = K[r28->data[i13] - 1];
    }

    eml_li_find(selector, r28);
    i13 = r28->size[0];
    r28->size[0] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)r28, i13, (int32_T)sizeof(int32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r28->data[i13]--;
    }

    eml_li_find(selector, r29);
    i13 = r29->size[0];
    r29->size[0] = r29->size[0];
    emxEnsureCapacity((emxArray__common *)r29, i13, (int32_T)sizeof(int32_T));
    i = r29->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r29->data[i13]--;
    }

    emxInit_int32_T(&r37, 1);
    i13 = r37->size[0];
    r37->size[0] = r29->size[0];
    emxEnsureCapacity((emxArray__common *)r37, i13, (int32_T)sizeof(int32_T));
    i = r29->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r37->data[i13] = r29->data[i13];
    }

    emxFree_int32_T(&r29);
    emxInit_int32_T(&r38, 1);
    i13 = r38->size[0];
    r38->size[0] = r28->size[0];
    emxEnsureCapacity((emxArray__common *)r38, i13, (int32_T)sizeof(int32_T));
    i = r28->size[0];
    for (i13 = 0; i13 < i; i13++) {
      r38->data[i13] = r28->data[i13];
    }

    emxFree_int32_T(&r28);
    i = e_a->size[0];
    for (i13 = 0; i13 < i; i13++) {
      br = d_a->size[1];
      for (i14 = 0; i14 < br; i14++) {
        dtheta_half_norm = e_a->data[i13] * r * d_a->data[d_a->size[0] * i14];
        P[r38->data[i13] + 20 * r37->data[i14]] = ((r36->data[i13 + r36->size[0]
          * i14] - C->data[i13 + C->size[0] * i14]) - KH->data[i13 + KH->size[0]
          * i14]) + dtheta_half_norm;
      }
    }

    emxFree_int32_T(&r38);
    emxFree_int32_T(&r37);
    emxFree_real32_T(&e_a);
    emxFree_real32_T(&d_a);
    emxFree_real32_T(&C);
    emxFree_real32_T(&r36);
    emxFree_real32_T(&KH);
    for (i13 = 0; i13 < 20; i13++) {
      K[i13] *= e;
    }

    for (i13 = 0; i13 < 3; i13++) {
      x->p[i13] += (real_T)K[i13];
    }

    for (i = 0; i < 3; i++) {
      y_Compass_N[i] = 0.5F * K[i + 3];
    }

    dtheta_half_norm = (real32_T)sqrt(dot(y_Compass_N, y_Compass_N));
    if ((real32_T)fabs(dtheta_half_norm) < 0.01F) {
      r = dtheta_half_norm * dtheta_half_norm;
      y = ((1.0F - 0.166666672F * r) + 0.00833333377F * (r * r)) -
        0.000198412701F * (r * r * r);
    } else {
      y = (real32_T)sin(dtheta_half_norm) / dtheta_half_norm;
    }

    for (i = 0; i < 3; i++) {
      dq[i] = y * y_Compass_N[i];
    }

    dq[3] = (real32_T)cos(dtheta_half_norm);

    /*  optimization (rectification of sinc) */
    /*  [ q3, q2, -q1, q0] */
    /*  [ -q2, q3, q0, q1] */
    /*  [ q1, -q0, q3, q2] */
    /*  [ -q0, -q1, -q2, q3] */
    /*  set float type */
    Q[0] = dq[3];
    Q[4] = dq[2];
    Q[8] = -dq[1];
    Q[12] = dq[0];
    Q[1] = -dq[2];
    Q[5] = dq[3];
    Q[9] = dq[0];
    Q[13] = dq[1];
    Q[2] = dq[1];
    Q[6] = -dq[0];
    Q[10] = dq[3];
    Q[14] = dq[2];
    Q[3] = -dq[0];
    Q[7] = -dq[1];
    Q[11] = -dq[2];
    Q[15] = dq[3];
    for (i13 = 0; i13 < 4; i13++) {
      dq[i13] = 0.0F;
      for (i14 = 0; i14 < 4; i14++) {
        r = dq[i13] + Q[i13 + (i14 << 2)] * x->q_NS[i14];
        dq[i13] = r;
      }
    }

    r = 0.0F;
    i = 0;
    br = 0;
    for (ar = 0; ar < 4; ar++) {
      r += dq[i] * dq[br];
      i++;
      br++;
    }

    r = (real32_T)sqrt(r);
    for (i13 = 0; i13 < 4; i13++) {
      dq[i13] /= r;
    }

    for (i = 0; i < 4; i++) {
      x->q_NS[i] = dq[i];
    }

    /*  Correct the angular state only with tilt D (tilt N and tilt E are Zero) */
    for (i13 = 0; i13 < 3; i13++) {
      x->v_N[i13] += K[6 + i13];
    }

    for (i13 = 0; i13 < 3; i13++) {
      x->b_g[i13] += K[9 + i13];
    }

    for (i13 = 0; i13 < 3; i13++) {
      x->b_a[i13] += K[12 + i13];
    }

    x->QFF += K[15];
    for (i13 = 0; i13 < 3; i13++) {
      x->w[i13] += K[16 + i13];
    }

    x->K += K[19];
  }
}
Esempio n. 15
0
/*
 * Arguments    : double m
 *                double f
 *                const emxArray_real_T *w
 *                emxArray_real_T *b
 * Return Type  : void
 */
void fkernel(double m, double f, const emxArray_real_T *w, emxArray_real_T *b)
{
  int k;
  int nm1d2;
  double anew;
  double y;
  int n;
  double apnd;
  double ndbl;
  double cdiff;
  double absa;
  double absb;
  emxArray_real_T *b_m;
  emxArray_int32_T *r0;
  emxArray_real_T *b_y;
  emxArray_real_T *x;
  k = b->size[0] * b->size[1];
  b->size[0] = 1;
  b->size[1] = w->size[1];
  emxEnsureCapacity((emxArray__common *)b, k, (int)sizeof(double));
  nm1d2 = w->size[1];
  for (k = 0; k < nm1d2; k++) {
    b->data[k] = 0.0;
  }

  anew = -m / 2.0;
  y = m / 2.0;
  if (rtIsNaN(anew) || rtIsNaN(y)) {
    n = 1;
    anew = rtNaN;
    apnd = y;
  } else if (y < anew) {
    n = 0;
    apnd = y;
  } else if (rtIsInf(anew) || rtIsInf(y)) {
    n = 1;
    anew = rtNaN;
    apnd = y;
  } else {
    ndbl = floor((y - anew) + 0.5);
    apnd = anew + ndbl;
    cdiff = apnd - y;
    absa = fabs(anew);
    absb = fabs(y);
    if ((absa >= absb) || rtIsNaN(absb)) {
      absb = absa;
    }

    if (fabs(cdiff) < 4.4408920985006262E-16 * absb) {
      ndbl++;
      apnd = y;
    } else if (cdiff > 0.0) {
      apnd = anew + (ndbl - 1.0);
    } else {
      ndbl++;
    }

    if (ndbl >= 0.0) {
      n = (int)ndbl;
    } else {
      n = 0;
    }
  }

  emxInit_real_T(&b_m, 2);
  k = b_m->size[0] * b_m->size[1];
  b_m->size[0] = 1;
  b_m->size[1] = n;
  emxEnsureCapacity((emxArray__common *)b_m, k, (int)sizeof(double));
  if (n > 0) {
    b_m->data[0] = anew;
    if (n > 1) {
      b_m->data[n - 1] = apnd;
      k = n - 1;
      nm1d2 = k / 2;
      for (k = 1; k < nm1d2; k++) {
        b_m->data[k] = anew + (double)k;
        b_m->data[(n - k) - 1] = apnd - (double)k;
      }

      if (nm1d2 << 1 == n - 1) {
        b_m->data[nm1d2] = (anew + apnd) / 2.0;
      } else {
        b_m->data[nm1d2] = anew + (double)nm1d2;
        b_m->data[nm1d2 + 1] = apnd - (double)nm1d2;
      }
    }
  }

  emxInit_int32_T(&r0, 2);
  n = b_m->size[1] - 1;
  nm1d2 = 0;
  for (k = 0; k <= n; k++) {
    if (b_m->data[k] == 0.0) {
      nm1d2++;
    }
  }

  k = r0->size[0] * r0->size[1];
  r0->size[0] = 1;
  r0->size[1] = nm1d2;
  emxEnsureCapacity((emxArray__common *)r0, k, (int)sizeof(int));
  nm1d2 = 0;
  for (k = 0; k <= n; k++) {
    if (b_m->data[k] == 0.0) {
      r0->data[nm1d2] = k + 1;
      nm1d2++;
    }
  }

  nm1d2 = r0->size[0] * r0->size[1];
  for (k = 0; k < nm1d2; k++) {
    b->data[r0->data[k] - 1] = 6.2831853071795862 * f;
  }

  /*  No division by zero */
  n = b_m->size[1] - 1;
  nm1d2 = 0;
  for (k = 0; k <= n; k++) {
    if (b_m->data[k] != 0.0) {
      nm1d2++;
    }
  }

  k = r0->size[0] * r0->size[1];
  r0->size[0] = 1;
  r0->size[1] = nm1d2;
  emxEnsureCapacity((emxArray__common *)r0, k, (int)sizeof(int));
  nm1d2 = 0;
  for (k = 0; k <= n; k++) {
    if (b_m->data[k] != 0.0) {
      r0->data[nm1d2] = k + 1;
      nm1d2++;
    }
  }

  emxInit_real_T(&b_y, 2);
  y = 6.2831853071795862 * f;
  k = b_y->size[0] * b_y->size[1];
  b_y->size[0] = 1;
  b_y->size[1] = r0->size[1];
  emxEnsureCapacity((emxArray__common *)b_y, k, (int)sizeof(double));
  nm1d2 = r0->size[0] * r0->size[1];
  for (k = 0; k < nm1d2; k++) {
    b_y->data[k] = y * b_m->data[r0->data[k] - 1];
  }

  emxInit_real_T(&x, 2);
  k = x->size[0] * x->size[1];
  x->size[0] = 1;
  x->size[1] = b_y->size[1];
  emxEnsureCapacity((emxArray__common *)x, k, (int)sizeof(double));
  nm1d2 = b_y->size[0] * b_y->size[1];
  for (k = 0; k < nm1d2; k++) {
    x->data[k] = b_y->data[k];
  }

  for (k = 0; k + 1 <= b_y->size[1]; k++) {
    x->data[k] = sin(x->data[k]);
  }

  k = b_y->size[0] * b_y->size[1];
  b_y->size[0] = 1;
  b_y->size[1] = x->size[1];
  emxEnsureCapacity((emxArray__common *)b_y, k, (int)sizeof(double));
  nm1d2 = x->size[0] * x->size[1];
  for (k = 0; k < nm1d2; k++) {
    b_y->data[k] = x->data[k] / b_m->data[r0->data[k] - 1];
  }

  emxFree_real_T(&x);
  emxFree_int32_T(&r0);
  n = b_m->size[1];
  nm1d2 = 0;
  for (k = 0; k < n; k++) {
    if (b_m->data[k] != 0.0) {
      b->data[k] = b_y->data[nm1d2];
      nm1d2++;
    }
  }

  emxFree_real_T(&b_y);
  emxFree_real_T(&b_m);

  /*  Sinc */
  k = b->size[0] * b->size[1];
  b->size[0] = 1;
  emxEnsureCapacity((emxArray__common *)b, k, (int)sizeof(double));
  nm1d2 = b->size[0];
  k = b->size[1];
  nm1d2 *= k;
  for (k = 0; k < nm1d2; k++) {
    b->data[k] *= w->data[k];
  }

  /*  Window */
  if (b->size[1] == 0) {
    y = 0.0;
  } else {
    y = b->data[0];
    for (k = 2; k <= b->size[1]; k++) {
      y += b->data[k - 1];
    }
  }

  k = b->size[0] * b->size[1];
  b->size[0] = 1;
  emxEnsureCapacity((emxArray__common *)b, k, (int)sizeof(double));
  nm1d2 = b->size[0];
  k = b->size[1];
  nm1d2 *= k;
  for (k = 0; k < nm1d2; k++) {
    b->data[k] /= y;
  }

  /*  Normalization to unity gain at DC */
}
Esempio n. 16
0
void a_melcepst(const real_T s[512], real_T fs, int32_T nc, emxArray_real_T *c)
{
  real_T b_s[512];
  int32_T i;
  static const real_T dv0[512] = { 0.080000000000000016, 0.080034772851092173,
    0.080139086147189731, 0.080312924117550422, 0.0805562604802531,
    0.08086905844617126, 0.081251270724534919, 0.0817028395300804,
    0.082223696591786744, 0.082813763163197218, 0.083472950034324755,
    0.084201157545139238, 0.084998275600634943, 0.085864183687475115,
    0.086798750892212118, 0.0878018359210796, 0.0888732871213544,
    0.0900129425042841, 0.091220629769577732, 0.092496166331455187,
    0.093839359346251483, 0.095250005741572386, 0.09672789224699585,
    0.09827279542631584, 0.099884481711322914, 0.10156270743711604,
    0.10330721887894206, 0.10511775229055487, 0.10699403394409035,
    0.10893578017145067, 0.11094269740719032, 0.11301448223289995,
    0.11515082142307836, 0.11735139199248851, 0.11961586124498802,
    0.12194388682382867, 0.12433511676341558, 0.12678918954252011,
    0.12930573413893637, 0.1318843700855753, 0.13452470752798562,
    0.13722634728329447, 0.13998888090055894, 0.1428118907225176,
    0.14569494994873494, 0.14863762270012759, 0.1516394640848634,
    0.15470002026562302, 0.15781882852821355, 0.16099541735152506,
    0.16422930647881784, 0.16752000699033076, 0.17086702137719906,
    0.17426984361667108, 0.177727959248612, 0.181240845453283,
    0.18480797113038444, 0.18842879697935122, 0.19210277558088723,
    0.19582935147972808, 0.19960796126861807, 0.20343803367348967,
    0.20731898963983236, 0.211250242420238, 0.21523119766310839,
    0.2192612535025138, 0.2233398006491864, 0.22746622248263659,
    0.23163989514437766, 0.23586018763224437, 0.24012646189579223,
    0.24443807293276187, 0.24879436888659412, 0.25319469114498255,
    0.25763837443944609, 0.26212474694590859, 0.26665313038626953,
    0.27122284013095055, 0.27583318530240147, 0.28048346887955239,
    0.28517298780319289, 0.2899010330822655, 0.29466688990105527,
    0.29946983772726005, 0.30430915042092521, 0.30918409634422606,
    0.31409393847208128, 0.31903793450358153, 0.32401533697421447,
    0.32902539336887182, 0.33406734623561868, 0.33914043330021065,
    0.34424388758133867, 0.34937693750658638, 0.35453880702908114,
    0.35972871574482179, 0.36494587901066489, 0.3701895080629527,
    0.37545881013676219, 0.38075298858576168, 0.38607124300265128,
    0.39141276934017522, 0.39677676003268147, 0.40216240411821519,
    0.40756888736112512, 0.41299539237516436, 0.41844109874706864,
    0.42390518316059117, 0.4293868195209769, 0.43488517907985663,
    0.44039943056054276, 0.44592874028370622, 0.45147227229341824,
    0.45702918848353491, 0.46259864872440754, 0.46817981098989864,
    0.47377183148468471, 0.47937386477182686, 0.48498506390058893,
    0.490604580534485, 0.49623156507953636, 0.50186516681271842,
    0.50750453401057793, 0.51314881407800261, 0.51879715367712187,
    0.524448698856319, 0.53010259517933778, 0.53575798785446094,
    0.54141402186374354, 0.5470698420922796, 0.55272459345748381,
    0.55837742103836852, 0.5640274702047956, 0.56967388674668551,
    0.57531581700316159, 0.58095240799161241, 0.58658280753665026,
    0.59220616439894935, 0.59782162840394082, 0.60342835057034794,
    0.60902548323854022, 0.61461218019868813, 0.62018759681869828,
    0.62575089017190988, 0.63130121916453474, 0.63683774466281806,
    0.64235962961990467, 0.64786603920238861, 0.65335614091652849,
    0.65882910473410994, 0.66428410321793319, 0.6697203116469117,
    0.67513690814075755, 0.68053307378423888, 0.68590799275098879,
    0.69126085242684687, 0.69659084353271583, 0.70189716024691284,
    0.70717900032699887, 0.7124355652310671, 0.717666060238471,
    0.72286969456997574, 0.72804568150731275, 0.73319323851212115,
    0.73831158734425673, 0.74339995417945037, 0.74845756972630173,
    0.75348366934258248, 0.75847749315084323, 0.76343828615329357,
    0.7683652983459498, 0.77325778483202323, 0.77811500593454008,
    0.78293622730816892, 0.78772072005024552, 0.7924677608109707,
    0.79717663190277332, 0.80184662140881269, 0.80647702329061177,
    0.81106713749480042, 0.8156162700589541, 0.82012373321651044,
    0.82458884550075162, 0.82901093184783137, 0.83338932369883667,
    0.83772335910086348, 0.84201238280709623, 0.84625574637587087,
    0.85045280826871128, 0.8546029339473209, 0.85870549596951617,
    0.86275987408408694, 0.86676545532457061, 0.8707216341019236,
    0.87462781229607822, 0.87848339934637087, 0.88228781234082576,
    0.88604047610428438, 0.88974082328536275, 0.89338829444222823,
    0.89698233812717909, 0.90052241097001584, 0.90400797776019148,
    0.90743851152772792, 0.91081349362288644, 0.91413241379458121,
    0.91739477026752081, 0.92060006981807141, 0.92374782784882448,
    0.92683756846186127, 0.9298688245307023, 0.93284113777093092,
    0.93575405880947859, 0.938607147252565, 0.94139997175227874,
    0.94413211007179187, 0.94680314914919594, 0.94941268515995136,
    0.95196032357793992, 0.95444567923511281, 0.95686837637972111,
    0.9592280487331255, 0.96152433954517225, 0.96375690164812866,
    0.965925397509171, 0.96802949928141335, 0.970068888853475,
    0.97204325789757351, 0.97395230791614062, 0.97579575028695,
    0.97757330630675354, 0.97928470723341743, 0.98092969432655219,
    0.98250801888663064, 0.98401944229258809, 0.98546373603789827,
    0.98684068176512052, 0.98815007129891252, 0.98939170667750365,
    0.99056540018262351, 0.99167097436788332, 0.99270826208560237,
    0.99367710651207919, 0.99457736117130091, 0.99540888995708832,
    0.9961715671536735, 0.99686527745470577, 0.99748991598068559,
    0.99804538829481926, 0.9985316104172981, 0.99894850883799369,
    0.99929602052757294, 0.99957409294702582, 0.99978268405560977,
    0.99992176231720475, 0.99999130670508207, 0.99999130670508207,
    0.99992176231720475, 0.99978268405560977, 0.99957409294702582,
    0.99929602052757294, 0.99894850883799369, 0.9985316104172981,
    0.99804538829481926, 0.99748991598068559, 0.99686527745470577,
    0.9961715671536735, 0.99540888995708832, 0.99457736117130091,
    0.99367710651207919, 0.99270826208560237, 0.99167097436788332,
    0.99056540018262351, 0.98939170667750365, 0.98815007129891264,
    0.98684068176512052, 0.98546373603789827, 0.9840194422925882,
    0.98250801888663064, 0.98092969432655219, 0.97928470723341743,
    0.97757330630675365, 0.97579575028695009, 0.97395230791614062,
    0.97204325789757351, 0.970068888853475, 0.96802949928141335,
    0.96592539750917106, 0.96375690164812866, 0.96152433954517225,
    0.9592280487331255, 0.95686837637972122, 0.95444567923511281,
    0.95196032357794014, 0.94941268515995125, 0.94680314914919594,
    0.94413211007179187, 0.94139997175227885, 0.938607147252565,
    0.93575405880947871, 0.93284113777093114, 0.92986882453070241,
    0.92683756846186127, 0.92374782784882448, 0.92060006981807163,
    0.91739477026752092, 0.91413241379458121, 0.91081349362288655,
    0.90743851152772792, 0.90400797776019148, 0.900522410970016,
    0.89698233812717909, 0.89338829444222834, 0.88974082328536275,
    0.8860404761042846, 0.88228781234082587, 0.878483399346371,
    0.87462781229607822, 0.87072163410192371, 0.86676545532457072,
    0.86275987408408716, 0.85870549596951617, 0.854602933947321,
    0.85045280826871128, 0.84625574637587087, 0.84201238280709623,
    0.83772335910086393, 0.83338932369883678, 0.82901093184783159,
    0.82458884550075162, 0.82012373321651089, 0.81561627005895421,
    0.81106713749480042, 0.80647702329061177, 0.80184662140881269,
    0.79717663190277355, 0.79246776081097081, 0.78772072005024563,
    0.78293622730816925, 0.7781150059345403, 0.77325778483202334,
    0.76836529834594991, 0.7634382861532939, 0.75847749315084312,
    0.7534836693425826, 0.7484575697263014, 0.74339995417945093,
    0.73831158734425684, 0.73319323851212148, 0.72804568150731264,
    0.72286969456997607, 0.717666060238471, 0.71243556523106721,
    0.70717900032699887, 0.701897160246913, 0.696590843532716,
    0.69126085242684687, 0.68590799275098913, 0.680533073784239,
    0.67513690814075789, 0.66972031164691181, 0.66428410321793374,
    0.65882910473411, 0.65335614091652838, 0.6478660392023885,
    0.64235962961990478, 0.63683774466281828, 0.63130121916453463,
    0.62575089017190988, 0.62018759681869828, 0.61461218019868846,
    0.60902548323854022, 0.603428350570348, 0.597821628403941,
    0.59220616439894924, 0.58658280753665026, 0.58095240799161219,
    0.57531581700316192, 0.56967388674668551, 0.56402747020479571,
    0.55837742103836829, 0.55272459345748415, 0.54706984209227971,
    0.54141402186374377, 0.53575798785446127, 0.53010259517933778,
    0.52444869885631917, 0.51879715367712176, 0.51314881407800306,
    0.50750453401057793, 0.50186516681271853, 0.49623156507953631,
    0.49060458053448541, 0.48498506390058904, 0.47937386477182709,
    0.47377183148468466, 0.46817981098989869, 0.46259864872440776,
    0.45702918848353485, 0.45147227229341824, 0.44592874028370633,
    0.440399430560543, 0.43488517907985663, 0.429386819520977,
    0.42390518316059139, 0.41844109874706892, 0.41299539237516436,
    0.40756888736112484, 0.40216240411821547, 0.39677676003268147,
    0.39141276934017533, 0.3860712430026515, 0.3807529885857619,
    0.3754588101367623, 0.37018950806295287, 0.36494587901066522,
    0.35972871574482168, 0.35453880702908119, 0.34937693750658616,
    0.34424388758133895, 0.33914043330021071, 0.33406734623561879,
    0.3290253933688716, 0.32401533697421481, 0.31903793450358164,
    0.31409393847208145, 0.30918409634422594, 0.30430915042092521,
    0.29946983772726016, 0.29466688990105516, 0.2899010330822655,
    0.285172987803193, 0.28048346887955261, 0.27583318530240147,
    0.2712228401309506, 0.2666531303862697, 0.26212474694590882,
    0.25763837443944609, 0.25319469114498266, 0.24879436888659429,
    0.24443807293276176, 0.24012646189579229, 0.23586018763224448,
    0.23163989514437777, 0.22746622248263659, 0.22333980064918652,
    0.21926125350251396, 0.21523119766310861, 0.21125024242023804,
    0.20731898963983225, 0.20343803367348989, 0.19960796126861807,
    0.19582935147972819, 0.19210277558088712, 0.18842879697935144,
    0.1848079711303845, 0.18124084545328312, 0.17772795924861196,
    0.17426984361667136, 0.17086702137719911, 0.16752000699033065,
    0.16422930647881784, 0.16099541735152512, 0.15781882852821361,
    0.15470002026562296, 0.15163946408486367, 0.14863762270012765,
    0.14569494994873505, 0.1428118907225176, 0.13998888090055922,
    0.13722634728329458, 0.13452470752798557, 0.1318843700855753,
    0.12930573413893648, 0.12678918954252016, 0.12433511676341558,
    0.12194388682382873, 0.11961586124498813, 0.11735139199248862,
    0.11515082142307836, 0.1130144822329, 0.11094269740719043,
    0.10893578017145061, 0.10699403394409041, 0.10511775229055476,
    0.10330721887894218, 0.10156270743711604, 0.09988448171132297,
    0.09827279542631584, 0.0967278922469959, 0.095250005741572386,
    0.093839359346251427, 0.092496166331455243, 0.091220629769577788,
    0.0900129425042841, 0.088873287121354339, 0.087801835921079707,
    0.086798750892212118, 0.085864183687475171, 0.084998275600634943,
    0.084201157545139349, 0.083472950034324811, 0.082813763163197218,
    0.082223696591786744, 0.081702839530080451, 0.081251270724534919,
    0.08086905844617126, 0.0805562604802531, 0.080312924117550422,
    0.080139086147189731, 0.080034772851092173, 0.080000000000000016 };

  emxArray_creal_T *f;
  emxArray_real_T *m;
  int32_T ia;
  int32_T a;
  int32_T i0;
  int32_T i1;
  int32_T br;
  emxArray_creal_T *pw;
  int32_T b_f[2];
  int32_T c_f[2];
  int32_T ar;
  emxArray_creal_T d_f;
  emxArray_creal_T e_f;
  real_T b_a;
  real_T b;
  real_T f_re;
  real_T f_im;
  creal_T ath;
  boolean_T exitg1;
  creal_T b_pw;
  emxArray_creal_T *f_f;
  int32_T g_f[2];
  emxArray_real_T *b_b;
  emxArray_real_T *y;
  int32_T c_k;
  uint32_T unnamed_idx_0;
  int32_T b_m;
  int32_T ic;
  int64_T i2;
  emxArray_int32_T *r0;
  emxArray_int32_T *idx;
  emxArray_boolean_T *c_b;
  emxArray_real_T *b_c;
  emxArray_real_T *c_c;

  /* MELCEPST Calculate the mel cepstrum of a signal C=(S,FS,W,NC,P,N,INC,FL,FH) */
  /*  */
  /*  */
  /*  Simple use: c=melcepst(s,fs)	% calculate mel cepstrum with 12 coefs, 256 sample frames */
  /* 				  c=melcepst(s,fs,'e0dD') % include log energy, 0th cepstral coef, delta and delta-delta coefs */
  /*  */
  /*  Inputs: */
  /*      s	 speech signal */
  /*      fs  sample rate in Hz (default 11025) */
  /*      nc  number of cepstral coefficients excluding 0'th coefficient (default 12) */
  /*      n   length of frame in samples (default power of 2 < (0.03*fs)) */
  /*      p   number of filters in filterbank (default: floor(3*log(fs)) = approx 2.1 per ocatave) */
  /*      inc frame increment (default n/2) */
  /*      fl  low end of the lowest filter as a fraction of fs (default = 0) */
  /*      fh  high end of highest filter as a fraction of fs (default = 0.5) */
  /*  */
  /* 		w   any sensible combination of the following: */
  /*  */
  /* 				'R'  rectangular window in time domain */
  /* 				'N'	Hanning window in time domain */
  /* 				'M'	Hamming window in time domain (default) */
  /*  */
  /* 		      't'  triangular shaped filters in mel domain (default) */
  /* 		      'n'  hanning shaped filters in mel domain */
  /* 		      'm'  hamming shaped filters in mel domain */
  /*  */
  /* 				'p'	filters act in the power domain */
  /* 				'a'	filters act in the absolute magnitude domain (default) */
  /*  */
  /* 			   '0'  include 0'th order cepstral coefficient */
  /* 				'E'  include log energy */
  /* 				'd'	include delta coefficients (dc/dt) */
  /* 				'D'	include delta-delta coefficients (d^2c/dt^2) */
  /*  */
  /* 		      'z'  highest and lowest filters taper down to zero (default) */
  /* 		      'y'  lowest filter remains at 1 down to 0 frequency and */
  /* 			   	  highest filter remains at 1 up to nyquist freqency */
  /*  */
  /* 		       If 'ty' or 'ny' is specified, the total power in the fft is preserved. */
  /*  */
  /*  Outputs:	c     mel cepstrum output: one frame per row. Log energy, if requested, is the */
  /*                  first element of each row followed by the delta and then the delta-delta */
  /*                  coefficients. */
  /*  */
  /*  BUGS: (1) should have power limit as 1e-16 rather than 1e-6 (or possibly a better way of choosing this) */
  /*            and put into VOICEBOX */
  /*        (2) get rdct to change the data length (properly) instead of doing it explicitly (wrongly) */
  /*       Copyright (C) Mike Brookes 1997 */
  /*       Version: $Id: melcepst.m,v 1.8 2011/09/02 16:24:14 dmb Exp $ */
  /*  */
  /*    VOICEBOX is a MATLAB toolbox for speech processing. */
  /*    Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html */
  /*  */
  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
  /*    This program is free software; you can redistribute it and/or modify */
  /*    it under the terms of the GNU General Public License as published by */
  /*    the Free Software Foundation; either version 2 of the License, or */
  /*    (at your option) any later version. */
  /*  */
  /*    This program is distributed in the hope that it will be useful, */
  /*    but WITHOUT ANY WARRANTY; without even the implied warranty of */
  /*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the */
  /*    GNU General Public License for more details. */
  /*  */
  /*    You can obtain a copy of the GNU General Public License from */
  /*    http://www.gnu.org/copyleft/gpl.html or by writing to */
  /*    Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA. */
  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
  /* floor(3*log(fs)); */
  /* 256; %20 / 1000 * fs;  % 10 ms window */
  /* nc = 20; */
  /* z=a_enframe(s,a_hamming(n),inc); */
  /*  HAMMING.M */
  /*  */
  /*  COPYRIGHT : (c) NUHAG, Dept.Math., University of Vienna, AUSTRIA */
  /*              http://nuhag.eu/ */
  /*              Permission is granted to modify and re-distribute this */
  /*              code in any manner as long as this notice is preserved. */
  /*              All standard disclaimers apply. */
  /*  */
  /*  HAMMING.M	- returns the N-point Hamming window. */
  /*  */
  /*  Input		: n = number */
  /*  */
  /*  Output	: w = vector */
  /*  */
  /*  Usage		: w = hamming (n) */
  /*  */
  /*  Comments	: allows also the call:  hamming(xx), taking only format from signal xx */
  /*  */
  /*  See also	: HAMMING2 */
  /*  modification of original MATLAB (3.5)  file */
  /*  HGFei, 1990   */
  /* z=enframe(s,hamming(n),inc); */
  for (i = 0; i < 512; i++) {
    b_s[i] = s[i] * dv0[i];
  }

  emxInit_creal_T(&f, 1);
  emxInit_real_T(&m, 2);
  a_rfft(b_s, f);
  a_melbankm(m, &a, &ia);

  /* [m,a,b]=melbankm(p,n,fs,fl,fh, 'M'); */
  if (a > ia) {
    i0 = 0;
    i1 = 0;
  } else {
    i0 = a - 1;
    i1 = ia;
  }

  if (a > ia) {
    br = 0;
  } else {
    br = a - 1;
  }

  emxInit_creal_T(&pw, 1);
  b_f[0] = f->size[0];
  b_f[1] = 1;
  c_f[0] = f->size[0];
  c_f[1] = 1;
  i = pw->size[0];
  pw->size[0] = i1 - i0;
  emxEnsureCapacity((emxArray__common *)pw, i, (int32_T)sizeof(creal_T));
  ar = (i1 - i0) - 1;
  for (i1 = 0; i1 <= ar; i1++) {
    d_f = *f;
    d_f.size = (int32_T *)&b_f;
    d_f.numDimensions = 1;
    e_f = *f;
    e_f.size = (int32_T *)&c_f;
    e_f.numDimensions = 1;
    b_a = e_f.data[br + i1].re;
    b = -e_f.data[br + i1].im;
    f_re = d_f.data[i0 + i1].re;
    f_im = d_f.data[i0 + i1].im;
    pw->data[i1].re = f_re * b_a - f_im * b;
    pw->data[i1].im = f_re * b + f_im * b_a;
  }

  i = 1;
  br = pw->size[0];
  ath = pw->data[0];
  if (br > 1) {
    if (rtIsNaN(pw->data[0].re) || rtIsNaN(pw->data[0].im)) {
      ar = 1;
      exitg1 = 0U;
      while ((exitg1 == 0U) && (ar + 1 <= br)) {
        i = ar + 1;
        if (!(rtIsNaN(pw->data[ar].re) || rtIsNaN(pw->data[ar].im))) {
          ath = pw->data[ar];
          exitg1 = 1U;
        } else {
          ar++;
        }
      }
    }

    if (i < br) {
      while (i + 1 <= br) {
        b_pw = pw->data[i];
        if (eml_relop(b_pw, ath, TRUE)) {
          ath = pw->data[i];
        }

        i++;
      }
    }
  }

  ath.re *= 1.0E-20;
  ath.im *= 1.0E-20;
  b_sqrt(&ath);
  if (a > ia) {
    i0 = 0;
    ia = 0;
  } else {
    i0 = a - 1;
  }

  emxInit_creal_T(&f_f, 1);
  g_f[0] = f->size[0];
  g_f[1] = 1;
  i1 = f_f->size[0];
  f_f->size[0] = ia - i0;
  emxEnsureCapacity((emxArray__common *)f_f, i1, (int32_T)sizeof(creal_T));
  ar = (ia - i0) - 1;
  for (i1 = 0; i1 <= ar; i1++) {
    d_f = *f;
    d_f.size = (int32_T *)&g_f;
    d_f.numDimensions = 1;
    f_f->data[i1] = d_f.data[i0 + i1];
  }

  b_emxInit_real_T(&b_b, 1);
  b_abs(f_f, b_b);
  emxFree_creal_T(&f_f);
  b_emxInit_real_T(&y, 1);
  if ((m->size[1] == 1) || (b_b->size[0] == 1)) {
    i0 = y->size[0];
    y->size[0] = m->size[0];
    emxEnsureCapacity((emxArray__common *)y, i0, (int32_T)sizeof(real_T));
    ar = m->size[0] - 1;
    for (i0 = 0; i0 <= ar; i0++) {
      y->data[i0] = 0.0;
      i = b_b->size[0] - 1;
      for (i1 = 0; i1 <= i; i1++) {
        y->data[i0] += m->data[i0 + m->size[0] * i1] * b_b->data[i1];
      }
    }
  } else {
    c_k = m->size[1];
    unnamed_idx_0 = (uint32_T)m->size[0];
    i0 = y->size[0];
    y->size[0] = (int32_T)unnamed_idx_0;
    emxEnsureCapacity((emxArray__common *)y, i0, (int32_T)sizeof(real_T));
    b_m = m->size[0];
    i = y->size[0];
    i0 = y->size[0];
    y->size[0] = i;
    emxEnsureCapacity((emxArray__common *)y, i0, (int32_T)sizeof(real_T));
    ar = i - 1;
    for (i0 = 0; i0 <= ar; i0++) {
      y->data[i0] = 0.0;
    }

    if (b_m == 0) {
    } else {
      for (i = 0; i <= 0; i += b_m) {
        i0 = i + b_m;
        for (ic = i; ic + 1 <= i0; ic++) {
          y->data[ic] = 0.0;
        }
      }

      br = 0;
      for (i = 0; i <= 0; i += b_m) {
        ar = 0;
        i0 = br + c_k;
        for (a = br; a + 1 <= i0; a++) {
          if (b_b->data[a] != 0.0) {
            ia = ar;
            i1 = i + b_m;
            for (ic = i; ic + 1 <= i1; ic++) {
              ia++;
              y->data[ic] += b_b->data[a] * m->data[ia - 1];
            }
          }

          ar += b_m;
        }

        br += c_k;
      }
    }
  }

  emxFree_real_T(&m);
  unnamed_idx_0 = (uint32_T)y->size[0];
  i0 = f->size[0];
  f->size[0] = (int32_T)unnamed_idx_0;
  emxEnsureCapacity((emxArray__common *)f, i0, (int32_T)sizeof(creal_T));
  i0 = f->size[0];
  for (c_k = 0; c_k + 1 <= i0; c_k++) {
    if (b_eml_relop(y->data[c_k], ath, TRUE) || rtIsNaN(y->data[c_k])) {
      b_a = ath.re;
      b = ath.im;
    } else {
      b_a = y->data[c_k];
      b = 0.0;
    }

    f->data[c_k].re = b_a;
    f->data[c_k].im = b;
  }

  emxFree_real_T(&y);
  i0 = pw->size[0];
  pw->size[0] = f->size[0];
  emxEnsureCapacity((emxArray__common *)pw, i0, (int32_T)sizeof(creal_T));
  ar = f->size[0] - 1;
  for (i0 = 0; i0 <= ar; i0++) {
    pw->data[i0] = f->data[i0];
  }

  for (c_k = 0; c_k <= f->size[0] - 1; c_k++) {
    ath = pw->data[c_k];
    if ((pw->data[c_k].im == 0.0) && rtIsNaN(pw->data[c_k].re)) {
    } else if ((fabs(pw->data[c_k].re) > 8.9884656743115785E+307) || (fabs
                (pw->data[c_k].im) > 8.9884656743115785E+307)) {
      b_a = fabs(pw->data[c_k].re / 2.0);
      b = fabs(pw->data[c_k].im / 2.0);
      if (b_a < b) {
        b_a /= b;
        b *= sqrt(b_a * b_a + 1.0);
      } else if (b_a > b) {
        b /= b_a;
        b = sqrt(b * b + 1.0) * b_a;
      } else if (rtIsNaN(b)) {
      } else {
        b = b_a * 1.4142135623730951;
      }

      ath.re = log(b) + 0.69314718055994529;
      ath.im = rt_atan2d_snf(pw->data[c_k].im, pw->data[c_k].re);
    } else {
      b_a = fabs(pw->data[c_k].re);
      b = fabs(pw->data[c_k].im);
      if (b_a < b) {
        b_a /= b;
        b *= sqrt(b_a * b_a + 1.0);
      } else if (b_a > b) {
        b /= b_a;
        b = sqrt(b * b + 1.0) * b_a;
      } else if (rtIsNaN(b)) {
      } else {
        b = b_a * 1.4142135623730951;
      }

      ath.re = log(b);
      ath.im = rt_atan2d_snf(pw->data[c_k].im, pw->data[c_k].re);
    }

    pw->data[c_k] = ath;
  }

  emxFree_creal_T(&f);
  a_rdct(pw, b_b);
  i0 = c->size[0] * c->size[1];
  c->size[0] = 1;
  emxEnsureCapacity((emxArray__common *)c, i0, (int32_T)sizeof(real_T));
  i = b_b->size[0];
  i0 = c->size[0] * c->size[1];
  c->size[1] = i;
  emxEnsureCapacity((emxArray__common *)c, i0, (int32_T)sizeof(real_T));
  emxFree_creal_T(&pw);
  ar = b_b->size[0] - 1;
  for (i0 = 0; i0 <= ar; i0++) {
    c->data[i0] = b_b->data[i0];
  }

  emxFree_real_T(&b_b);
  i2 = (int64_T)nc + 1L;
  if (i2 > 2147483647L) {
    i2 = 2147483647L;
  } else {
    if (i2 < -2147483648L) {
      i2 = -2147483648L;
    }
  }

  nc = (int32_T)i2;
  if (32 > nc) {
    b_emxInit_int32_T(&r0, 1);
    i0 = c->size[1];
    i1 = r0->size[0];
    r0->size[0] = i0 - nc;
    emxEnsureCapacity((emxArray__common *)r0, i1, (int32_T)sizeof(int32_T));
    ar = (i0 - nc) - 1;
    for (i0 = 0; i0 <= ar; i0++) {
      r0->data[i0] = (nc + i0) + 1;
    }

    emxInit_int32_T(&idx, 2);
    i0 = idx->size[0] * idx->size[1];
    idx->size[0] = 1;
    emxEnsureCapacity((emxArray__common *)idx, i0, (int32_T)sizeof(int32_T));
    i = r0->size[0];
    i0 = idx->size[0] * idx->size[1];
    idx->size[1] = i;
    emxEnsureCapacity((emxArray__common *)idx, i0, (int32_T)sizeof(int32_T));
    ar = r0->size[0] - 1;
    for (i0 = 0; i0 <= ar; i0++) {
      idx->data[i0] = r0->data[i0];
    }

    emxFree_int32_T(&r0);
    if (idx->size[1] == 1) {
      i = c->size[1] - 1;
      for (ar = idx->data[0]; ar <= i; ar++) {
        c->data[c->size[0] * (ar - 1)] = c->data[c->size[0] * ar];
      }
    } else {
      emxInit_boolean_T(&c_b, 2);
      i0 = c_b->size[0] * c_b->size[1];
      c_b->size[0] = 1;
      emxEnsureCapacity((emxArray__common *)c_b, i0, (int32_T)sizeof(boolean_T));
      i = c->size[1];
      i0 = c_b->size[0] * c_b->size[1];
      c_b->size[1] = i;
      emxEnsureCapacity((emxArray__common *)c_b, i0, (int32_T)sizeof(boolean_T));
      ar = c->size[1] - 1;
      for (i0 = 0; i0 <= ar; i0++) {
        c_b->data[i0] = FALSE;
      }

      for (c_k = 1; c_k <= idx->size[1]; c_k++) {
        c_b->data[idx->data[c_k - 1] - 1] = TRUE;
      }

      i = 0;
      for (c_k = 1; c_k <= c_b->size[1]; c_k++) {
        ia = c_b->data[c_k - 1];
        i += ia;
      }

      i = c->size[1] - i;
      br = c_b->size[1];
      ar = 0;
      i0 = c->size[1];
      for (c_k = 1; c_k <= i0; c_k++) {
        if ((c_k > br) || (!c_b->data[c_k - 1])) {
          c->data[c->size[0] * ar] = c->data[c->size[0] * (c_k - 1)];
          ar++;
        }
      }

      emxFree_boolean_T(&c_b);
    }

    emxFree_int32_T(&idx);
    if (1 > i) {
      i = 0;
    }

    emxInit_real_T(&b_c, 2);
    i0 = b_c->size[0] * b_c->size[1];
    b_c->size[0] = 1;
    b_c->size[1] = i;
    emxEnsureCapacity((emxArray__common *)b_c, i0, (int32_T)sizeof(real_T));
    ar = i - 1;
    for (i0 = 0; i0 <= ar; i0++) {
      b_c->data[b_c->size[0] * i0] = c->data[c->size[0] * i0];
    }

    i0 = c->size[0] * c->size[1];
    c->size[0] = 1;
    c->size[1] = b_c->size[1];
    emxEnsureCapacity((emxArray__common *)c, i0, (int32_T)sizeof(real_T));
    ar = b_c->size[1] - 1;
    for (i0 = 0; i0 <= ar; i0++) {
      c->data[c->size[0] * i0] = b_c->data[b_c->size[0] * i0];
    }

    emxFree_real_T(&b_c);
  } else {
    if (32 < nc) {
      emxInit_real_T(&b_c, 2);
      i = nc - 32;
      i0 = b_c->size[0] * b_c->size[1];
      b_c->size[0] = 1;
      b_c->size[1] = c->size[1] + i;
      emxEnsureCapacity((emxArray__common *)b_c, i0, (int32_T)sizeof(real_T));
      ar = c->size[1] - 1;
      for (i0 = 0; i0 <= ar; i0++) {
        b_c->data[b_c->size[0] * i0] = c->data[c->size[0] * i0];
      }

      ar = i - 1;
      for (i0 = 0; i0 <= ar; i0++) {
        b_c->data[b_c->size[0] * (i0 + c->size[1])] = 0.0;
      }

      i0 = c->size[0] * c->size[1];
      c->size[0] = 1;
      c->size[1] = b_c->size[1];
      emxEnsureCapacity((emxArray__common *)c, i0, (int32_T)sizeof(real_T));
      ar = b_c->size[1] - 1;
      for (i0 = 0; i0 <= ar; i0++) {
        c->data[c->size[0] * i0] = b_c->data[b_c->size[0] * i0];
      }

      emxFree_real_T(&b_c);
    }
  }

  i = c->size[1] - 1;
  for (ar = 1; ar <= i; ar++) {
    c->data[c->size[0] * (ar - 1)] = c->data[c->size[0] * ar];
  }

  if (1 > i) {
    i = 0;
  }

  emxInit_real_T(&c_c, 2);
  i0 = c_c->size[0] * c_c->size[1];
  c_c->size[0] = 1;
  c_c->size[1] = i;
  emxEnsureCapacity((emxArray__common *)c_c, i0, (int32_T)sizeof(real_T));
  ar = i - 1;
  for (i0 = 0; i0 <= ar; i0++) {
    c_c->data[c_c->size[0] * i0] = c->data[c->size[0] * i0];
  }

  i0 = c->size[0] * c->size[1];
  c->size[0] = 1;
  c->size[1] = c_c->size[1];
  emxEnsureCapacity((emxArray__common *)c, i0, (int32_T)sizeof(real_T));
  ar = c_c->size[1] - 1;
  for (i0 = 0; i0 <= ar; i0++) {
    c->data[c->size[0] * i0] = c_c->data[c_c->size[0] * i0];
  }

  emxFree_real_T(&c_c);
}