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)); }