void
PuzzleSolver::LinearSolver(PuzzleSolution* solution)
{
Position nextPop = solution->NextPop();
if (nextPop.m_x < BoardWidth && nextPop.m_y < BoardHeight)
{
solution->CurrentPoints() += solution->Board().Pop(nextPop.m_x, nextPop.m_y);
solution->m_solutionPath.push_back(Position(solution->NextPop().m_x, solution->NextPop().m_y));
}
PieceMap matches;
solution->Board().GetPoppableItems(matches);
//if this is the end of this line of pops then add to solutions and return
if (0 >= matches.size())
{
PuzzleSolution* processSolution = new PuzzleSolution(*solution);
m_outputQueue.Push(processSolution);
m_process.notify_one();
}
for (auto pos : matches)
{
PuzzleSolution* newSolution = new PuzzleSolution(*solution);
newSolution->NextPop() = pos;
newSolution->Level() += 1;
LinearSolver(newSolution);
}
delete solution; solution = nullptr;
}
void ImplicitFixedStepIntegratorInternal::init() {
// Call the base class init
FixedStepIntegratorInternal::init();
// Get the NLP creator function
std::string implicit_function_name = getOption("implicit_solver");
// Allocate an NLP solver
implicit_solver_ = ImplicitFunction(implicit_function_name, F_, Function(), LinearSolver());
implicit_solver_.setOption("name", string(getOption("name")) + "_implicit_solver");
implicit_solver_.setOption("implicit_input", DAE_Z);
implicit_solver_.setOption("implicit_output", DAE_ALG);
// Pass options
if (hasSetOption("implicit_solver_options")) {
const Dictionary& implicit_solver_options = getOption("implicit_solver_options");
implicit_solver_.setOption(implicit_solver_options);
}
// Initialize the solver
implicit_solver_.init();
// Allocate a root-finding solver for the backward problem
if (nRZ_>0) {
// Get the NLP creator function
std::string backward_implicit_function_name = getOption("implicit_solver");
// Allocate an NLP solver
backward_implicit_solver_ = ImplicitFunction(backward_implicit_function_name,
G_, Function(), LinearSolver());
backward_implicit_solver_.setOption("name",
string(getOption("name")) + "_backward_implicit_solver");
backward_implicit_solver_.setOption("implicit_input", RDAE_RZ);
backward_implicit_solver_.setOption("implicit_output", RDAE_ALG);
// Pass options
if (hasSetOption("implicit_solver_options")) {
const Dictionary& backward_implicit_solver_options = getOption("implicit_solver_options");
backward_implicit_solver_.setOption(backward_implicit_solver_options);
}
// Initialize the solver
backward_implicit_solver_.init();
}
}
void QcqpToSocp::init() {
// Initialize the base classes
QcqpSolverInternal::init();
// Collection of sparsities that will make up SOCP_SOLVER_G
std::vector<Sparsity> socp_g;
// Allocate Cholesky solvers
cholesky_.push_back(LinearSolver("csparsecholesky", st_[QCQP_STRUCT_H]));
for (int i=0;i<nq_;++i) {
cholesky_.push_back(
LinearSolver("csparsecholesky",
DMatrix(st_[QCQP_STRUCT_P])(range(i*n_, (i+1)*n_), ALL).sparsity()));
}
for (int i=0;i<nq_+1;++i) {
// Initialize Cholesky solve
cholesky_[i].init();
// Harvest Cholsesky sparsity patterns
// Note that we add extra scalar to make room for the epigraph-reformulation variable
socp_g.push_back(blkdiag(
cholesky_[i].getFactorizationSparsity(false), Sparsity::dense(1, 1)));
}
// Create an SocpSolver instance
solver_ = SocpSolver(getOption(solvername()),
socpStruct("g", horzcat(socp_g),
"a", horzcat(input(QCQP_SOLVER_A).sparsity(),
Sparsity::sparse(nc_, 1))));
//solver_.setQCQPOptions();
if (hasSetOption(optionsname())) solver_.setOption(getOption(optionsname()));
std::vector<int> ni(nq_+1);
for (int i=0;i<nq_+1;++i) {
ni[i] = n_+1;
}
solver_.setOption("ni", ni);
// Initialize the SocpSolver
solver_.init();
}
/**
* Parametrized constructor of the factory object.
* \param aF The vector-function to fit.
* \param aFillJac The jacobian of the vector-function to fit.
* \param aY The desired output vector of the function (to be matched by the algorithm).
* \param aMaxIter The maximum number of iterations to perform.
* \param aTau The initial, relative damping value to damp the Hessian matrix.
* \param aEpsj The tolerance on the norm of the gradient.
* \param aEpsx The tolerance on the norm of the step size.
* \param aEpsy The tolerance on the norm of the residual.
* \param aImposeLimits The functor that can impose simple limits on the search domain (i.e. using this boils down to a gradient projection method, for more complex constraints please use a constraint optimization method instead).
* \param aLinSolver The functor that can solve a linear system.
*/
levenberg_marquardt_nllsq_factory(Function aF,
JacobianFunction aFillJac,
OutputVector aY, unsigned int aMaxIter = 100,
T aTau = T(1e-3), T aEpsj = T(1e-6),
T aEpsx = T(1e-6), T aEpsy = T(1e-6),
LimitFunction aImposeLimits = LimitFunction(),
LinearSolver aLinSolver = LinearSolver()) :
f(aF), fill_jac(aFillJac), y(aY), max_iter(aMaxIter),
tau(aTau), epsj(aEpsj), epsx(aEpsx), epsy(aEpsy),
impose_limits(aImposeLimits),
lin_solve(aLinSolver) { };
LinearSolver GslInternal::getLinearSolver() { return LinearSolver();}
void SDPSDQPInternal::init() {
// Initialize the base classes
SdqpSolverInternal::init();
cholesky_ = LinearSolver("csparsecholesky", st_[SDQP_STRUCT_H]);
cholesky_.init();
MX g_socp = MX::sym("x", cholesky_.getFactorizationSparsity(true));
MX h_socp = MX::sym("h", n_);
MX f_socp = sqrt(inner_prod(h_socp, h_socp));
MX en_socp = 0.5/f_socp;
MX f_sdqp = MX::sym("f", input(SDQP_SOLVER_F).sparsity());
MX g_sdqp = MX::sym("g", input(SDQP_SOLVER_G).sparsity());
std::vector<MX> fi(n_+1);
MX znp = MX::sparse(n_+1, n_+1);
for (int k=0;k<n_;++k) {
MX gk = vertcat(g_socp(ALL, k), DMatrix::sparse(1, 1));
MX fk = -blockcat(znp, gk, gk.T(), DMatrix::sparse(1, 1));
// TODO(Joel): replace with ALL
fi.push_back(blkdiag(f_sdqp(ALL, Slice(f_sdqp.size1()*k, f_sdqp.size1()*(k+1))), fk));
}
MX fin = en_socp*DMatrix::eye(n_+2);
fin(n_, n_+1) = en_socp;
fin(n_+1, n_) = en_socp;
fi.push_back(blkdiag(DMatrix::sparse(f_sdqp.size1(), f_sdqp.size1()), -fin));
MX h0 = vertcat(h_socp, DMatrix::sparse(1, 1));
MX g = blockcat(f_socp*DMatrix::eye(n_+1), h0, h0.T(), f_socp);
g = blkdiag(g_sdqp, g);
IOScheme mappingIn("g_socp", "h_socp", "f_sdqp", "g_sdqp");
IOScheme mappingOut("f", "g");
mapping_ = MXFunction(mappingIn("g_socp", g_socp, "h_socp", h_socp,
"f_sdqp", f_sdqp, "g_sdqp", g_sdqp),
mappingOut("f", horzcat(fi), "g", g));
mapping_.init();
// Create an sdpsolver instance
std::string sdpsolver_name = getOption("sdp_solver");
sdpsolver_ = SdpSolver(sdpsolver_name,
sdpStruct("g", mapping_.output("g").sparsity(),
"f", mapping_.output("f").sparsity(),
"a", horzcat(input(SDQP_SOLVER_A).sparsity(),
Sparsity::sparse(nc_, 1))));
if (hasSetOption("sdp_solver_options")) {
sdpsolver_.setOption(getOption("sdp_solver_options"));
}
// Initialize the SDP solver
sdpsolver_.init();
sdpsolver_.input(SDP_SOLVER_C).at(n_)=1;
// Output arguments
setNumOutputs(SDQP_SOLVER_NUM_OUT);
output(SDQP_SOLVER_X) = DMatrix::zeros(n_, 1);
std::vector<int> r = range(input(SDQP_SOLVER_G).size1());
output(SDQP_SOLVER_P) = sdpsolver_.output(SDP_SOLVER_P).isEmpty() ? DMatrix() :
sdpsolver_.output(SDP_SOLVER_P)(r, r);
output(SDQP_SOLVER_DUAL) = sdpsolver_.output(SDP_SOLVER_DUAL).isEmpty() ? DMatrix() :
sdpsolver_.output(SDP_SOLVER_DUAL)(r, r);
output(SDQP_SOLVER_COST) = 0.0;
output(SDQP_SOLVER_DUAL_COST) = 0.0;
output(SDQP_SOLVER_LAM_X) = DMatrix::zeros(n_, 1);
output(SDQP_SOLVER_LAM_A) = DMatrix::zeros(nc_, 1);
}
::Spacy::LinearSolver LinearOperator::solver() const
{
return LinearSolver( get() );
}
Function implicitRK(Function& f, const std::string& impl, const Dictionary& impl_options,
const MX& tf, int order, const std::string& scheme, int ne) {
casadi_assert_message(ne>=1, "Parameter ne (number of elements must be at least 1), "
"but got " << ne << ".");
casadi_assert_message(order==4, "Only RK order 4 is supported now.");
casadi_assert_message(f.getNumInputs()==DAE_NUM_IN && f.getNumOutputs()==DAE_NUM_OUT,
"Supplied function must adhere to dae scheme.");
casadi_assert_message(f.output(DAE_QUAD).isEmpty(),
"Supplied function cannot have quadrature states.");
// Obtain collocation points
std::vector<double> tau_root = collocationPoints(order, "legendre");
// Retrieve collocation interpolating matrices
std::vector < std::vector <double> > C;
std::vector < double > D;
collocationInterpolators(tau_root, C, D);
// Retrieve problem dimensions
int nx = f.input(DAE_X).size();
int nz = f.input(DAE_Z).size();
int np = f.input(DAE_P).size();
//Variables for one finite element
MX X = MX::sym("X", nx);
MX P = MX::sym("P", np);
MX V = MX::sym("V", order*(nx+nz)); // Unknowns
MX X0 = X;
// Components of the unknowns that correspond to states at collocation points
std::vector<MX> Xc;Xc.reserve(order);
Xc.push_back(X0);
// Components of the unknowns that correspond to algebraic states at collocation points
std::vector<MX> Zc;Zc.reserve(order);
// Splitting the unknowns
std::vector<int> splitPositions = range(0, order*nx, nx);
if (nz>0) {
std::vector<int> Zc_pos = range(order*nx, order*nx+(order+1)*nz, nz);
splitPositions.insert(splitPositions.end(), Zc_pos.begin(), Zc_pos.end());
} else {
splitPositions.push_back(order*nx);
}
std::vector<MX> Vs = vertsplit(V, splitPositions);
// Extracting unknowns from Z
for (int i=0;i<order;++i) {
Xc.push_back(X0+Vs[i]);
}
if (nz>0) {
for (int i=0;i<order;++i) {
Zc.push_back(Vs[order+i]);
}
}
// Get the collocation Equations (that define V)
std::vector<MX> V_eq;
// Local start time
MX t0_l=MX::sym("t0");
MX h = MX::sym("h");
for (int j=1;j<order+1;++j) {
// Expression for the state derivative at the collocation point
MX xp_j = 0;
for (int r=0;r<order+1;++r) {
xp_j+= C[j][r]*Xc[r];
}
// Append collocation equations & algebraic constraints
std::vector<MX> f_out;
MX t_l = t0_l+tau_root[j]*h;
if (nz>0) {
f_out = f.call(daeIn("t", t_l, "x", Xc[j], "p", P, "z", Zc[j-1]));
} else {
f_out = f.call(daeIn("t", t_l, "x", Xc[j], "p", P));
}
V_eq.push_back(h*f_out[DAE_ODE]-xp_j);
V_eq.push_back(f_out[DAE_ALG]);
}
// Root-finding function, implicitly defines V as a function of X0 and P
std::vector<MX> vfcn_inputs;
vfcn_inputs.push_back(V);
vfcn_inputs.push_back(X);
vfcn_inputs.push_back(P);
vfcn_inputs.push_back(t0_l);
vfcn_inputs.push_back(h);
Function vfcn = MXFunction(vfcn_inputs, vertcat(V_eq));
vfcn.init();
try {
// Attempt to convert to SXFunction to decrease overhead
vfcn = SXFunction(vfcn);
vfcn.init();
} catch(CasadiException & e) {
//
}
// Create a implicit function instance to solve the system of equations
ImplicitFunction ifcn(impl, vfcn, Function(), LinearSolver());
ifcn.setOption(impl_options);
ifcn.init();
// Get an expression for the state at the end of the finite element
std::vector<MX> ifcn_call_in(5);
ifcn_call_in[0] = MX::zeros(V.sparsity());
std::copy(vfcn_inputs.begin()+1, vfcn_inputs.end(), ifcn_call_in.begin()+1);
std::vector<MX> ifcn_call_out = ifcn.call(ifcn_call_in, true);
Vs = vertsplit(ifcn_call_out[0], splitPositions);
MX XF = 0;
for (int i=0;i<order+1;++i) {
XF += D[i]*(i==0? X : X + Vs[i-1]);
}
// Get the discrete time dynamics
ifcn_call_in.erase(ifcn_call_in.begin());
MXFunction F = MXFunction(ifcn_call_in, XF);
F.init();
// Loop over all finite elements
MX h_ = tf/ne;
MX t0_ = 0;
for (int i=0;i<ne;++i) {
std::vector<MX> F_in;
F_in.push_back(X);
F_in.push_back(P);
F_in.push_back(t0_);
F_in.push_back(h_);
t0_+= h_;
std::vector<MX> F_out = F.call(F_in);
X = F_out[0];
}
// Create a ruturn function with Integrator signature
MXFunction ret = MXFunction(integratorIn("x0", X0, "p", P), integratorOut("xf", X));
ret.init();
return ret;
}
void SdqpToSdp::init() {
// Initialize the base classes
SdqpSolverInternal::init();
cholesky_ = LinearSolver("cholesky", "csparsecholesky", st_[SDQP_STRUCT_H]);
MX g_socp = MX::sym("x", cholesky_.getFactorizationSparsity(true));
MX h_socp = MX::sym("h", n_);
MX f_socp = sqrt(inner_prod(h_socp, h_socp));
MX en_socp = 0.5/f_socp;
MX f_sdqp = MX::sym("f", input(SDQP_SOLVER_F).sparsity());
MX g_sdqp = MX::sym("g", input(SDQP_SOLVER_G).sparsity());
std::vector<MX> fi(n_+1);
MX znp = MX(n_+1, n_+1);
for (int k=0;k<n_;++k) {
MX gk = vertcat(g_socp(ALL, k), MX(1, 1));
MX fk = -blockcat(znp, gk, gk.T(), MX(1, 1));
// TODO(Joel): replace with ALL
fi.push_back(diagcat(f_sdqp(ALL, Slice(f_sdqp.size1()*k, f_sdqp.size1()*(k+1))), fk));
}
MX fin = en_socp*DMatrix::eye(n_+2);
fin(n_, n_+1) = en_socp;
fin(n_+1, n_) = en_socp;
fi.push_back(diagcat(DMatrix(f_sdqp.size1(), f_sdqp.size1()), -fin));
MX h0 = vertcat(h_socp, DMatrix(1, 1));
MX g = blockcat(f_socp*DMatrix::eye(n_+1), h0, h0.T(), f_socp);
g = diagcat(g_sdqp, g);
Dict opts;
opts["input_scheme"] = IOScheme("g_socp", "h_socp", "f_sdqp", "g_sdqp");
opts["output_scheme"] = IOScheme("f", "g");
mapping_ = MXFunction("mapping", make_vector(g_socp, h_socp, f_sdqp, g_sdqp),
make_vector(horzcat(fi), g), opts);
Dict options;
if (hasSetOption(optionsname())) options = getOption(optionsname());
// Create an SdpSolver instance
solver_ = SdpSolver("sdpsolver", getOption(solvername()),
make_map("g", mapping_.output("g").sparsity(),
"f", mapping_.output("f").sparsity(),
"a", horzcat(input(SDQP_SOLVER_A).sparsity(),
Sparsity(nc_, 1))),
options);
solver_.input(SDP_SOLVER_C).at(n_)=1;
// Output arguments
obuf_.resize(SDQP_SOLVER_NUM_OUT);
output(SDQP_SOLVER_X) = DMatrix::zeros(n_, 1);
std::vector<int> r = range(input(SDQP_SOLVER_G).size1());
output(SDQP_SOLVER_P) = solver_.output(SDP_SOLVER_P).isempty() ? DMatrix() :
solver_.output(SDP_SOLVER_P)(r, r);
output(SDQP_SOLVER_DUAL) = solver_.output(SDP_SOLVER_DUAL).isempty() ? DMatrix() :
solver_.output(SDP_SOLVER_DUAL)(r, r);
output(SDQP_SOLVER_COST) = 0.0;
output(SDQP_SOLVER_DUAL_COST) = 0.0;
output(SDQP_SOLVER_LAM_X) = DMatrix::zeros(n_, 1);
output(SDQP_SOLVER_LAM_A) = DMatrix::zeros(nc_, 1);
}
void ImplicitFunctionInternal::init() {
// Initialize the residual function
f_.init(false);
// Which input/output correspond to the root-finding problem?
iin_ = getOption("implicit_input");
iout_ = getOption("implicit_output");
// Get the number of equations and check consistency
casadi_assert_message(iin_>=0 && iin_<f_.getNumInputs() && f_.getNumInputs()>0,
"Implicit input not in range");
casadi_assert_message(iout_>=0 && iout_<f_.getNumOutputs() && f_.getNumOutputs()>0,
"Implicit output not in range");
casadi_assert_message(f_.output(iout_).isDense() && f_.output(iout_).isVector(),
"Residual must be a dense vector");
casadi_assert_message(f_.input(iin_).isDense() && f_.input(iin_).isVector(),
"Unknown must be a dense vector");
n_ = f_.output(iout_).size();
casadi_assert_message(n_ == f_.input(iin_).size(),
"Dimension mismatch. Input size is "
<< f_.input(iin_).size()
<< ", while output size is "
<< f_.output(iout_).size());
// Allocate inputs
setNumInputs(f_.getNumInputs());
for (int i=0; i<getNumInputs(); ++i) {
input(i) = f_.input(i);
}
// Allocate output
setNumOutputs(f_.getNumOutputs());
for (int i=0; i<getNumOutputs(); ++i) {
output(i) = f_.output(i);
}
// Same input and output schemes
setInputScheme(f_.getInputScheme());
setOutputScheme(f_.getOutputScheme());
// Call the base class initializer
FunctionInternal::init();
// Generate Jacobian if not provided
if (jac_.isNull()) jac_ = f_.jacobian(iin_, iout_);
jac_.init(false);
// Check for structural singularity in the Jacobian
casadi_assert_message(
!jac_.output().sparsity().isSingular(),
"ImplicitFunctionInternal::init: singularity - the jacobian is structurally rank-deficient. "
"sprank(J)=" << sprank(jac_.output()) << " (instead of "<< jac_.output().size1() << ")");
// Get the linear solver creator function
if (linsol_.isNull()) {
if (hasSetOption("linear_solver")) {
std::string linear_solver_name = getOption("linear_solver");
// Allocate an NLP solver
linsol_ = LinearSolver(linear_solver_name, jac_.output().sparsity(), 1);
// Pass options
if (hasSetOption("linear_solver_options")) {
const Dictionary& linear_solver_options = getOption("linear_solver_options");
linsol_.setOption(linear_solver_options);
}
// Initialize
linsol_.init();
}
} else {
// Initialize the linear solver, if provided
linsol_.init(false);
casadi_assert(linsol_.input().sparsity()==jac_.output().sparsity());
}
// No factorization yet;
fact_up_to_date_ = false;
// Constraints
if (hasSetOption("constraints")) u_c_ = getOption("constraints");
casadi_assert_message(u_c_.size()==n_ || u_c_.empty(),
"Constraint vector if supplied, must be of length n, but got "
<< u_c_.size() << " and n = " << n_);
}
