void makeSmooth(SXMatrix &ex, SXMatrix &bvar, SXMatrix &bexpr){
// Initialize
SXFunction fcn(SXMatrix(),ex);
casadi_assert(bexpr.empty());
// Nodes to be replaced
std::map<int,SX> replace;
// Go through all nodes and check if any node is non-smooth
for(int i=0; i<fcn->algorithm.size(); ++i){
// Check if we have a step node
if(fcn->algorithm[i].op == STEP){
// Get the index of the child
int ch0 = fcn->algorithm[i].ch[0];
// Binary variable corresponding to the the switch
SXMatrix sw;
#if 0
// Find out if the switch has already been added
for(int j=0; j<bexpr.size(); ++j)
if(bexpr[j].isEqual(algorithm[i]->child0)){
sw = bvar[j];
break;
}
#endif
if(sw.empty()){ // the switch has not yet been added
// Get an approriate name of the switch
std::stringstream name;
name << "sw_" << bvar.size1();
sw = SX(name.str());
// Add to list of switches
bvar << sw;
// bexpr << algorithm[i]->child0;
}
// Add to the substition map
replace[i] = sw[0];
}
}
SXMatrix res;
fcn->eval(SXMatrix(),res,replace,bexpr);
for(int i=0; i<bexpr.size(); ++i)
bexpr[i] = bexpr[i]->dep(0);
ex = res;
#if 0
// Make sure that the binding expression is smooth
bexpr.init(SXMatrix());
SXMatrix b;
bexpr.eval_symbolic(SXMatrix(),b,replace,bexpr);
bexpr = b;
#endif
}
