void transceive102_energy(transceive102_energyStackData *SD, const emlrtStack
*sp, const creal_T d2s[1408], boolean_T ft, real_T txGain, real_T rxGain,
real_T centerFreqTx, real_T centerFreqRx, real_T intFactor, real_T decFactor,
real_T swapFreqFlag, creal_T dr[1408], uint32_T *ns)
{
emlrtStack st;
st.prev = sp;
st.tls = sp->tls;
memset(&dr[0], 0, 1408U * sizeof(creal_T));
*ns = 0U;
if (!htx_not_empty) {
st.site = &emlrtRSI;
SDRuTransmitter_SDRuTransmitter(&st, &htx, centerFreqTx, txGain, intFactor);
htx_not_empty = true;
}
if (!hrx_not_empty) {
st.site = &b_emlrtRSI;
SDRuReceiver_SDRuReceiver(&st, &hrx, centerFreqRx, decFactor, rxGain);
hrx_not_empty = true;
}
/* listening mode: */
if (muDoubleScalarAbs(centerFreqTx - centerFreqRx) > 0.0) {
/* if Rx and Tx is different, switch for Listening mode */
if (swapFreqFlag != 0.0) {
st.site = &c_emlrtRSI;
SDRuBase_set_CenterFrequency(&hrx, centerFreqTx);
} else {
st.site = &d_emlrtRSI;
SDRuBase_set_CenterFrequency(&hrx, centerFreqRx);
}
}
if (ft) {
st.site = &e_emlrtRSI;
SystemCore_release(&st, &hrx);
st.site = &f_emlrtRSI;
b_SystemCore_release(&st, &htx);
} else {
st.site = &g_emlrtRSI;
SystemCore_step(&st, &htx, d2s);
while (*ns < 1U) {
st.site = &h_emlrtRSI;
b_SystemCore_step(SD, &st, &hrx, dr, ns);
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
}
}
/* Function Definitions */
static void c_getStatefromKepler_Alg_mexFun(int32_T nlhs, mxArray *plhs[2],
int32_T nrhs, const mxArray *prhs[7])
{
int32_T n;
const mxArray *inputs[7];
const mxArray *outputs[2];
int32_T b_nlhs;
emlrtStack st = { NULL, /* site */
NULL, /* tls */
NULL /* prev */
};
st.tls = emlrtRootTLSGlobal;
/* Check for proper number of arguments. */
if (nrhs != 7) {
emlrtErrMsgIdAndTxt(&st, "EMLRT:runTime:WrongNumberOfInputs", 5, 12, 7, 4,
22, "getStatefromKepler_Alg");
}
if (nlhs > 2) {
emlrtErrMsgIdAndTxt(&st, "EMLRT:runTime:TooManyOutputArguments", 3, 4, 22,
"getStatefromKepler_Alg");
}
/* Temporary copy for mex inputs. */
for (n = 0; n < nrhs; n++) {
inputs[n] = prhs[n];
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(&st);
}
}
/* Call the function. */
getStatefromKepler_Alg_api(inputs, outputs);
/* Copy over outputs to the caller. */
if (nlhs < 1) {
b_nlhs = 1;
} else {
b_nlhs = nlhs;
}
emlrtReturnArrays(b_nlhs, plhs, outputs);
/* Module termination. */
getStatefromKepler_Alg_terminate();
}
/* Function Definitions */
static void rffe_test_mexFunction(int32_T nlhs, mxArray *plhs[1], int32_T nrhs,
const mxArray *prhs[4])
{
int32_T n;
const mxArray *inputs[4];
const mxArray *outputs[1];
int32_T b_nlhs;
emlrtStack st = { NULL, NULL, NULL };
st.tls = emlrtRootTLSGlobal;
/* Check for proper number of arguments. */
if (nrhs != 4) {
emlrtErrMsgIdAndTxt(&st, "EMLRT:runTime:WrongNumberOfInputs", 5, 12, 4, 4, 9,
"rffe_test");
}
if (nlhs > 1) {
emlrtErrMsgIdAndTxt(&st, "EMLRT:runTime:TooManyOutputArguments", 3, 4, 9,
"rffe_test");
}
/* Temporary copy for mex inputs. */
for (n = 0; n < nrhs; n++) {
inputs[n] = prhs[n];
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(&st);
}
}
/* Call the function. */
rffe_test_api(inputs, outputs);
/* Copy over outputs to the caller. */
if (nlhs < 1) {
b_nlhs = 1;
} else {
b_nlhs = nlhs;
}
emlrtReturnArrays(b_nlhs, plhs, outputs);
/* Module termination. */
rffe_test_terminate();
}
void transceive202(transceive202StackData *SD, const emlrtStack *sp, const
creal_T d2s[1408], boolean_T ft, real_T txGain, real_T rxGain,
real_T centerFreqTx, real_T centerFreqRx, real_T intFactor,
real_T decFactor, creal_T dr[1408], uint32_T *ns)
{
emlrtStack st;
st.prev = sp;
st.tls = sp->tls;
memset(&dr[0], 0, 1408U * sizeof(creal_T));
*ns = 0U;
if (!htx_not_empty) {
st.site = &emlrtRSI;
SDRuTransmitter_SDRuTransmitter(&st, &htx, centerFreqTx, txGain, intFactor);
htx_not_empty = true;
}
if (!hrx_not_empty) {
st.site = &b_emlrtRSI;
SDRuReceiver_SDRuReceiver(&st, &hrx, centerFreqRx, decFactor, rxGain);
hrx_not_empty = true;
}
if (ft) {
st.site = &c_emlrtRSI;
SystemCore_release(&st, &hrx);
st.site = &d_emlrtRSI;
b_SystemCore_release(&st, &htx);
} else {
st.site = &e_emlrtRSI;
SystemCore_step(&st, &htx, d2s);
while (*ns < 1U) {
st.site = &f_emlrtRSI;
b_SystemCore_step(SD, &st, &hrx, dr, ns);
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
}
}
/* 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 */
void cadjlon(const emlrtStack *sp, real32_T *theta)
{
int32_T ii_size_idx_0;
int32_T ii_size_idx_1;
int32_T ii_data[1];
int32_T loop_ub;
int32_T i0;
int32_T ind_data[1];
int32_T tmp_data[1];
real32_T fv0[1];
real32_T fv1[1];
/* CADJLON reduces argument to range from -pi to pi for single value, */
/* use Csetminmax instead */
/* */
/* function [theta]=cadjlon(theta); */
/* */
if (*theta > 3.1415926535897931) {
ii_size_idx_0 = 1;
ii_size_idx_1 = 1;
ii_data[0] = 1;
} else {
ii_size_idx_0 = 0;
ii_size_idx_1 = 0;
}
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
ind_data[0] = 1;
i0 = 1;
}
while (!((ii_size_idx_0 == 0) || (ii_size_idx_1 == 0))) {
i0 = 0;
while (i0 <= 0) {
ii_data[0] = ind_data[0] - 1;
i0 = 1;
}
i0 = 0;
while (i0 <= 0) {
tmp_data[0] = ind_data[0];
i0 = 1;
}
fv0[0] = *theta;
i0 = 0;
while (i0 <= 0) {
fv0[tmp_data[0] - 1] = *theta - 6.28318548F;
i0 = 1;
}
*theta = fv0[0];
if (fv0[0] > 3.1415926535897931) {
ii_size_idx_0 = 1;
ii_size_idx_1 = 1;
ii_data[0] = 1;
} else {
ii_size_idx_0 = 0;
ii_size_idx_1 = 0;
}
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
ind_data[0] = ii_data[0];
i0 = 1;
}
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
if (*theta < -3.1415926535897931) {
ii_size_idx_0 = 1;
ii_size_idx_1 = 1;
ii_data[0] = 1;
} else {
ii_size_idx_0 = 0;
ii_size_idx_1 = 0;
}
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
ind_data[0] = ii_data[0];
i0 = 1;
}
while (!((ii_size_idx_0 == 0) || (ii_size_idx_1 == 0))) {
i0 = 0;
while (i0 <= 0) {
ii_data[0] = ind_data[0] - 1;
i0 = 1;
}
i0 = 0;
while (i0 <= 0) {
tmp_data[0] = ind_data[0];
i0 = 1;
}
fv1[0] = *theta;
i0 = 0;
while (i0 <= 0) {
fv1[tmp_data[0] - 1] = *theta + 6.28318548F;
i0 = 1;
}
*theta = fv1[0];
if (fv1[0] < -3.1415926535897931) {
ii_size_idx_0 = 1;
ii_size_idx_1 = 1;
ii_data[0] = 1;
} else {
ii_size_idx_0 = 0;
ii_size_idx_1 = 0;
}
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
ind_data[0] = ii_data[0];
i0 = 1;
}
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(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 Definitions */
void clcPMP_olyHyb_tmp(const emlrtStack *sp, real_T engKinPre, real_T engKinAct,
real_T gea, real_T slp, real_T batEng, real_T psiBatEng, real_T psiTim, real_T
batPwrAux, real_T batEngStp, real_T wayStp, const struct0_T *par, real_T
*cosHamMin, real_T *batFrcOut, real_T *fulFrcOut)
{
real_T mtmp;
real_T vehVel;
real_T b_engKinPre[2];
real_T crsSpdVec[2];
int32_T i18;
int32_T k;
boolean_T y;
boolean_T exitg3;
boolean_T exitg2;
real_T crsSpd;
real_T whlTrq;
real_T crsTrq;
real_T iceTrqMax;
real_T iceTrqMin;
real_T b_par[100];
real_T emoTrqMaxPos;
real_T emoTrqMinPos;
real_T emoTrqMax;
real_T emoTrqMin;
real_T batPwrMax;
real_T batPwrMin;
real_T batOcv;
real_T batEngDltMin;
real_T batEngDltMax;
real_T batEngDltMinInx;
real_T batEngDltMaxInx;
real_T batEngDlt;
real_T fulFrc;
real_T batFrc;
real_T b_batFrc;
real_T batPwr;
real_T emoTrq;
real_T iceTrq;
real_T fulPwr;
int32_T ixstart;
int32_T itmp;
int32_T ix;
boolean_T exitg1;
emlrtStack st;
emlrtStack b_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
/* CLCPMP Minimizing Hamiltonian: Co-States for soc and time */
/* Erstellungsdatum der ersten Version 19.08.2015 - Stephan Uebel */
/* */
/* Batterieleistungsgrenzen hinzugefügt am 13.12.2015 */
/* ^^added battery power limit */
/* */
/* Massenaufschlag durch Trägheitsmoment herausgenommen */
/* ^^Mass increment removed by inertia */
/* */
/* % Inputdefinition */
/* */
/* engKinPre - Double(1,1) - kinetische Energie am Intervallanfang in J */
/* ^^ kinetic energy at start of interval (J) */
/* engKinAct - Double(1,1) - kinetische Energie am Intervallende in J */
/* ^^ kinetic energe at end of interval (J) */
/* gea - Double(1,1) - Gang */
/* ^^ gear */
/* slp - Double(1,1) - Steigung in rad */
/* ^^ slope in radians */
/* iceFlg - Boolean(1,1) - Flag für Motorzustand */
/* ^^ flag for motor condition */
/* batEng - Double(1,1) - Batterieenergie in J */
/* ^^ battery energy (J) */
/* psibatEng - Double(1,1) - Costate für Batterieenergie ohne Einheit */
/* ^^ costate for battery energy w/o unity */
/* psiTim - Double(1,1) - Costate für die Zeit ohne Einheit */
/* ^^ costate for time without unity */
/* batPwrAux - Double(1,1) - elektr. Nebenverbraucherleistung in W */
/* ^^ electric auxiliary power consumed (W) */
/* batEngStp - Double(1,1) - Drehmomentschritt */
/* ^^ torque step <- no, it's a battery step */
/* wayStp - Double(1,1) - Intervallschrittweite in m */
/* ^^ interval step distance (m) */
/* par - Struct(1,1) - Modelldaten */
/* ^^ model data */
/* % Initialisieren der Ausgabe der Funktion */
/* initializing function output */
/* Ausgabewert des Minimums der Hamiltonfunktion */
/* output for minimizing the hamiltonian */
*cosHamMin = rtInf;
/* Batterieladungsänderung im Wegschritt beir minimaler Hamiltonfunktion */
/* battery change in path_idx step with the minial hamiltonian */
*batFrcOut = rtInf;
/* Kraftstoffkraftänderung im Wegschritt bei minimaler Hamiltonfunktion */
/* fuel change in path_idx step with the minimal hamiltonian */
*fulFrcOut = 0.0;
/* % Initialisieren der persistent Größen */
/* initialize the persistance variables */
/* Diese werden die nur einmal für die Funktion berechnet */
/* only calculated once for the function */
if (!crsSpdHybMax_not_empty) {
/* maximale Drehzahl Elektrommotor */
/* maximum electric motor rotational speed */
/* maximale Drehzahl der Kurbelwelle */
/* maximum crankshaft rotational speed */
crsSpdHybMax = muDoubleScalarMin(par->iceSpdMgd[14850], par->emoSpdMgd[14850]);
crsSpdHybMax_not_empty = true;
/* minimale Drehzahl der Kurbelwelle */
/* minimum crankshaft rotational speed */
crsSpdHybMin = par->iceSpdMgd[0];
}
/* % Initialisieren der allgemein benötigten Kenngrößen */
/* initializing the commonly required parameters */
/* mittlere kinetische Energie im Wegschritt berechnen */
/* define the average kinetic energy at path_idx step - is just previous KE */
/* mittlere Geschwindigkeit im Wegschritt berechnen */
/* define the average speed at path_idx step */
mtmp = 2.0 * engKinPre / par->vehMas;
st.site = &g_emlrtRSI;
if (mtmp < 0.0) {
b_st.site = &h_emlrtRSI;
eml_error(&b_st);
}
vehVel = muDoubleScalarSqrt(mtmp);
/* % vorzeitiger Funktionsabbruch? */
/* premature function termination? */
/* Drehzahl der Kurbelwelle und Grenzen */
/* crankshaft speed and limits */
/* Aus den kinetischen Energien des Fahrzeugs wird über die Raddrehzahl */
/* und die übersetzung vom Getriebe die Kurbelwellendrehzahl berechnet. */
/* Zeilenrichtung entspricht den Gängen. (Zeilenvektor) */
/* from the vehicle's kinetic energy, the crankshaft speed is calculated */
/* by the speed and gearbox translation. Line direction corresponding to */
/* the aisles (row rector). EQUATION 1 */
b_engKinPre[0] = engKinPre;
b_engKinPre[1] = engKinAct;
for (i18 = 0; i18 < 2; i18++) {
crsSpdVec[i18] = 2.0 * b_engKinPre[i18] / par->vehMas;
}
st.site = &f_emlrtRSI;
for (k = 0; k < 2; k++) {
if (crsSpdVec[k] < 0.0) {
b_st.site = &h_emlrtRSI;
eml_error(&b_st);
}
}
for (k = 0; k < 2; k++) {
crsSpdVec[k] = muDoubleScalarSqrt(crsSpdVec[k]);
}
i18 = par->geaRat->size[1];
k = (int32_T)gea;
emlrtDynamicBoundsCheckR2012b(k, 1, i18, &mb_emlrtBCI, sp);
mtmp = par->geaRat->data[(int32_T)gea - 1];
for (i18 = 0; i18 < 2; i18++) {
crsSpdVec[i18] = mtmp * crsSpdVec[i18] / par->whlDrr;
}
/* Abbruch, wenn die Drehzahlen der Kurbelwelle zu hoch im hybridischen */
/* Modus */
/* stop if the crankshaft rotatoinal speed is too high in hybrid mode */
y = false;
k = 0;
exitg3 = false;
while ((!exitg3) && (k < 2)) {
if (!!(crsSpdVec[k] > crsSpdHybMax)) {
y = true;
exitg3 = true;
} else {
k++;
}
}
if (y) {
} else {
/* Falls die Drehzahl des Verbrennungsmotors niedriger als die */
/* Leerlaufdrehzahl ist, */
/* stop if crankhaft rotional speed is lower than the idling speed */
y = false;
k = 0;
exitg2 = false;
while ((!exitg2) && (k < 2)) {
if (!!(crsSpdVec[k] < crsSpdHybMin)) {
y = true;
exitg2 = true;
} else {
k++;
}
}
if (y) {
} else {
/* Prüfen, ob die Drehzahlgrenze des Elektromotors eingehalten wird */
/* check if electric motor speed limit is maintained */
/* mittlere Kurbelwellendrehzahlen berechnen */
/* calculate average crankshaft rotational speed */
/* - really just selecting the previous path_idx KE crankshaft speed */
crsSpd = crsSpdVec[0];
/* % Längsdynamik berechnen */
/* calculate longitundinal dynamics */
/* Es wird eine konstante Beschleunigung angenommen, die im Wegschritt */
/* wayStp das Fahrzeug von velPre auf velAct beschleunigt. */
/* constant acceleration assumed when transitioning from velPre to velAct */
/* for the selected wayStp path_idx step distance */
/* Berechnen der konstanten Beschleunigung */
/* calculate the constant acceleration */
/* Aus der mittleren kinetischen Energie im Intervall, der mittleren */
/* Steigung und dem Gang lässt sich über die Fahrwiderstandsgleichung */
/* die nötige Fahrwiderstandskraft berechnen, die aufgebracht werden */
/* muss, um diese zu realisieren. */
/* from the (avg) kinetic energy in the interval, the (avg) slope and */
/* transition can calculate the necessary traction force on the driving */
/* resistance equation (PART OF EQUATION 5) */
/* Steigungskraft aus der mittleren Steigung berechnen (Skalar) */
/* gradiant force from the calculated (average) gradient */
/* Rollreibungskraft berechnen (Skalar) */
/* calculated rolling friction force - not included in EQ 5??? */
/* Luftwiderstandskraft berechnen (2*c_a/m * E_kin) (Skalar) */
/* calculated air resistance force */
/* % Berechnung der minimalen kosten der Hamiltonfunktion */
/* Calculating the minimum cost of the Hamiltonian */
/* % Berechnen der Kraft am Rad für Antriebsstrangmodus */
/* calculate the force on the wheel for the drivetrain mode */
/* % dynamische Fahrzeugmasse bei Fahrzeugmotor an berechnen. Das */
/* % heißt es werden Trägheitsmoment von Verbrennungsmotor, */
/* % Elektromotor und Rädern mit einbezogen. */
/* calculate dynamic vehicle mass with the vehicle engine (with the moment */
/* of intertia of the ICE, electric motor, and wheels) */
/* vehMasDyn = (par.iceMoi_geaRat(gea) +... */
/* par.emoGeaMoi_geaRat(gea) + par.whlMoi)/par.whlDrr^2 ... */
/* + par.vehMas; */
/* Radkraft berechnen (Beschleunigungskraft + Steigungskraft + */
/* Rollwiderstandskraft + Luftwiderstandskraft) */
/* caluclating wheel forces (accerlation force + gradient force + rolling */
/* resistance + air resistance) EQUATION 5 */
/* % Getriebeübersetzung und -verlust */
/* gear ratio and loss */
/* Das Drehmoment des Rades ergibt sich über den Radhalbmesser aus */
/* der Fahrwiderstandskraft. */
/* the weel torque is obtained from the wheel radius of the rolling */
/* resistance force (torque = force * distance (in this case, radius) */
whlTrq = ((((engKinAct - engKinPre) / (par->vehMas * wayStp) * par->vehMas
+ par->vehMas * 9.81 * muDoubleScalarSin(slp)) +
par->whlRolResCof * par->vehMas * 9.81 * muDoubleScalarCos(slp))
+ 2.0 * par->drgCof / par->vehMas * engKinPre) * par->whlDrr;
/* Berechnung des Kurbelwellenmoments */
/* Hier muss unterschieden werden, ob das Radmoment positiv oder */
/* negativ ist, da nur ein einfacher Wirkungsgrad für das Getriebe */
/* genutzt wird */
/* it's important to determine sign of crankshaft torque (positive or */
/* negative), since only a simple efficiency is used for the transmission */
/* PART OF EQ4 <- perhaps reversed? not too sure */
if (whlTrq < 0.0) {
i18 = par->geaRat->size[1];
k = (int32_T)gea;
emlrtDynamicBoundsCheckR2012b(k, 1, i18, &nb_emlrtBCI, sp);
crsTrq = whlTrq / par->geaRat->data[(int32_T)gea - 1] * par->geaEfy;
} else {
i18 = par->geaRat->size[1];
k = (int32_T)gea;
emlrtDynamicBoundsCheckR2012b(k, 1, i18, &ob_emlrtBCI, sp);
crsTrq = whlTrq / par->geaRat->data[(int32_T)gea - 1] / par->geaEfy;
}
/* % Verbrennungsmotor */
/* internal combustion engine */
/* maximales Moment des Verbrennungsmotors berechnen */
/* calculate max torque of the engine (quadratic based on rotation speed) */
iceTrqMax = (par->iceTrqMaxCof[0] * (crsSpdVec[0] * crsSpdVec[0]) +
par->iceTrqMaxCof[1] * crsSpdVec[0]) + par->iceTrqMaxCof[2];
/* minimales Moment des Verbrennungsmotors berechnen */
/* calculating mimimum ICE moment */
iceTrqMin = (par->iceTrqMinCof[0] * (crsSpdVec[0] * crsSpdVec[0]) +
par->iceTrqMinCof[1] * crsSpdVec[0]) + par->iceTrqMinCof[2];
/* % Elektromotor */
/* electric motor */
/* maximales Moment, dass die E-Maschine liefern kann */
/* max torque that the electric motor can provide - from interpolation */
/* emoTrqMaxPos = ... */
/* lininterp1(par.emoSpdMgd(1,:)',par.emoTrqMax_emoSpd,crsSpd); */
for (i18 = 0; i18 < 100; i18++) {
b_par[i18] = par->emoSpdMgd[150 * i18];
}
emoTrqMaxPos = interp1q(b_par, par->emoTrqMax_emoSpd, crsSpdVec[0]);
/* Die gültigen Kurbelwellenmomente müssen kleiner sein als das */
/* Gesamtmoment von E-Motor und Verbrennungsmotor */
/* The valid crankshaft moments must be less than the total moment of the */
/* electric motor and the ICE.Otherwise, leave the function */
if (crsTrq > iceTrqMax + emoTrqMaxPos) {
} else {
/* % %% Optimaler Momentensplit - Minimierung der Hamiltonfunktion */
/* optimum torque split - minimizing the Hamiltonian */
/* Die Vorgehensweise ist ähnlich wie bei der ECMS. Es wird ein Vektor der */
/* möglichen Batterieenergieänderungen aufgestellt. Aus diesen lässt sich */
/* eine Batterieklemmleistung berechnen. Aus der über das */
/* Kurbelwellenmoment, ein Elektromotormoment berechnet werden kann. */
/* Über das geforderte Kurbelwellenmoment, kann für jedes Moment des */
/* Elektromotors ein Moment des Verbrennungsmotors gefunden werden. Für */
/* jedes Momentenpaar kann die Hamiltonfunktion berechnet werden. */
/* Ausgegeben wird der minimale Wert der Hamiltonfunktion und die */
/* durch das dabei verwendete Elektromotormoment verursachte */
/* Batterieladungsänderung. */
/* The procedure is similar to ECMS. It's based on a vector of possible */
/* battery energy changes, from which a battery terminal power can be */
/* calculated. */
/* From the crankshaft torque, an electric motor torque can be */
/* calculated, and an engine torque can be found for every electric motor */
/* moment. */
/* For every moment-pair the Hamiltonian can be calculated. It */
/* outputs the minimum Hamilotnian value and the battery charge change */
/* caused by the electric motor torque used. */
/* % Elektromotor - Aufstellen des Batterienergievektors */
/* electric motor - positioning the battery energy vectors */
if (batEngStp > 0.0) {
/* Skalar - änderung der minimalen Batterieenergieänderung */
/* Skalar - änderung der maximalen Batterieenergieänderung */
/* FUNCTION CALL */
/* Skalar - Wegschrittweite */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Nebenverbraucherlast */
/* Skalar - Batterieenergie */
/* struct - Fahrzeugparameter */
/* Skalar - crankshaft rotational speed */
/* Skalar - crankshaft torque */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque allowed */
/* Skalar - max EM torque possible */
st.site = &e_emlrtRSI;
/* Skalar - änderung der minimalen Batterieenergieänderung */
/* Skalar - änderung der maximalen Batterieenergieänderung */
/* Skalar - Wegschrittweite */
/* Skalar - Geschwindigkeit im Intervall */
/* Skalar - Nebenverbraucherlast */
/* Skalar - Batterieenergie */
/* struct - Fahrzeugparameter */
/* Skalar - crankshaft rotational speed */
/* Skalar - crankshaft torque */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* Skalar - max EM torque possible */
/* BatEngDltClc Calculates the change in battery energy */
/* */
/* Erstellungsdatum der ersten Version 17.11.2015 - Stephan Uebel */
/* Berechnung der Verluste des Elektromotors bei voller rein elektrischer */
/* Fahrt (voller Lastpunktabsenkung) und bei voller Lastpunktanhebung */
/* Calculations of loss of electric motor at purely full electric */
/* Driving (full load point lowering) and at full load point raising */
/* */
/* Version vom 17.02.2016: Keine Einbeziehung von Rotationsmassen */
/* ^^ No inclusion of rotational masses */
/* */
/* Version vom 25.05.2016: Zero-Order-Hold (keine mittlere Geschwindigkeit) */
/* ^^ Zero-Order-Hold (no average velocities) */
/* % Initialisieren der Ausgabe der Funktion */
/* initializing the function output (delta battery_energy min and max) */
/* % Elektromotor */
/* minimales Moment, dass die E-Maschine liefern kann */
/* minimum moment that the EM can provide (max is an input to function) */
/* emoTrqMinPos = ... */
/* lininterp1(par.emoSpdMgd(1,:)',par.emoTrqMin_emoSpd,crsSpd); */
for (i18 = 0; i18 < 100; i18++) {
b_par[i18] = par->emoSpdMgd[150 * i18];
}
emoTrqMinPos = interp1q(b_par, par->emoTrqMin_emoSpd, crsSpdVec[0]);
/* % Verbrennungsmotor berechnen */
/* Durch EM zu lieferndes Kurbelwellenmoment */
/* crankshaft torque to be delivered by the electric motor (min and max) */
emoTrqMax = crsTrq - iceTrqMin;
emoTrqMin = crsTrq - iceTrqMax;
/* % Elektromotor berechnen */
/* calculate the electric motor */
if (emoTrqMaxPos < emoTrqMax) {
/* Moment des Elektromotors bei maximaler Entladung der Batterie */
/* EM torque at max battery discharge */
emoTrqMax = emoTrqMaxPos;
}
if (emoTrqMaxPos < emoTrqMin) {
/* Moment des Elektromotors bei minimaler Entladung der Batterie */
/* EM torque at min battery discharge */
emoTrqMin = emoTrqMaxPos;
}
emoTrqMax = muDoubleScalarMax(emoTrqMinPos, emoTrqMax);
emoTrqMin = muDoubleScalarMax(emoTrqMinPos, emoTrqMin);
/* % Berechnung der änderung der Batterieladung */
/* calculating the change in battery charge */
/* Interpolation der benötigten Batterieklemmleistung für das */
/* EM-Moment. Stellen die nicht definiert sind, werden mit inf */
/* ausgegeben. Positive Werte entsprechen entladen der Batterie. */
/* interpolating the required battery terminal power for the EM torque. */
/* Assign undefined values to inf. Positive values coresspond with battery */
/* discharge. */
/* batPwrMax = lininterp2(par.emoSpdMgd(1,:),par.emoTrqMgd(:,1),... */
/* par.emoPwr_emoSpd_emoTrq',crsSpd,emoTrqMax) + batPwrAux; */
/* */
/* batPwrMin = lininterp2(par.emoSpdMgd(1,:),par.emoTrqMgd(:,1),... */
/* par.emoPwr_emoSpd_emoTrq',crsSpd,emoTrqMin) + batPwrAux; */
b_st.site = &i_emlrtRSI;
batPwrMax = codegen_interp2(&b_st, par->emoSpdMgd, par->emoTrqMgd,
par->emoPwr_emoSpd_emoTrq, crsSpdVec[0], emoTrqMax) + batPwrAux;
b_st.site = &j_emlrtRSI;
batPwrMin = codegen_interp2(&b_st, par->emoSpdMgd, par->emoTrqMgd,
par->emoPwr_emoSpd_emoTrq, crsSpdVec[0], emoTrqMin) + batPwrAux;
/* überprüfen, ob Batterieleistung möglich */
/* make sure that current battery max power is not above bat max bounds */
if (batPwrMax > par->batPwrMax) {
batPwrMax = par->batPwrMax;
}
/* überprüfen, ob Batterieleistung möglich */
/* make sure that current battery min power is not below bat min bounds */
if (batPwrMin > par->batPwrMax) {
batPwrMin = par->batPwrMax;
}
/* Es kann vorkommen, dass mehr Leistung gespeist werden soll, als */
/* möglich ist. */
/* double check that the max and min still remain within the other bounds */
if (batPwrMax < par->batPwrMin) {
batPwrMax = par->batPwrMin;
}
if (batPwrMin < par->batPwrMin) {
batPwrMin = par->batPwrMin;
}
/* Batteriespannung aus Kennkurve berechnen */
/* calculating battery voltage of characteristic curve - eq?-------------- */
batOcv = batEng * par->batOcvCof_batEng[0] + par->batOcvCof_batEng[1];
/* FUNCTION CALL - min delta bat.energy */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open-circuit voltage */
batEngDltMin = batFrcClc_tmp(batPwrMax, vehVel, par->batRstDch,
par->batRstChr, batOcv) * wayStp;
/* <-multiply by delta_S */
/* FUNCTION CALL - max delta bat.energy */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open-circuit voltage */
batEngDltMax = batFrcClc_tmp(batPwrMin, vehVel, par->batRstDch,
par->batRstChr, batOcv) * wayStp;
/* Es werden 2 Fälle unterschieden: */
/* there are 2 different cases */
if ((batEngDltMin > 0.0) && (batEngDltMax > 0.0)) {
/* %% konventionelles Bremsen + Rekuperieren */
/* conventional brakes + recuperation */
/* */
/* set change in energy to discretized integer values w/ ceil and */
/* floor. This also helps for easy looping */
/* Konventionelles Bremsen wird ebenfalls untersucht. */
/* Hier liegt eventuell noch Beschleunigungspotential, da diese */
/* Zustandswechsel u.U. ausgeschlossen werden können. */
/* NOTE: u.U. = unter Ümständen = circumstances permitting */
/* convetional breaks also investigated. An accelerating potential */
/* is still possible, as these states can be excluded */
/* (circumstances permitting) <- ??? not sure what above means */
/* */
/* so if both min and max changes in battery energy are */
/* increasing, then set the delta min to zero */
batEngDltMinInx = 0.0;
batEngDltMaxInx = muDoubleScalarFloor(batEngDltMax / batEngStp);
} else {
batEngDltMinInx = muDoubleScalarCeil(batEngDltMin / batEngStp);
batEngDltMaxInx = muDoubleScalarFloor(batEngDltMax / batEngStp);
}
} else {
batEngDltMinInx = 0.0;
batEngDltMaxInx = 0.0;
}
/* you got a larger min chnage and a max change, you're out of bounds. Leave */
/* the function */
if (batEngDltMaxInx < batEngDltMinInx) {
} else {
/* % Schleife über alle Elektromotormomente */
/* now loop through all the electric-motor torques */
i18 = (int32_T)(batEngDltMaxInx + (1.0 - batEngDltMinInx));
emlrtForLoopVectorCheckR2012b(batEngDltMinInx, 1.0, batEngDltMaxInx,
mxDOUBLE_CLASS, i18, &o_emlrtRTEI, sp);
k = 0;
while (k <= i18 - 1) {
batEngDlt = (batEngDltMinInx + (real_T)k) * batEngStp;
/* open circuit voltage over each iteration */
batOcv = (batEng + batEngDlt) * par->batOcvCof_batEng[0] +
par->batOcvCof_batEng[1];
/* Skalar für die Batterieleistung in W */
/* Skalar Krafstoffkraft in N */
/* FUNCTION CALL */
/* Skalar für die Wegschrittweite in m, */
/* Skalar - vehicular velocity */
/* Nebenverbraucherlast */
/* Skalar - battery open circuit voltage */
/* Skalar - Batterieenergie�nderung, */
/* Skalar - crankshaft speed at given path_idx */
/* Skalar - crankshaft torque at given path_idx */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* struct der Fahrzeugparameter */
st.site = &d_emlrtRSI;
/* Skalar für die Batterieleistung */
/* Skalar Kraftstoffkraft */
/* Skalar für die Wegschrittweite in m */
/* vehicular velocity */
/* Nebenverbraucherlast */
/* Skalar - battery open circuit voltage */
/* Skalar - Batterieenergieänderung */
/* Skalar - crankshaft speed at given path_idx */
/* Skalar - crankshaft torque at given path_idx */
/* Skalar - min ICE torque allowed */
/* Skalar - max ICE torque */
/* struct der Fahrzeugparameter */
/* */
/* FULENGCLC Calculating fuel consumption */
/* Erstellungsdatum der ersten Version 04.09.2015 - Stephan Uebel */
/* */
/* Diese Funktion berechnet den Kraftstoffverbrauch für einen gegebenen */
/* Wegschritt der kinetischen Energie, der Batterieenergie und des */
/* Antriebsstrangzustands über dem Weg. */
/* this function calculates fuel consumption for a given route step of */
/* KE, the battery energy, and drivetrain state of the current path_idx */
/* */
/* Version vom 17.02.2016 : Rotationsmassen vernachlässigt */
/* ^^ neglected rotatary masses */
/* */
/* Version vom 25.05.2016: Zero-Order-Hold (keine mittlere Geschwindigkeit) */
/* */
/* version from 1.06.2016 - removed crsTrq calulations - they are already */
/* done in parent function (clcPMP_olHyb_tmp) and do not change w/ each */
/* iteration, making the caluclation here unnecessary */
/* % Initialisieren der Ausgabe der Funktion */
/* initializing function output */
/* Skalar - electric battery power (W) */
fulFrc = rtInf;
/* Skalar - fuel force intialization (N) */
/* % Batterie */
/* Batterieenergieänderung über dem Weg (Batteriekraft) */
/* Change in battery energy over the path_idx way (ie battery power) */
batFrc = batEngDlt / wayStp;
/* Fallunterscheidung, ob Batterie geladen oder entladen wird */
/* Case analysis - check if battery is charging or discharging. Set */
/* resistance accordingly */
/* elektrische Leistung des Elektromotors */
/* electric power from electric motor - DERIVATION? dunno */
/* innere Batterieleistung / internal batt power */
/* dissipat. Leist. / dissipated */
if (batFrc < 0.0) {
b_batFrc = par->batRstDch;
} else {
b_batFrc = par->batRstChr;
}
batPwr = (-batFrc * vehVel - batFrc * batFrc * (vehVel * vehVel) /
(batOcv * batOcv) * b_batFrc) - batPwrAux;
/* Nebenverbrauchlast / auxiliary power */
/* % Elektromotor */
/* Ermitteln des Kurbelwellenmoments durch EM aus Batterieleistung */
/* determine crankshaft torque cauesd by EM's battery power */
/* switching out codegen_interp2 for lininterp2-just might work! */
/* */
b_st.site = &k_emlrtRSI;
emoTrq = codegen_interp2(&b_st, par->emoSpdMgd, par->emoPwrMgd,
par->emoTrq_emoSpd_emoPwr, crsSpd, batPwr);
/* emoTrq = lininterp2(par.emoSpdMgd(1,:), par.emoPwrMgd(:,1),... */
/* par.emoTrq_emoSpd_emoPwr',crsSpd,emoPwrEle); */
if (muDoubleScalarIsInf(emoTrq)) {
} else {
/* Grenzen des Elektromotors müssen hier nicht überprüft werden, da die */
/* Ladungsdeltas schon so gewählt wurden, dass das maximale Motormoment */
/* nicht überstiegen wird. */
/* Electric motor limits need not be checked here, since the charge deltas */
/* have been chosen so that the max torque is not exceeded. */
/* % Verbrennungsmotor */
/* Ermitteln des Kurbelwellenmoments durch den Verbrennungsmotor */
/* Determining the crankshaft moment from the ICE */
iceTrq = crsTrq - emoTrq;
/* Wenn das Verbrennungsmotormoment kleiner als das minimale */
/* Moment ist, ist dieser in der Schubabschaltung. Das */
/* verbleibende Moment liefern die Bremsen */
/* If engine torque is less than the min torque, fuel is cut. The */
/* remaining torque is deliver the brakes. */
if (iceTrq < iceTrqMin) {
fulPwr = 0.0;
} else if (iceTrq > iceTrqMax) {
fulPwr = rtInf;
} else {
/* replacing another coden_interp2 */
b_st.site = &l_emlrtRSI;
fulPwr = codegen_interp2(&b_st, par->iceSpdMgd, par->iceTrqMgd,
par->iceFulPwr_iceSpd_iceTrq, crsSpd, iceTrq);
/* fulPwr = lininterp2(par.iceSpdMgd(1,:), par.iceTrqMgd(:,1), ... */
/* par.iceFulPwr_iceSpd_iceTrq', crsSpd, iceTrq); */
}
/* Berechnen der Kraftstoffkraft */
/* calculate fuel force */
fulFrc = fulPwr / vehVel;
/* Berechnen der Kraftstoffvolumenänderung */
/* caluclate change in fuel volume - energy, no? */
/* % Ende der Funktion */
}
/* FUNCTION CALL */
/* Skalar - Batterieklemmleistung */
/* Skalar - mittlere Geschwindigkeit im Intervall */
/* Skalar - Entladewiderstand */
/* Skalar - Ladewiderstand */
/* Skalar - battery open circuit voltage */
batFrc = batFrcClc_tmp(batPwr, vehVel, par->batRstDch,
par->batRstChr, batOcv);
/* %% Hamiltonfunktion bestimmen */
/* determine the hamiltonian */
/* Eq (29b) */
crsSpdVec[0] = (fulFrc + psiBatEng * batFrc) + psiTim / vehVel;
ixstart = 1;
mtmp = crsSpdVec[0];
itmp = 1;
if (muDoubleScalarIsNaN(crsSpdVec[0])) {
ix = 2;
exitg1 = false;
while ((!exitg1) && (ix < 3)) {
ixstart = 2;
if (!muDoubleScalarIsNaN(*cosHamMin)) {
mtmp = *cosHamMin;
itmp = 2;
exitg1 = true;
} else {
ix = 3;
}
}
}
if ((ixstart < 2) && (*cosHamMin < mtmp)) {
mtmp = *cosHamMin;
itmp = 2;
}
*cosHamMin = mtmp;
/* Wenn der aktuelle Punkt besser ist, als der in cosHamMin */
/* gespeicherte Wert, werden die Ausgabegrößen neu beschrieben. */
/* if the current point is better than the stored cosHamMin value, */
/* then the output values are rewritten (selected prev. value is = 2) */
if (itmp == 1) {
*batFrcOut = batFrc;
*fulFrcOut = fulFrc;
}
k++;
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
}
}
}
}
/* end of function */
}
/* Function Definitions */
real_T compressedindex(const emlrtStack *sp, const emxArray_real_T *x, const
emxArray_real_T *ctable, real_T range, real_T dims)
{
real_T y;
real_T py;
int32_T i;
int32_T i2;
int32_T i3;
real_T d4;
int32_T k;
int32_T vlen;
boolean_T p;
boolean_T b_p;
int32_T exitg1;
int32_T b_k;
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;
/* for a given vector, find its index within the transition space matrix */
/* workspace; */
py = 1.0;
range++;
emlrtForLoopVectorCheckR2012b(1.0, 1.0, dims - 2.0, mxDOUBLE_CLASS, (int32_T)
(dims - 2.0), (emlrtRTEInfo *)&o_emlrtRTEI, sp);
i = 0;
while (i <= (int32_T)(dims - 2.0) - 1) {
i2 = x->size[1];
if (!((i + 1 >= 1) && (i + 1 <= i2))) {
emlrtDynamicBoundsCheckR2012b(i + 1, 1, i2, (emlrtBCInfo *)&bb_emlrtBCI,
sp);
}
if (x->data[i] == 0.0) {
} else {
i2 = x->size[1];
if (!((i + 1 >= 1) && (i + 1 <= i2))) {
emlrtDynamicBoundsCheckR2012b(i + 1, 1, i2, (emlrtBCInfo *)&cb_emlrtBCI,
sp);
}
d4 = range - (x->data[i] - 1.0);
if (d4 > range) {
i3 = 1;
i2 = 1;
} else {
if (d4 != (int32_T)muDoubleScalarFloor(d4)) {
emlrtIntegerCheckR2012b(d4, (emlrtDCInfo *)&k_emlrtDCI, sp);
}
i2 = ctable->size[0];
i3 = (int32_T)d4;
if (!((i3 >= 1) && (i3 <= i2))) {
emlrtDynamicBoundsCheckR2012b(i3, 1, i2, (emlrtBCInfo *)&x_emlrtBCI,
sp);
}
if (range != (int32_T)muDoubleScalarFloor(range)) {
emlrtIntegerCheckR2012b(range, (emlrtDCInfo *)&k_emlrtDCI, sp);
}
i2 = ctable->size[0];
k = (int32_T)range;
if (!((k >= 1) && (k <= i2))) {
emlrtDynamicBoundsCheckR2012b(k, 1, i2, (emlrtBCInfo *)&x_emlrtBCI, sp);
}
i2 = k + 1;
}
st.site = &l_emlrtRSI;
d4 = dims - (1.0 + (real_T)i);
if (d4 != (int32_T)muDoubleScalarFloor(d4)) {
emlrtIntegerCheckR2012b(d4, (emlrtDCInfo *)&l_emlrtDCI, &st);
}
k = ctable->size[1];
vlen = (int32_T)d4;
if (!((vlen >= 1) && (vlen <= k))) {
emlrtDynamicBoundsCheckR2012b(vlen, 1, k, (emlrtBCInfo *)&y_emlrtBCI,
&st);
}
b_st.site = &m_emlrtRSI;
if ((i2 - i3 == 1) || (i2 - i3 != 1)) {
p = true;
} else {
p = false;
}
if (p) {
} else {
emlrtErrorWithMessageIdR2012b(&b_st, &p_emlrtRTEI,
"Coder:toolbox:autoDimIncompatibility", 0);
}
p = false;
b_p = false;
k = 0;
do {
exitg1 = 0;
if (k < 2) {
if (k + 1 <= 1) {
b_k = i2 - i3;
} else {
b_k = 1;
}
if (b_k != 0) {
exitg1 = 1;
} else {
k++;
}
} else {
b_p = true;
exitg1 = 1;
}
} while (exitg1 == 0);
if (!b_p) {
} else {
p = true;
}
if (!p) {
} else {
emlrtErrorWithMessageIdR2012b(&b_st, &q_emlrtRTEI,
"Coder:toolbox:UnsupportedSpecialEmpty", 0);
}
c_st.site = &n_emlrtRSI;
if (i2 - i3 == 0) {
y = 0.0;
} else {
vlen = i2 - i3;
y = ctable->data[(i3 + ctable->size[0] * ((int32_T)(dims - (1.0 +
(real_T)i)) - 1)) - 1];
d_st.site = &o_emlrtRSI;
if ((!(2 > i2 - i3)) && (i2 - i3 > 2147483646)) {
e_st.site = &j_emlrtRSI;
check_forloop_overflow_error(&e_st);
}
for (k = 0; k + 2 <= vlen; k++) {
y += ctable->data[(i3 + k) + ctable->size[0] * ((int32_T)(dims - (1.0
+ (real_T)i)) - 1)];
}
}
py += y;
i2 = x->size[1];
if (!((i + 1 >= 1) && (i + 1 <= i2))) {
emlrtDynamicBoundsCheckR2012b(i + 1, 1, i2, (emlrtBCInfo *)&db_emlrtBCI,
sp);
}
range -= x->data[i];
}
i++;
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
i2 = x->size[1];
i3 = x->size[1] - 1;
if (!((i3 >= 1) && (i3 <= i2))) {
emlrtDynamicBoundsCheckR2012b(i3, 1, i2, (emlrtBCInfo *)&ab_emlrtBCI, sp);
}
return py + x->data[i3 - 1];
}
/* 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 [counts] = get_unique_counts(a,uniques)
*/
void get_unique_counts(const emlrtStack *sp, const emxArray_real_T *a, const
emxArray_real_T *uniques, emxArray_real_T *counts)
{
int32_T i0;
int32_T loop_ub;
int32_T i;
emxArray_boolean_T *s;
real_T b_uniques;
int32_T k;
emlrtHeapReferenceStackEnterFcnR2012b(sp);
/* 'get_unique_counts:3' counts = zeros(numel(uniques),1); */
i0 = counts->size[0];
counts->size[0] = uniques->size[0];
emxEnsureCapacity(sp, (emxArray__common *)counts, i0, (int32_T)sizeof(real_T),
&emlrtRTEI);
loop_ub = uniques->size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
counts->data[i0] = 0.0;
}
/* 'get_unique_counts:5' for i = 1:numel(uniques) */
i = 1;
emxInit_boolean_T(sp, &s, 3, &emlrtRTEI, true);
while (i - 1 <= uniques->size[0] - 1) {
/* 'get_unique_counts:6' counts(i) = nnz(a==uniques(i)); */
i0 = s->size[0] * s->size[1] * s->size[2];
s->size[0] = a->size[0];
s->size[1] = a->size[1];
s->size[2] = a->size[2];
emxEnsureCapacity(sp, (emxArray__common *)s, i0, (int32_T)sizeof(boolean_T),
&emlrtRTEI);
i0 = uniques->size[0];
if (!((i >= 1) && (i <= i0))) {
emlrtDynamicBoundsCheckR2012b(i, 1, i0, &emlrtBCI, sp);
}
b_uniques = uniques->data[i - 1];
loop_ub = a->size[0] * a->size[1] * a->size[2];
for (i0 = 0; i0 < loop_ub; i0++) {
s->data[i0] = (a->data[i0] == b_uniques);
}
loop_ub = 0;
i0 = s->size[0] * s->size[1] * s->size[2];
for (k = 0; k < i0; k++) {
if (s->data[k]) {
loop_ub++;
}
}
i0 = counts->size[0];
if (!((i >= 1) && (i <= i0))) {
emlrtDynamicBoundsCheckR2012b(i, 1, i0, &b_emlrtBCI, sp);
}
counts->data[i - 1] = loop_ub;
i++;
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
emxFree_boolean_T(&s);
emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
