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 }