/**
* Return the coefficients for PROMOTE: a column vector of size N with all
* entries 1. Note this is treated as sparse for consistency of later
* multiplications with sparse matrices.
*
* Parameters: linOP with type PROMOTE
*
* Returns: vector containing coefficient matrix ONES.
*
*/
std::vector<Matrix> get_promote_mat(LinOp &lin) {
assert(lin.type == PROMOTE);
int num_entries = lin.size[0] * lin.size[1];
Matrix ones = sparse_ones(num_entries, 1);
ones.makeCompressed();
return build_vector(ones);
}
bool solve(Matrix& A, Vector const& b, Vector& x, EigenOption& opt) override
{
INFO("-> solve with %s (precon %s)",
EigenOption::getSolverName(opt.solver_type).c_str(),
EigenOption::getPreconName(opt.precon_type).c_str());
_solver.setTolerance(opt.error_tolerance);
_solver.setMaxIterations(opt.max_iterations);
if (!A.isCompressed())
A.makeCompressed();
_solver.compute(A);
if(_solver.info()!=Eigen::Success) {
ERR("Failed during Eigen linear solver initialization");
return false;
}
x = _solver.solveWithGuess(b, x);
INFO("\t iteration: %d/%ld", _solver.iterations(), opt.max_iterations);
INFO("\t residual: %e\n", _solver.error());
if(_solver.info()!=Eigen::Success) {
ERR("Failed during Eigen linear solve");
return false;
}
return true;
}
/**
* Return the coefficients for MUL (left multiplication): a NUM_BLOCKS * ROWS
* by NUM_BLOCKS * COLS block diagonal matrix where each diagonal block is the
* constant data BLOCK.
*
* Parameters: linOp with type MUL
*
* Returns: vector containing coefficient matrix COEFFS
*
*/
std::vector<Matrix> get_mul_mat(LinOp &lin) {
assert(lin.type == MUL);
Matrix block = get_constant_data(lin, false);
int block_rows = block.rows();
int block_cols = block.cols();
// Don't replicate scalars
if(block_rows == 1 && block_cols == 1){
return build_vector(block);
}
int num_blocks = lin.size[1];
Matrix coeffs (num_blocks * block_rows, num_blocks * block_cols);
std::vector<Triplet> tripletList;
tripletList.reserve(num_blocks * block.nonZeros());
for (int curr_block = 0; curr_block < num_blocks; curr_block++) {
int start_i = curr_block * block_rows;
int start_j = curr_block * block_cols;
for ( int k = 0; k < block.outerSize(); ++k ) {
for ( Matrix::InnerIterator it(block, k); it; ++it ) {
tripletList.push_back(Triplet(start_i + it.row(), start_j + it.col(),
it.value()));
}
}
}
coeffs.setFromTriplets(tripletList.begin(), tripletList.end());
coeffs.makeCompressed();
return build_vector(coeffs);
}
/**
* Return the coefficients for NEG: -I, where I is an identity of size m * n.
*
* Parameters: linOp with type NEG
*
* Returns: vector containing the coefficient matrix COEFFS
*/
std::vector<Matrix> get_neg_mat(LinOp &lin) {
assert(lin.type == NEG);
int n = lin.size[0] * lin.size[1];
Matrix coeffs = sparse_eye(n);
coeffs *= -1;
coeffs.makeCompressed();
return build_vector(coeffs);
}
/**
* Return the coefficient matrix for SUM_ENTRIES. A single row vector of 1's
* of size 1 by (data.rows x data.cols).
*
* Parameters: LinOp with type SUM_ENTRIES
*
* Returns: vector containing the coefficient matrix COEFFS
*/
std::vector<Matrix> get_sum_entries_mat(LinOp &lin) {
assert(lin.type == SUM_ENTRIES);
// assumes all args have the same size
int rows = lin.args[0]->size[0];
int cols = lin.args[0]->size[1];
Matrix coeffs = sparse_ones(1, rows * cols);
coeffs.makeCompressed();
return build_vector(coeffs);
}
/**
* Return the coefficients for TRACE: A single row vector v^T \in R^(n^2)
* with 1 if v_{i} corresponds to a diagonal entry (i.e. i * n + i) and 0
* otherwise.
*
* Parameters: LinOp with type TRACE
*
* Returns: vector containing the coefficient matrix COEFFS
*
*/
std::vector<Matrix> get_trace_mat(LinOp &lin) {
assert(lin.type == TRACE);
int rows = lin.args[0]->size[0];
Matrix coeffs (1, rows * rows);
for (int i = 0; i < rows; i++) {
coeffs.insert(0, i * rows + i) = 1;
}
coeffs.makeCompressed();
return build_vector(coeffs);
}
/**
* Return the coefficients for DIV: a diagonal matrix where each diagonal
* entry is 1 / DIVISOR.
*
* Parameters: linOp with type DIV
*
* Returns: vector containing the coefficient matrix COEFFS
*
*/
std::vector<Matrix> get_div_mat(LinOp &lin) {
assert(lin.type == DIV);
// assumes scalar divisor
double divisor = get_divisor_data(lin);
int n = lin.size[0] * lin.size[1];
Matrix coeffs = sparse_eye(n);
coeffs /= divisor;
coeffs.makeCompressed();
return build_vector(coeffs);
}
/**
* Return the coefficients for INDEX: a N by ROWS*COLS matrix
* where N is the number of total elements in the slice. Element i, j
* is 1 if element j in the vectorized matrix is the i-th element of the
* slice and 0 otherwise.
*
* Parameters: LinOp of type INDEX
*
* Returns: vector containing coefficient matrix COEFFS
*
*/
std::vector<Matrix> get_index_mat(LinOp &lin) {
assert(lin.type == INDEX);
int rows = lin.args[0]->size[0];
int cols = lin.args[0]->size[1];
Matrix coeffs (lin.size[0] * lin.size[1], rows * cols);
/* If slice is empty, return empty matrix */
if (coeffs.rows () == 0 || coeffs.cols() == 0) {
return build_vector(coeffs);
}
std::vector<std::vector<int> > slices = get_slice_data(lin, rows, cols);
/* Row Slice Data */
int row_start = slices[0][0];
int row_end = slices[0][1];
int row_step = slices[0][2];
/* Column Slice Data */
int col_start = slices[1][0];
int col_end = slices[1][1];
int col_step = slices[1][2];
/* Set the index coefficients by looping over the column selection
* first to remain consistent with CVXPY. */
std::vector<Triplet> tripletList;
int col = col_start;
int counter = 0;
while (true) {
if (col < 0 || col >= cols) {
break;
}
int row = row_start;
while (true) {
if (row < 0 || row >= rows) {
break;
}
int row_idx = counter;
int col_idx = col * rows + row;
tripletList.push_back(Triplet(row_idx, col_idx, 1.0));
counter++;
row += row_step;
if ((row_step > 0 && row >= row_end) || (row_step < 0 && row <= row_end)) {
break;
}
}
col += col_step;
if ((col_step > 0 && col >= col_end) || (col_step < 0 && col <= col_end)) {
break;
}
}
coeffs.setFromTriplets(tripletList.begin(), tripletList.end());
coeffs.makeCompressed();
return build_vector(coeffs);
}
/**
* Returns a map from CONSTANT_ID to the data matrix of the corresponding
* CONSTANT type LinOp. The coefficient matrix is the data matrix reshaped
* as a ROWS * COLS by 1 column vector.
* Note the data is treated as sparse regardless of the underlying
* representation.
*
* Parameters: CONSTANT linop LIN
*
* Returns: map from CONSTANT_ID to the coefficient matrix COEFFS for LIN.
*/
std::map<int, Matrix> get_const_coeffs(LinOp &lin) {
assert(lin.has_constant_type());
std::map<int, Matrix> id_to_coeffs;
int id = CONSTANT_ID;
// get coeffs as a column vector
Matrix coeffs = get_constant_data(lin, true);
coeffs.makeCompressed();
id_to_coeffs[id] = coeffs;
return id_to_coeffs;
}
/**
* Return a map from the variable ID to the coefficient matrix for the
* corresponding VARIABLE linOp, which is an identity matrix of total
* linop size x total linop size.
*
* Parameters: VARIABLE Type LinOp LIN
*
* Returns: Map from VARIABLE_ID to coefficient matrix COEFFS for LIN
*
*/
std::map<int, Matrix> get_variable_coeffs(LinOp &lin) {
assert(lin.type == VARIABLE);
std::map<int, Matrix> id_to_coeffs;
int id = get_id_data(lin);
// create a giant identity matrix
int n = lin.size[0] * lin.size[1];
Matrix coeffs = sparse_eye(n);
coeffs.makeCompressed();
id_to_coeffs[id] = coeffs;
return id_to_coeffs;
}
bool solve(Matrix& A, Vector const& b, Vector& x, EigenOption& opt) override
{
INFO("-> solve with %s",
EigenOption::getSolverName(opt.solver_type).c_str());
if (!A.isCompressed()) A.makeCompressed();
_solver.compute(A);
if(_solver.info()!=Eigen::Success) {
ERR("Failed during Eigen linear solver initialization");
return false;
}
x = _solver.solve(b);
if(_solver.info()!=Eigen::Success) {
ERR("Failed during Eigen linear solve");
return false;
}
return true;
}
/**
* Returns the matrix stored in the data field of LIN as a sparse eigen matrix
* If COLUMN is true, the matrix is reshaped into a column vector which
* preserves the columnwise ordering of the elements, equivalent to
* matlab (:) operator.
*
* Note all matrices are returned in a sparse representation to force
* sparse matrix operations in build_matrix.
*
* Params: LinOp LIN with DATA containing a 2d vector representation of a
* matrix. boolean COLUMN
*
* Returns: sparse eigen matrix COEFFS
*
*/
Matrix get_constant_data(LinOp &lin, bool column) {
Matrix coeffs;
if (lin.sparse) {
if (column) {
coeffs = sparse_reshape_to_vec(lin.sparse_data);
} else {
coeffs = lin.sparse_data;
}
} else {
if (column) {
Eigen::Map<Eigen::MatrixXd> column(lin.dense_data.data(),
lin.dense_data.rows() *
lin.dense_data.cols(), 1);
coeffs = column.sparseView();
} else {
coeffs = lin.dense_data.sparseView();
}
}
coeffs.makeCompressed();
return coeffs;
}
virtual Matrix A(Real t) {
Matrix M = grid.matrix();
M.reserve(IntegerVector::Constant(
grid.size(),
TwoToTheDimension::value
));
Real args[Dimension + ControlDimension + 1];
Real plus[Dimension];
// TODO: Fix. Currying explodes in Clang; not sure why...
/*
Function<Dimension + ControlDimension> transitions_t[Dimension];
for(Index i = 0; i < Dimension; ++i) {
transitions_t[i] =
curry<1 + Dimension + ControlDimension>(
transitions[i],
t
)
;
}
*/
args[0] = t;
Index k = 0;
for(auto node : grid) {
// Coordinates
for(int i = 0; i < Dimension; ++i) {
args[i + 1] = node[i];
}
// Control coordinates
for(int i = 0; i < ControlDimension; ++i) {
args[Dimension + i + 1]=(this->control(i))(k);
}
// Get new state
for(int i = 0; i < Dimension; ++i) {
plus[i] =
packAndCall<
Dimension
+ ControlDimension
+ 1
>(
transitions[i],
args
)
;
}
auto data = linearInterpolationData(grid, plus);
Index idxs[Dimension];
for(
std::intmax_t i = 0;
i < TwoToTheDimension::value;
++i
) {
Real factor = 1.;
for(Index j = 0; j < Dimension; ++j) {
if(i & (1 << j)) {
// j-th bit of i is 1
idxs[j] = std::get<0>(data[j]);
factor *= std::get<1>(data[j]);
} else {
// j-th bit of i is 0
idxs[j] = std::get<0>(data[j])
+ 1 ;
factor *= -std::get<1>(data[j])
+ 1.;
}
}
// n-Dimensional
NaryMethodConst<
Index,
RectilinearGrid<Dimension>,
Dimension, Index
> tmp = &RectilinearGrid<Dimension>::index;
Index j = packAndCall<Dimension>(grid, tmp,
idxs);
M.insert(k, j) = factor;
};
++k;
}
M.makeCompressed();
if(Negative) {
return M - grid.identity();
}
return grid.identity() - M;
}
