/* 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 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); }
/* Function Definitions */ void imshift(uint8_T I[270000], const real_T disparity[2]) { real_T xStart2; real_T yStart2; real_T xEnd2; real_T yEnd2; int32_T i2; int32_T i3; int32_T i4; int32_T i5; int32_T tmp_size_idx_0; int32_T loop_ub; int32_T i6; int32_T tmp_data[225]; int32_T b_tmp_size_idx_0; int32_T b_tmp_data[400]; emxArray_uint8_T *b_I; int32_T iv3[3]; int32_T i7; int32_T b_loop_ub; int32_T i8; int32_T c_I[3]; int32_T c_tmp_data[400]; int32_T d_tmp_data[225]; emxArray_uint8_T *d_I; emlrtHeapReferenceStackEnterFcnR2012b(emlrtRootTLSGlobal); xStart2 = muDoubleScalarMax(1.0, 1.0 + disparity[0]); yStart2 = muDoubleScalarMax(1.0, 1.0 + disparity[1]); xEnd2 = muDoubleScalarMin(225.0, 225.0 + disparity[0]); yEnd2 = muDoubleScalarMin(400.0, 400.0 + disparity[1]); i2 = (int32_T)muDoubleScalarMax(1.0, 1.0 - disparity[0]) - 1; i3 = (int32_T)muDoubleScalarMin(225.0, 225.0 - disparity[0]); i4 = (int32_T)muDoubleScalarMax(1.0, 1.0 - disparity[1]) - 1; i5 = (int32_T)muDoubleScalarMin(400.0, 400.0 - disparity[1]); tmp_size_idx_0 = ((int32_T)xEnd2 - (int32_T)xStart2) + 1; loop_ub = (int32_T)xEnd2 - (int32_T)xStart2; for (i6 = 0; i6 <= loop_ub; i6++) { tmp_data[i6] = ((int32_T)xStart2 + i6) - 1; } b_tmp_size_idx_0 = ((int32_T)yEnd2 - (int32_T)yStart2) + 1; loop_ub = (int32_T)yEnd2 - (int32_T)yStart2; for (i6 = 0; i6 <= loop_ub; i6++) { b_tmp_data[i6] = ((int32_T)yStart2 + i6) - 1; } emxInit_uint8_T(&b_I, 3, &emlrtRTEI, TRUE); iv3[0] = tmp_size_idx_0; iv3[1] = b_tmp_size_idx_0; iv3[2] = 3; i6 = b_I->size[0] * b_I->size[1] * b_I->size[2]; b_I->size[0] = i3 - i2; b_I->size[1] = i5 - i4; b_I->size[2] = 3; emxEnsureCapacity((emxArray__common *)b_I, i6, (int32_T)sizeof(uint8_T), &emlrtRTEI); for (i6 = 0; i6 < 3; i6++) { loop_ub = i5 - i4; for (i7 = 0; i7 < loop_ub; i7++) { b_loop_ub = i3 - i2; for (i8 = 0; i8 < b_loop_ub; i8++) { b_I->data[(i8 + b_I->size[0] * i7) + b_I->size[0] * b_I->size[1] * i6] = I[((i2 + i8) + 225 * (i4 + i7)) + 90000 * i6]; } } } for (i6 = 0; i6 < 3; i6++) { c_I[i6] = b_I->size[i6]; } emxFree_uint8_T(&b_I); emlrtSubAssignSizeCheckR2012b(iv3, 3, c_I, 3, &emlrtECI, emlrtRootTLSGlobal); for (i6 = 0; i6 < b_tmp_size_idx_0; i6++) { c_tmp_data[i6] = b_tmp_data[i6]; } for (i6 = 0; i6 < tmp_size_idx_0; i6++) { d_tmp_data[i6] = tmp_data[i6]; } emxInit_uint8_T(&d_I, 3, &emlrtRTEI, TRUE); i6 = d_I->size[0] * d_I->size[1] * d_I->size[2]; d_I->size[0] = i3 - i2; d_I->size[1] = i5 - i4; d_I->size[2] = 3; emxEnsureCapacity((emxArray__common *)d_I, i6, (int32_T)sizeof(uint8_T), &emlrtRTEI); for (i6 = 0; i6 < 3; i6++) { loop_ub = i5 - i4; for (i7 = 0; i7 < loop_ub; i7++) { b_loop_ub = i3 - i2; for (i8 = 0; i8 < b_loop_ub; i8++) { d_I->data[(i8 + d_I->size[0] * i7) + d_I->size[0] * d_I->size[1] * i6] = I[((i2 + i8) + 225 * (i4 + i7)) + 90000 * i6]; } } } for (i2 = 0; i2 < 3; i2++) { loop_ub = d_I->size[1]; for (i3 = 0; i3 < loop_ub; i3++) { b_loop_ub = d_I->size[0]; for (i4 = 0; i4 < b_loop_ub; i4++) { I[(d_tmp_data[i4] + 225 * c_tmp_data[i3]) + 90000 * i2] = d_I->data[(i4 + d_I->size[0] * i3) + d_I->size[0] * d_I->size[1] * i2]; } } } emxFree_uint8_T(&d_I); if (xStart2 == 1.0) { if (xEnd2 + 1.0 > 225.0) { i2 = 0; i3 = 0; } else { i2 = (int32_T)(xEnd2 + 1.0); i2 = emlrtDynamicBoundsCheckFastR2012b(i2, 1, 225, &b_emlrtBCI, emlrtRootTLSGlobal) - 1; i3 = 225; } tmp_size_idx_0 = i3 - i2; for (i3 = 0; i3 < 3; i3++) { for (i4 = 0; i4 < 400; i4++) { for (i5 = 0; i5 < tmp_size_idx_0; i5++) { I[((i2 + i5) + 225 * i4) + 90000 * i3] = 0; } } } } else { for (i2 = 0; i2 < 3; i2++) { for (i3 = 0; i3 < 400; i3++) { loop_ub = (int32_T)(xStart2 - 1.0); for (i4 = 0; i4 < loop_ub; i4++) { I[(i4 + 225 * i3) + 90000 * i2] = 0; } } } } if (yStart2 == 1.0) { if (yEnd2 + 1.0 > 400.0) { i2 = 0; i3 = 0; } else { i2 = (int32_T)(yEnd2 + 1.0); i2 = emlrtDynamicBoundsCheckFastR2012b(i2, 1, 400, &emlrtBCI, emlrtRootTLSGlobal) - 1; i3 = 400; } tmp_size_idx_0 = i3 - i2; for (i3 = 0; i3 < 3; i3++) { for (i4 = 0; i4 < tmp_size_idx_0; i4++) { for (i5 = 0; i5 < 225; i5++) { I[(i5 + 225 * (i2 + i4)) + 90000 * i3] = 0; } } } } else { for (i2 = 0; i2 < 3; i2++) { loop_ub = (int32_T)(yStart2 - 1.0); for (i3 = 0; i3 < loop_ub; i3++) { for (i4 = 0; i4 < 225; i4++) { I[(i4 + 225 * i3) + 90000 * i2] = 0; } } } } emlrtHeapReferenceStackLeaveFcnR2012b(emlrtRootTLSGlobal); }
/* Function Definitions */ void update_eq_17_state(const real_T xk_data[6250], const int32_T xk_size[2], const emxArray_real_T *vk, const real_T uk_data[250], const int32_T uk_size[1], real_T xkPlus_data[6250], int32_T xkPlus_size[2]) { int32_T i0; int32_T i1; uint8_T uv0[2]; int32_T loop_ub; int32_T k; real_T xdot[17]; real_T vk_data[100]; real_T wi[3]; real_T ai[3]; real_T b_xk_data[25]; real_T xk[9]; real_T b_xk[3]; real_T c_xk[3]; real_T dv0[9]; real_T B; real_T dv1[9]; real_T d_xk[12]; real_T dv2[12]; static const int8_T b[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; real_T xkPlus[4]; real_T dv3[9]; real_T e_xk[9]; real_T c_xk_data[25]; real_T dv4[9]; uint8_T tmp_data[25]; int32_T iv0[1]; static const int32_T iv1[1] = { 17 }; real_T f_xk[17]; /* xk: 17 x 2N+1, inertial position, inertial velocity, inertial quaternion, relative position, and relative quaternion */ /* vk: 12 x 2N+1, noise on wi,(wj_est),ai,(vj_inertial) */ /* uk[wi;ai;mag_i] : 9 x 2N+1 */ /* IMU measurements of me */ emlrtVectorVectorIndexCheckR2012b(uk_size[0], 1, 1, 3, &l_emlrtECI, emlrtRootTLSGlobal); for (i0 = 0; i0 < 3; i0++) { i1 = 1 + i0; emlrtDynamicBoundsCheckFastR2012b(i1, 1, uk_size[0], &j_emlrtBCI, emlrtRootTLSGlobal); } emlrtVectorVectorIndexCheckR2012b(uk_size[0], 1, 1, 3, &k_emlrtECI, emlrtRootTLSGlobal); for (i0 = 0; i0 < 3; i0++) { i1 = 4 + i0; emlrtDynamicBoundsCheckFastR2012b(i1, 1, uk_size[0], &k_emlrtBCI, emlrtRootTLSGlobal); } for (i0 = 0; i0 < 2; i0++) { uv0[i0] = (uint8_T)xk_size[i0]; } xkPlus_size[0] = uv0[0]; xkPlus_size[1] = uv0[1]; loop_ub = uv0[0] * uv0[1]; for (i0 = 0; i0 < loop_ub; i0++) { xkPlus_data[i0] = 0.0; } k = 0; while (k <= xk_size[1] - 1) { i0 = (int32_T)(1.0 + (real_T)k); emlrtDynamicBoundsCheckFastR2012b(i0, 1, xk_size[1], &i_emlrtBCI, emlrtRootTLSGlobal); /* hist position relative to my. My body frame */ emlrtVectorVectorIndexCheckR2012b(xk_size[0], 1, 1, 3, &j_emlrtECI, emlrtRootTLSGlobal); for (i0 = 0; i0 < 3; i0++) { i1 = 11 + i0; emlrtDynamicBoundsCheckFastR2012b(i1, 1, xk_size[0], &l_emlrtBCI, emlrtRootTLSGlobal); } /* my velocity */ emlrtVectorVectorIndexCheckR2012b(xk_size[0], 1, 1, 3, &i_emlrtECI, emlrtRootTLSGlobal); for (i0 = 0; i0 < 3; i0++) { i1 = 4 + i0; emlrtDynamicBoundsCheckFastR2012b(i1, 1, xk_size[0], &m_emlrtBCI, emlrtRootTLSGlobal); } /* my attitude */ emlrtVectorVectorIndexCheckR2012b(xk_size[0], 1, 1, 4, &h_emlrtECI, emlrtRootTLSGlobal); for (i0 = 0; i0 < 4; i0++) { i1 = 7 + i0; emlrtDynamicBoundsCheckFastR2012b(i1, 1, xk_size[0], &n_emlrtBCI, emlrtRootTLSGlobal); } /* his attitude relative to mine. His body frame */ emlrtVectorVectorIndexCheckR2012b(xk_size[0], 1, 1, 4, &g_emlrtECI, emlrtRootTLSGlobal); for (i0 = 0; i0 < 4; i0++) { i1 = 14 + i0; emlrtDynamicBoundsCheckFastR2012b(i1, 1, xk_size[0], &o_emlrtBCI, emlrtRootTLSGlobal); } i0 = vk->size[1]; i1 = (int32_T)(1.0 + (real_T)k); emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &h_emlrtBCI, emlrtRootTLSGlobal); memset(&xdot[0], 0, 17U * sizeof(real_T)); i0 = vk->size[0]; emlrtVectorVectorIndexCheckR2012b(i0, 1, 1, 3, &f_emlrtECI, emlrtRootTLSGlobal); i0 = vk->size[0]; for (i1 = 0; i1 < 3; i1++) { loop_ub = 10 + i1; emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i0, &p_emlrtBCI, emlrtRootTLSGlobal); } i0 = vk->size[0]; emlrtVectorVectorIndexCheckR2012b(i0, 1, 1, 3, &e_emlrtECI, emlrtRootTLSGlobal); i0 = vk->size[0]; for (i1 = 0; i1 < 3; i1++) { loop_ub = 4 + i1; emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i0, &q_emlrtBCI, emlrtRootTLSGlobal); } i0 = vk->size[0]; emlrtVectorVectorIndexCheckR2012b(i0, 1, 1, 3, &d_emlrtECI, emlrtRootTLSGlobal); i0 = vk->size[0]; loop_ub = vk->size[0]; for (i1 = 0; i1 < loop_ub; i1++) { vk_data[i1] = vk->data[i1 + vk->size[0] * k]; } for (i1 = 0; i1 < 3; i1++) { loop_ub = 1 + i1; wi[i1] = uk_data[i1] + vk_data[emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i0, &r_emlrtBCI, emlrtRootTLSGlobal) - 1]; } i0 = vk->size[0]; emlrtVectorVectorIndexCheckR2012b(i0, 1, 1, 3, &c_emlrtECI, emlrtRootTLSGlobal); i0 = vk->size[0]; loop_ub = vk->size[0]; for (i1 = 0; i1 < loop_ub; i1++) { vk_data[i1] = vk->data[i1 + vk->size[0] * k]; } for (i1 = 0; i1 < 3; i1++) { loop_ub = 7 + i1; ai[i1] = uk_data[i1 + 3] + vk_data[emlrtDynamicBoundsCheckFastR2012b (loop_ub, 1, i0, &s_emlrtBCI, emlrtRootTLSGlobal) - 1]; } /* cosine matrix */ /* Tim Woodbury */ /* modified for AERO 622 */ /* */ /* [Y] = attpar(X,I,options) */ /* Function to convert between attitude parametrizations */ /* */ /* INPUTS: */ /* X - matrix input of appropriate dimension (detailed later) */ /* I - 2 x 1 indexing vector indicating the input (I(1)) and output (I(2)) */ /* attitude parametrizations, corresponding to the numbers in the section, */ /* "I/O SPECIFICATION PARAMETERS". */ /* options - a data structure. Currently only allows output Euler angle */ /* sequence to be defined. Supported members are "seq" which should be a */ /* [3 x 1] vector describing the first, second, and third rotations in */ /* the desired output sequence. */ /* */ /* OUTPUTS: */ /* Y - output matrix of appropriate dimensions. */ /* All angles are in radians. */ /* */ /* I/O SPECIFICATION PARAMETERS: */ /* 1 - Direction cosine matrix, dimensions [3 x 3] */ /* 2 - Euler principal axis/angle, [3 x 2]. [:,1] is the principal axis; */ /* [1,2] is the principal angle (rad). */ /* 3 - 2-angle parametrization, [3 x 4] */ /* 4 - Euler angle sequence, [3 x 2]. [:,1] are the rotation angles in radians, */ /* and [2,1]-[2,2]-[2,3] is the rotation sequence. */ /* [3-1-3] sequence is default for output. Other sequences may be specified */ /* by passing an optional data structure with a [3 x 1] member "seq" whose */ /* entries [a;b;c] correspond to the sequence a-b-c. Any input sequence */ /* may be used by specifying the second column of input appropriately. */ /* 5 - Classical Rodrigues parameters [3 x 1] */ /* 6 - quaternion [4 x 1]. The scalar part of the quaternion comes FIRST. */ /* 7 - modified Rodrigues parameters [3 x 1] */ /* 8 - exponential matrix, [3 x 3] */ /* 9 - Cayley-Klein parameters, [2 x 2] */ /* check if any options are passed */ /* %check if output and inp are the same - if so do nothing */ /* for each inp, convert to DCM */ /* quaternion */ /* disp('Input value specified as quaternion.'); */ /* convert DCM to output form */ /* DCM */ /* disp('Output value specified as direction cosine matrix.'); */ /* time rate of my position */ loop_ub = xk_size[0]; for (i0 = 0; i0 < loop_ub; i0++) { b_xk_data[i0] = xk_data[i0 + xk_size[0] * k]; } xk[0] = ((xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 7] - xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 8]) - xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 9]) + xk_data[xk_size[0] * k + 6] * xk_data[xk_size[0] * k + 6]; xk[1] = 2.0 * (xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 8] + xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 6]); xk[2] = 2.0 * (xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 9] - xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 6]); xk[3] = 2.0 * (xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 8] - xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 6]); xk[4] = ((xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 8] - xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 7]) - xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 9]) + xk_data[xk_size[0] * k + 6] * xk_data[xk_size[0] * k + 6]; xk[5] = 2.0 * (xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 9] + xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 6]); xk[6] = 2.0 * (xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 9] + xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 6]); xk[7] = 2.0 * (xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 9] - xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 6]); xk[8] = ((xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 9] - xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 7]) - xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 8]) + xk_data[xk_size[0] * k + 6] * xk_data[xk_size[0] * k + 6]; for (i0 = 0; i0 < 3; i0++) { b_xk[i0] = b_xk_data[i0 + 3]; } for (i0 = 0; i0 < 3; i0++) { c_xk[i0] = 0.0; for (i1 = 0; i1 < 3; i1++) { c_xk[i0] += xk[i0 + 3 * i1] * b_xk[i1]; } xdot[i0] = c_xk[i0]; } /* time rate of my velocity */ /* returns the cross product matrix of the vector x */ loop_ub = xk_size[0]; for (i0 = 0; i0 < loop_ub; i0++) { b_xk_data[i0] = xk_data[i0 + xk_size[0] * k]; } dv0[0] = 0.0; dv0[3] = -wi[2]; dv0[6] = wi[1]; dv0[1] = wi[2]; dv0[4] = 0.0; dv0[7] = -wi[0]; dv0[2] = -wi[1]; dv0[5] = wi[0]; dv0[8] = 0.0; for (i0 = 0; i0 < 3; i0++) { b_xk[i0] = b_xk_data[i0 + 3]; } for (i0 = 0; i0 < 3; i0++) { B = 0.0; for (i1 = 0; i1 < 3; i1++) { B += dv0[i0 + 3 * i1] * b_xk[i1]; } c_xk[i0] = ai[i0] - B; } for (i0 = 0; i0 < 3; i0++) { xdot[3 + i0] = c_xk[i0]; } /* time rate of my attitude */ /* A matrix for qin */ /* returns the cross product matrix of the vector x */ loop_ub = xk_size[0]; for (i0 = 0; i0 < loop_ub; i0++) { b_xk_data[i0] = xk_data[i0 + xk_size[0] * k]; } dv1[0] = 0.0; dv1[3] = -xk_data[xk_size[0] * k + 9]; dv1[6] = xk_data[xk_size[0] * k + 8]; dv1[1] = xk_data[xk_size[0] * k + 9]; dv1[4] = 0.0; dv1[7] = -xk_data[xk_size[0] * k + 7]; dv1[2] = -xk_data[xk_size[0] * k + 8]; dv1[5] = xk_data[xk_size[0] * k + 7]; dv1[8] = 0.0; for (i0 = 0; i0 < 3; i0++) { d_xk[i0 << 2] = -b_xk_data[i0 + 7]; for (i1 = 0; i1 < 3; i1++) { d_xk[(i1 + (i0 << 2)) + 1] = xk_data[xk_size[0] * k + 6] * (real_T)b[i1 + 3 * i0] + dv1[i1 + 3 * i0]; } for (i1 = 0; i1 < 4; i1++) { dv2[i1 + (i0 << 2)] = 0.5 * d_xk[i1 + (i0 << 2)]; } } for (i0 = 0; i0 < 4; i0++) { xkPlus[i0] = 0.0; for (i1 = 0; i1 < 3; i1++) { xkPlus[i0] += dv2[i0 + (i1 << 2)] * wi[i1]; } xdot[6 + i0] = xkPlus[i0]; } /* relative attitude cosine matrix */ /* Tim Woodbury */ /* modified for AERO 622 */ /* */ /* [Y] = attpar(X,I,options) */ /* Function to convert between attitude parametrizations */ /* */ /* INPUTS: */ /* X - matrix input of appropriate dimension (detailed later) */ /* I - 2 x 1 indexing vector indicating the input (I(1)) and output (I(2)) */ /* attitude parametrizations, corresponding to the numbers in the section, */ /* "I/O SPECIFICATION PARAMETERS". */ /* options - a data structure. Currently only allows output Euler angle */ /* sequence to be defined. Supported members are "seq" which should be a */ /* [3 x 1] vector describing the first, second, and third rotations in */ /* the desired output sequence. */ /* */ /* OUTPUTS: */ /* Y - output matrix of appropriate dimensions. */ /* All angles are in radians. */ /* */ /* I/O SPECIFICATION PARAMETERS: */ /* 1 - Direction cosine matrix, dimensions [3 x 3] */ /* 2 - Euler principal axis/angle, [3 x 2]. [:,1] is the principal axis; */ /* [1,2] is the principal angle (rad). */ /* 3 - 2-angle parametrization, [3 x 4] */ /* 4 - Euler angle sequence, [3 x 2]. [:,1] are the rotation angles in radians, */ /* and [2,1]-[2,2]-[2,3] is the rotation sequence. */ /* [3-1-3] sequence is default for output. Other sequences may be specified */ /* by passing an optional data structure with a [3 x 1] member "seq" whose */ /* entries [a;b;c] correspond to the sequence a-b-c. Any input sequence */ /* may be used by specifying the second column of input appropriately. */ /* 5 - Classical Rodrigues parameters [3 x 1] */ /* 6 - quaternion [4 x 1]. The scalar part of the quaternion comes FIRST. */ /* 7 - modified Rodrigues parameters [3 x 1] */ /* 8 - exponential matrix, [3 x 3] */ /* 9 - Cayley-Klein parameters, [2 x 2] */ /* check if any options are passed */ /* %check if output and inp are the same - if so do nothing */ /* for each inp, convert to DCM */ /* quaternion */ /* disp('Input value specified as quaternion.'); */ /* convert DCM to output form */ /* DCM */ /* disp('Output value specified as direction cosine matrix.'); */ /* relative angular velocity in j frame */ /* A matrix for q_ji */ /* returns the cross product matrix of the vector x */ /* time rate of relative attitude */ loop_ub = xk_size[0]; for (i0 = 0; i0 < loop_ub; i0++) { b_xk_data[i0] = xk_data[i0 + xk_size[0] * k]; } dv3[0] = 0.0; dv3[3] = -xk_data[xk_size[0] * k + 16]; dv3[6] = xk_data[xk_size[0] * k + 15]; dv3[1] = xk_data[xk_size[0] * k + 16]; dv3[4] = 0.0; dv3[7] = -xk_data[xk_size[0] * k + 14]; dv3[2] = -xk_data[xk_size[0] * k + 15]; dv3[5] = xk_data[xk_size[0] * k + 14]; dv3[8] = 0.0; for (i0 = 0; i0 < 3; i0++) { d_xk[i0 << 2] = -b_xk_data[i0 + 14]; for (i1 = 0; i1 < 3; i1++) { d_xk[(i1 + (i0 << 2)) + 1] = xk_data[xk_size[0] * k + 13] * (real_T)b[i1 + 3 * i0] + dv3[i1 + 3 * i0]; } } loop_ub = vk->size[0]; for (i0 = 0; i0 < loop_ub; i0++) { vk_data[i0] = vk->data[i0 + vk->size[0] * k]; } e_xk[0] = ((xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 14] - xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 15]) - xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k + 16]) + xk_data[xk_size[0] * k + 13] * xk_data[xk_size[0] * k + 13]; e_xk[3] = 2.0 * (xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 15] + xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k + 13]); e_xk[6] = 2.0 * (xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 16] - xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 13]); e_xk[1] = 2.0 * (xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 15] - xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k + 13]); e_xk[4] = ((xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 15] - xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 14]) - xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k + 16]) + xk_data[xk_size[0] * k + 13] * xk_data[xk_size[0] * k + 13]; e_xk[7] = 2.0 * (xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 16] + xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 13]); e_xk[2] = 2.0 * (xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 16] + xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 13]); e_xk[5] = 2.0 * (xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 16] - xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 13]); e_xk[8] = ((xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k + 16] - xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 14]) - xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 15]) + xk_data[xk_size[0] * k + 13] * xk_data[xk_size[0] * k + 13]; for (i0 = 0; i0 < 3; i0++) { for (i1 = 0; i1 < 4; i1++) { dv2[i1 + (i0 << 2)] = 0.5 * d_xk[i1 + (i0 << 2)]; } } for (i0 = 0; i0 < 3; i0++) { B = 0.0; for (i1 = 0; i1 < 3; i1++) { B += e_xk[i0 + 3 * i1] * wi[i1]; } c_xk[i0] = vk_data[i0 + 3] - B; } for (i0 = 0; i0 < 4; i0++) { xkPlus[i0] = 0.0; for (i1 = 0; i1 < 3; i1++) { xkPlus[i0] += dv2[i0 + (i1 << 2)] * c_xk[i1]; } xdot[13 + i0] = xkPlus[i0]; } /* time rate of relative position */ /* returns the cross product matrix of the vector x */ loop_ub = vk->size[0]; for (i0 = 0; i0 < loop_ub; i0++) { vk_data[i0] = vk->data[i0 + vk->size[0] * k]; } loop_ub = xk_size[0]; for (i0 = 0; i0 < loop_ub; i0++) { b_xk_data[i0] = xk_data[i0 + xk_size[0] * k]; } loop_ub = xk_size[0]; for (i0 = 0; i0 < loop_ub; i0++) { c_xk_data[i0] = xk_data[i0 + xk_size[0] * k]; } dv4[0] = 0.0; dv4[3] = -wi[2]; dv4[6] = wi[1]; dv4[1] = wi[2]; dv4[4] = 0.0; dv4[7] = -wi[0]; dv4[2] = -wi[1]; dv4[5] = wi[0]; dv4[8] = 0.0; for (i0 = 0; i0 < 3; i0++) { b_xk[i0] = c_xk_data[i0 + 10]; } for (i0 = 0; i0 < 3; i0++) { B = 0.0; for (i1 = 0; i1 < 3; i1++) { B += dv4[i0 + 3 * i1] * b_xk[i1]; } c_xk[i0] = (vk_data[i0 + 9] - b_xk_data[i0 + 3]) - B; } for (i0 = 0; i0 < 3; i0++) { xdot[10 + i0] = c_xk[i0]; } emlrtSizeEqCheck1DFastR2012b(xk_size[0], 17, &b_emlrtECI, emlrtRootTLSGlobal); loop_ub = uv0[0]; for (i0 = 0; i0 < loop_ub; i0++) { tmp_data[i0] = (uint8_T)i0; } i0 = uv0[1]; i1 = (int32_T)(1.0 + (real_T)k); emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &g_emlrtBCI, emlrtRootTLSGlobal); iv0[0] = uv0[0]; emlrtSubAssignSizeCheckR2012b(iv0, 1, iv1, 1, &emlrtECI, emlrtRootTLSGlobal); loop_ub = xk_size[0]; for (i0 = 0; i0 < loop_ub; i0++) { b_xk_data[i0] = xk_data[i0 + xk_size[0] * k]; } loop_ub = xk_size[0]; for (i0 = 0; i0 < loop_ub; i0++) { c_xk_data[i0] = b_xk_data[i0]; } for (i0 = 0; i0 < 17; i0++) { f_xk[i0] = c_xk_data[i0] + Ts * xdot[i0]; } loop_ub = uv0[0]; for (i0 = 0; i0 < loop_ub; i0++) { xkPlus_data[tmp_data[i0] + uv0[0] * k] = f_xk[i0]; } /* re-normalize */ i0 = uv0[1]; i1 = (int32_T)(1.0 + (real_T)k); emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &f_emlrtBCI, emlrtRootTLSGlobal); for (i0 = 0; i0 < 4; i0++) { i1 = uv0[0]; loop_ub = 7 + i0; emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &t_emlrtBCI, emlrtRootTLSGlobal); } i0 = uv0[1]; i1 = 1 + k; emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &e_emlrtBCI, emlrtRootTLSGlobal); for (i0 = 0; i0 < 4; i0++) { i1 = uv0[0]; loop_ub = 7 + i0; xkPlus[i0] = xkPlus_data[(emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &u_emlrtBCI, emlrtRootTLSGlobal) + uv0[0] * k) - 1]; } B = norm(xkPlus); i0 = uv0[1]; i1 = 1 + k; emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &d_emlrtBCI, emlrtRootTLSGlobal); for (i0 = 0; i0 < 4; i0++) { xkPlus[i0] = xkPlus_data[(i0 + uv0[0] * k) + 6] / B; } for (i0 = 0; i0 < 4; i0++) { i1 = uv0[0]; loop_ub = 7 + i0; xkPlus_data[(emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &v_emlrtBCI, emlrtRootTLSGlobal) + uv0[0] * k) - 1] = xkPlus[i0]; } i0 = uv0[1]; i1 = (int32_T)(1.0 + (real_T)k); emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &c_emlrtBCI, emlrtRootTLSGlobal); for (i0 = 0; i0 < 4; i0++) { i1 = uv0[0]; loop_ub = 14 + i0; emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &w_emlrtBCI, emlrtRootTLSGlobal); } i0 = uv0[1]; i1 = 1 + k; emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &b_emlrtBCI, emlrtRootTLSGlobal); for (i0 = 0; i0 < 4; i0++) { i1 = uv0[0]; loop_ub = 14 + i0; xkPlus[i0] = xkPlus_data[(emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &x_emlrtBCI, emlrtRootTLSGlobal) + uv0[0] * k) - 1]; } B = norm(xkPlus); i0 = uv0[1]; i1 = 1 + k; emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &emlrtBCI, emlrtRootTLSGlobal); for (i0 = 0; i0 < 4; i0++) { xkPlus[i0] = xkPlus_data[(i0 + uv0[0] * k) + 13] / B; } for (i0 = 0; i0 < 4; i0++) { i1 = uv0[0]; loop_ub = 14 + i0; xkPlus_data[(emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &y_emlrtBCI, emlrtRootTLSGlobal) + uv0[0] * k) - 1] = xkPlus[i0]; } k++; emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, emlrtRootTLSGlobal); } }