/*
* 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);
}
/*
* 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);
}
/* 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);
}
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);
}
/* 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;
}
}
/*
* 计算肯德尔系数
* 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;
}
/* 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);
}
}
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 */
}
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);
}
/* 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 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];
}
}
/*
* 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 */
}
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);
}