std::vector<Sparsity> LrDpleInternal::getSparsity(const std::map<std::string, std::vector<Sparsity> >& st, const std::vector< std::vector<int> > &Hs_) { // Chop-up the arguments std::vector<Sparsity> As, Vs, Cs, Hs; if (st.count("a")) As = st.at("a"); if (st.count("v")) Vs = st.at("v"); if (st.count("c")) Cs = st.at("c"); if (st.count("h")) Hs = st.at("h"); bool with_H = !Hs.empty(); int K = As.size(); Sparsity A; if (K==1) { A = As[0]; } else { Sparsity AL = diagcat(vector_slice(As, range(As.size()-1))); Sparsity AL2 = horzcat(AL, Sparsity(AL.size1(), As[0].size2())); Sparsity AT = horzcat(Sparsity(As[0].size1(), AL.size2()), As.back()); A = vertcat(AT, AL2); } Sparsity V = diagcat(Vs.back(), diagcat(vector_slice(Vs, range(Vs.size()-1)))); Sparsity C; if (!Cs.empty()) { C = diagcat(Cs.back(), diagcat(vector_slice(Cs, range(Cs.size()-1)))); } Sparsity H; std::vector<int> Hs_agg; std::vector<int> Hi(1, 0); if (Hs_.size()>0 && with_H) { H = diagcat(Hs.back(), diagcat(vector_slice(Hs, range(Hs.size()-1)))); casadi_assert(K==Hs_.size()); for (int k=0;k<K;++k) { Hs_agg.insert(Hs_agg.end(), Hs_[k].begin(), Hs_[k].end()); int sum=0; for (int i=0;i<Hs_[k].size();++i) { sum+= Hs_[k][i]; } Hi.push_back(Hi.back()+sum); } } Sparsity res = LrDleInternal::getSparsity(make_map("a", A, "v", V, "c", C, "h", H), Hs_agg); if (with_H) { return diagsplit(res, Hi); } else { return diagsplit(res, As[0].size2()); } }
std::vector<T> permute(const std::vector<T> &a, const std::vector<casadi_int> &order) { casadi_assert_dev(order.size()==a.size()); std::set<casadi_int> order_set(order.begin(), order.end()); casadi_assert_dev(order_set.size()==a.size()); casadi_assert_dev(*order_set.begin()==0); casadi_assert_dev(*order_set.rbegin()==a.size()-1); return vector_slice(a, order); }
void LiftingLrDpleInternal::init() { form_ = getOptionEnumValue("form"); // Initialize the base classes LrDpleInternal::init(); casadi_assert_message(!pos_def_, "pos_def option set to True: Solver only handles the indefinite case."); casadi_assert_message(const_dim_, "const_dim option set to False: Solver only handles the True case."); // We will construct an MXFunction to facilitate the calculation of derivatives MX As = MX::sym("As", input(LR_DLE_A).sparsity()); MX Vs = MX::sym("Vs", input(LR_DLE_V).sparsity()); MX Cs = MX::sym("Cs", input(LR_DLE_C).sparsity()); MX Hs = MX::sym("Hs", input(LR_DLE_H).sparsity()); n_ = A_[0].size1(); // Chop-up the arguments std::vector<MX> As_ = horzsplit(As, n_); std::vector<MX> Vs_ = horzsplit(Vs, V_[0].size2()); std::vector<MX> Cs_ = horzsplit(Cs, V_[0].size2()); std::vector<MX> Hs_; if (with_H_) { Hs_ = horzsplit(Hs, Hsi_); } MX A; if (K_==1) { A = As; } else { if (form_==0) { MX AL = diagcat(vector_slice(As_, range(As_.size()-1))); MX AL2 = horzcat(AL, MX::sparse(AL.size1(), As_[0].size2())); MX AT = horzcat(MX::sparse(As_[0].size1(), AL.size2()), As_.back()); A = vertcat(AT, AL2); } else { MX AL = diagcat(reverse(vector_slice(As_, range(As_.size()-1)))); MX AL2 = horzcat(MX::sparse(AL.size1(), As_[0].size2()), AL); MX AT = horzcat(As_.back(), MX::sparse(As_[0].size1(), AL.size2())); A = vertcat(AL2, AT); } } MX V; MX C; MX H; if (form_==0) { V = diagcat(Vs_.back(), diagcat(vector_slice(Vs_, range(Vs_.size()-1)))); if (with_C_) { C = diagcat(Cs_.back(), diagcat(vector_slice(Cs_, range(Cs_.size()-1)))); } } else { V = diagcat(diagcat(reverse(vector_slice(Vs_, range(Vs_.size()-1)))), Vs_.back()); if (with_C_) { C = diagcat(diagcat(reverse(vector_slice(Cs_, range(Cs_.size()-1)))), Cs_.back()); } } if (with_H_) { H = diagcat(form_==0? Hs_ : reverse(Hs_)); } // Create an LrDleSolver instance solver_ = LrDleSolver(getOption(solvername()), lrdleStruct("a", A.sparsity(), "v", V.sparsity(), "c", C.sparsity(), "h", H.sparsity())); solver_.setOption("Hs", Hss_); if (hasSetOption(optionsname())) solver_.setOption(getOption(optionsname())); solver_.init(); std::vector<MX> v_in(LR_DPLE_NUM_IN); v_in[LR_DLE_A] = As; v_in[LR_DLE_V] = Vs; if (with_C_) { v_in[LR_DLE_C] = Cs; } if (with_H_) { v_in[LR_DLE_H] = Hs; } std::vector<MX> Pr = solver_.call(lrdpleIn("a", A, "v", V, "c", C, "h", H)); MX Pf = Pr[0]; std::vector<MX> Ps = with_H_ ? diagsplit(Pf, Hsi_) : diagsplit(Pf, n_); if (form_==1) { Ps = reverse(Ps); } f_ = MXFunction(v_in, dpleOut("p", horzcat(Ps))); f_.setInputScheme(SCHEME_LR_DPLEInput); f_.setOutputScheme(SCHEME_LR_DPLEOutput); f_.init(); Wrapper::checkDimensions(); }