SXMatrix pw_const(const SXMatrix &t, const SXMatrix &tval, const SXMatrix &val){ // number of intervals int n = val.numel(); casadi_assert_message(isScalar(t),"t must be a scalar"); casadi_assert_message(tval.numel() == n-1, "dimensions do not match"); SXMatrix ret = val(0); for(int i=0; i<n-1; ++i){ ret += (val(i+1)-val(i)) * (t>=tval(i)); } return ret; }
SXMatrix pw_lin(const SX &t, const SXMatrix &tval, const SXMatrix &val){ // Number of points int N = tval.numel(); casadi_assert_message(N>=2,"pw_lin: N>=2"); // Gradient for each line segment SXMatrix g(N-1,1); for(int i=0; i<N-1; ++i){ g(i) = (val(i+1)- val(i))/(tval(i+1)-tval(i)); } // Line segments SXMatrix lseg(N-1,1); for(int i=0; i<N-1; ++i) lseg(i) = val(i) + g(i)*(t-tval(i)); // interior time points SXMatrix tint = tval(range(N-2),0); // Return piecewise linear function return pw_const(t, tint, lseg); }
void getIntValue(const SXMatrix& ex, int *res) { for(int i=0; i<ex.numel(); ++i) res[i] = ex(i).toScalar().getIntValue(); }
void getValue(const SXMatrix& ex, double *res) { for(int i=0; i<ex.numel(); ++i) res[i] = ex(i).toScalar()->getValue(); }