void CASADI_PREFIX(densify)(const real1_t* x, const int* sp_x, real2_t* y, int tr) {
/* Quick return - output ignored */
if (!y) return;
int nrow_x = sp_x[0], ncol_x = sp_x[1];
const int *colind_x = sp_x+2, *row_x = sp_x+ncol_x+3;
/* Zero out return value */
CASADI_PREFIX(fill)(y, nrow_x*ncol_x, CASADI_CAST(real2_t, 0));
/* Quick return - input is zero */
if (!x) return;
/* Copy nonzeros */
int i, el;
if (tr) {
for (i=0; i<ncol_x; ++i) {
for (el=colind_x[i]; el!=colind_x[i+1]; ++el) {
y[i + row_x[el]*ncol_x] = CASADI_CAST(real2_t, *x++);
}
}
} else {
for (i=0; i<ncol_x; ++i) {
for (el=colind_x[i]; el!=colind_x[i+1]; ++el) {
y[row_x[el] + i*nrow_x] = CASADI_CAST(real2_t, *x++);
}
}
}
}
real_t CASADI_PREFIX(interpn)(int ndim, const real_t* grid, const int* offset,
const real_t* values, const real_t* x, int* iw, real_t* w) {
/* Work vectors */
real_t* alpha = w; w += ndim;
int* index = iw; iw += ndim;
int* corner = iw; iw += ndim;
/* Left index and fraction of interval */
CASADI_PREFIX(interpn_weights)(ndim, grid, offset, x, alpha, index);
/* Loop over all corners, add contribution to output */
CASADI_PREFIX(fill)(corner, ndim, 0);
real_t ret = 0;
do {
real_t* coeff = 0;
ret += CASADI_PREFIX(interpn_interpolate)(ndim, offset, values,
alpha, index, corner, coeff);
} while (CASADI_PREFIX(flip)(corner, ndim));
return ret;
}
void CASADI_PREFIX(dense_transfer)(real_t factor, const real_t* x, const int* sp_x, real_t* y, const int* sp_y, real_t* w) {
CASADI_PREFIX(sparsify)(x, w, sp_x, false);
int nrow_y = sp_y[0];
int ncol_y = sp_y[1];
const int *colind_y = sp_y+2, *row_y = sp_y + 2 + ncol_y+1;
/* Loop over columns of y */
int i, el;
for (i=0; i<ncol_y; ++i) {
for (el=colind_y[i]; el<colind_y[i+1]; ++el) y[nrow_y*i + row_y[el]] += factor*(*w++);
}
}
void CASADI_PREFIX(interpn_grad)(real_t* grad, int ndim, const real_t* grid, const int* offset,
const real_t* values, const real_t* x, int* iw, real_t* w) {
/* Quick return */
if (!grad) return;
/* Work vectors */
real_t* alpha = w; w += ndim;
real_t* coeff = w; w += ndim;
int* index = iw; iw += ndim;
int* corner = iw; iw += ndim;
/* Left index and fraction of interval */
CASADI_PREFIX(interpn_weights)(ndim, grid, offset, x, alpha, index);
/* Loop over all corners, add contribution to output */
CASADI_PREFIX(fill)(corner, ndim, 0);
CASADI_PREFIX(fill)(grad, ndim, 0.);
do {
/* Get coefficients */
real_t v = CASADI_PREFIX(interpn_interpolate)(ndim, offset, values,
alpha, index, corner, coeff);
/* Propagate to alpha */
int i;
for (i=ndim-1; i>=0; --i) {
if (corner[i]) {
grad[i] += v*coeff[i];
v *= alpha[i];
} else {
grad[i] -= v*coeff[i];
v *= 1-alpha[i];
}
}
} while (CASADI_PREFIX(flip)(corner, ndim));
/* Propagate to x */
int i;
for (i=0; i<ndim; ++i) {
const real_t* g = grid + offset[i];
int j = index[i];
grad[i] /= g[j+1]-g[j];
}
}
void CASADI_PREFIX(interpn_weights)(int ndim, const T1* grid, const int* offset, const T1* x, T1* alpha, int* index, const int* lookup_mode) { // NOLINT(whitespace/line_length)
// Left index and fraction of interval
int i;
for (i=0; i<ndim; ++i) {
// Grid point
T1 xi = x ? x[i] : 0;
// Grid
const T1* g = grid + offset[i];
int ng = offset[i+1]-offset[i];
// Find left index
int j = index[i] = CASADI_PREFIX(low)(xi, g, ng, lookup_mode[i]);
// Get interpolation/extrapolation alpha
alpha[i] = (xi-g[j])/(g[j+1]-g[j]);
}
}
void CASADI_PREFIX(interpn_weights)(int ndim, const real_t* grid, const int* offset,
const real_t* x, real_t* alpha, int* index) {
/* Left index and fraction of interval */
int i;
for (i=0; i<ndim; ++i) {
/* Grid point */
real_t xi = x ? x[i] : 0;
/* Grid */
const real_t* g = grid + offset[i];
int ng = offset[i+1]-offset[i];
/* Find left index */
int j = index[i] = CASADI_PREFIX(low)(xi, g, ng);
/* Get interpolation/extrapolation alpha */
alpha[i] = (xi-g[j])/(g[j+1]-g[j]);
}
}
inline void CASADI_PREFIX(fill_int)(int* x, int n, int alpha) {
CASADI_PREFIX(fill)(x, n, alpha);
}
// Alias names
inline void CASADI_PREFIX(copy_int)(const int* x, int n, int* y) {
CASADI_PREFIX(copy)(x, n, y);
}
real_t CASADI_PREFIX(norm_2)(int n, const real_t* x) {
return sqrt(CASADI_PREFIX(dot)(n, x, x));
}
