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