/// Oil formation volume factor.
/// \param[in] po Array of n oil pressure values.
/// \param[in] rs Array of n gas solution factor values.
/// \param[in] cells Array of n cell indices to be associated with the pressure values.
/// \return Array of n formation volume factor values.
ADB BlackoilPropsAdFromDeck::bOil(const ADB& po,
const ADB& rs,
const Cells& cells) const
{
if (!phase_usage_.phase_used[Oil]) {
OPM_THROW(std::runtime_error, "Cannot call muOil(): oil phase not present.");
}
const int n = cells.size();
assert(po.size() == n);
V b(n);
V dbdp(n);
V dbdr(n);
props_[phase_usage_.phase_pos[Oil]]->b(n, po.value().data(), rs.value().data(),
b.data(), dbdp.data(), dbdr.data());
ADB::M dbdp_diag = spdiag(dbdp);
ADB::M dbdr_diag = spdiag(dbdr);
const int num_blocks = po.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = dbdp_diag * po.derivative()[block] + dbdr_diag * rs.derivative()[block];
}
return ADB::function(b, jacs);
}
ADB PolymerPropsAd::polymerWaterVelocityRatio(const ADB& c) const
{
const int nc = c.size();
V mc(nc);
V dmc(nc);
for (int i = 0; i < nc; ++i) {
double m = 0;
double dm = 0;
polymer_props_.computeMcWithDer(c.value()(i), m, dm);
mc(i) = m;
dmc(i) = dm;
}
ADB::M dmc_diag = spdiag(dmc);
const int num_blocks = c.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = dmc_diag * c.derivative()[block];
}
return ADB::function(std::move(mc), std::move(jacs));
}
/// Gas viscosity.
/// \param[in] pg Array of n gas pressure values.
/// \param[in] rv Array of n vapor oil/gas ratio
/// \param[in] cond Array of n objects, each specifying which phases are present with non-zero saturation in a cell.
/// \param[in] cells Array of n cell indices to be associated with the pressure values.
/// \return Array of n viscosity values.
ADB BlackoilPropsAd::muGas(const ADB& pg,
const ADB& rv,
const std::vector<PhasePresence>& cond,
const Cells& cells) const
{
#if 1
return ADB::constant(muGas(pg.value(), rv.value(),cond,cells), pg.blockPattern());
#else
if (!pu_.phase_used[Gas]) {
OPM_THROW(std::runtime_error, "Cannot call muGas(): gas phase not present.");
}
const int n = cells.size();
assert(pg.value().size() == n);
const int np = props_.numPhases();
Block z = Block::Zero(n, np);
if (pu_.phase_used[Oil]) {
// Faking a z with the right ratio:
// rv = zo/zg
z.col(pu_.phase_pos[Oil]) = rv;
z.col(pu_.phase_pos[Gas]) = V::Ones(n, 1);
}
Block mu(n, np);
Block dmu(n, np);
props_.viscosity(n, pg.value().data(), z.data(), cells.data(), mu.data(), dmu.data());
ADB::M dmu_diag = spdiag(dmu.col(pu_.phase_pos[Gas]));
const int num_blocks = pg.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = dmu_diag * pg.derivative()[block];
}
return ADB::function(mu.col(pu_.phase_pos[Gas]), jacs);
#endif
}
/// Gas viscosity.
/// \param[in] pg Array of n gas pressure values.
/// \param[in] cells Array of n cell indices to be associated with the pressure values.
/// \return Array of n viscosity values.
ADB BlackoilPropsAd::muGas(const ADB& pg,
const Cells& cells) const
{
#if 1
return ADB::constant(muGas(pg.value(), cells), pg.blockPattern());
#else
if (!pu_.phase_used[Gas]) {
THROW("Cannot call muGas(): gas phase not present.");
}
const int n = cells.size();
ASSERT(pg.value().size() == n);
const int np = props_.numPhases();
Block z = Block::Zero(n, np);
Block mu(n, np);
Block dmu(n, np);
props_.viscosity(n, pg.value().data(), z.data(), cells.data(), mu.data(), dmu.data());
ADB::M dmu_diag = spdiag(dmu.col(pu_.phase_pos[Gas]));
const int num_blocks = pg.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = dmu_diag * pg.derivative()[block];
}
return ADB::function(mu.col(pu_.phase_pos[Gas]), jacs);
#endif
}
/// Oil viscosity.
/// \param[in] po Array of n oil pressure values.
/// \param[in] rs Array of n gas solution factor values.
/// \param[in] cells Array of n cell indices to be associated with the pressure values.
/// \return Array of n viscosity values.
ADB BlackoilPropsAd::muOil(const ADB& po,
const ADB& rs,
const Cells& cells) const
{
#if 1
return ADB::constant(muOil(po.value(), rs.value(), cells), po.blockPattern());
#else
if (!pu_.phase_used[Oil]) {
THROW("Cannot call muOil(): oil phase not present.");
}
const int n = cells.size();
ASSERT(po.value().size() == n);
const int np = props_.numPhases();
Block z = Block::Zero(n, np);
if (pu_.phase_used[Gas]) {
// Faking a z with the right ratio:
// rs = zg/zo
z.col(pu_.phase_pos[Oil]) = V::Ones(n, 1);
z.col(pu_.phase_pos[Gas]) = rs.value();
}
Block mu(n, np);
Block dmu(n, np);
props_.viscosity(n, po.value().data(), z.data(), cells.data(), mu.data(), dmu.data());
ADB::M dmu_diag = spdiag(dmu.col(pu_.phase_pos[Oil]));
const int num_blocks = po.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
// For now, we deliberately ignore the derivative with respect to rs,
// since the BlackoilPropertiesInterface class does not evaluate it.
// We would add to the next line: + dmu_drs_diag * rs.derivative()[block]
jacs[block] = dmu_diag * po.derivative()[block];
}
return ADB::function(mu.col(pu_.phase_pos[Oil]), jacs);
#endif
}
/// Oil formation volume factor.
/// \param[in] po Array of n oil pressure values.
/// \param[in] rs Array of n gas solution factor values.
/// \param[in] cond Array of n taxonomies classifying fluid condition.
/// \param[in] cells Array of n cell indices to be associated with the pressure values.
/// \return Array of n formation volume factor values.
ADB BlackoilPropsAd::bOil(const ADB& po,
const ADB& rs,
const std::vector<PhasePresence>& /*cond*/,
const Cells& cells) const
{
if (!pu_.phase_used[Oil]) {
OPM_THROW(std::runtime_error, "Cannot call muOil(): oil phase not present.");
}
const int n = cells.size();
assert(po.value().size() == n);
const int np = props_.numPhases();
Block z = Block::Zero(n, np);
if (pu_.phase_used[Gas]) {
// Faking a z with the right ratio:
// rs = zg/zo
z.col(pu_.phase_pos[Oil]) = V::Ones(n, 1);
z.col(pu_.phase_pos[Gas]) = rs.value();
}
Block matrix(n, np*np);
Block dmatrix(n, np*np);
props_.matrix(n, po.value().data(), z.data(), cells.data(), matrix.data(), dmatrix.data());
const int phase_ind = pu_.phase_pos[Oil];
const int column = phase_ind*np + phase_ind; // Index of our sought diagonal column.
ADB::M db_diag = spdiag(dmatrix.col(column));
const int num_blocks = po.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
// For now, we deliberately ignore the derivative with respect to rs,
// since the BlackoilPropertiesInterface class does not evaluate it.
// We would add to the next line: + db_drs_diag * rs.derivative()[block]
jacs[block] = db_diag * po.derivative()[block];
}
return ADB::function(matrix.col(column), jacs);
}
/// Gas formation volume factor.
/// \param[in] pg Array of n gas pressure values.
/// \param[in] cells Array of n cell indices to be associated with the pressure values.
/// \return Array of n formation volume factor values.
ADB BlackoilPropsAdFromDeck::bGas(const ADB& pg,
const Cells& cells) const
{
if (!phase_usage_.phase_used[Gas]) {
OPM_THROW(std::runtime_error, "Cannot call muGas(): gas phase not present.");
}
const int n = cells.size();
assert(pg.size() == n);
V b(n);
V dbdp(n);
V dbdr(n);
const double* rs = 0;
props_[phase_usage_.phase_pos[Gas]]->b(n, pg.value().data(), rs,
b.data(), dbdp.data(), dbdr.data());
ADB::M dbdp_diag = spdiag(dbdp);
const int num_blocks = pg.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = dbdp_diag * pg.derivative()[block];
}
return ADB::function(b, jacs);
}
/// Gas formation volume factor.
/// \param[in] pg Array of n gas pressure values.
/// \param[in] rv Array of n vapor oil/gas ratio
/// \param[in] cond Array of n objects, each specifying which phases are present with non-zero saturation in a cell.
/// \param[in] cells Array of n cell indices to be associated with the pressure values.
/// \return Array of n formation volume factor values.
ADB BlackoilPropsAd::bGas(const ADB& pg,
const ADB& rv,
const std::vector<PhasePresence>& /*cond*/,
const Cells& cells) const
{
if (!pu_.phase_used[Gas]) {
OPM_THROW(std::runtime_error, "Cannot call muGas(): gas phase not present.");
}
const int n = cells.size();
assert(pg.value().size() == n);
const int np = props_.numPhases();
Block z = Block::Zero(n, np);
if (pu_.phase_used[Oil]) {
// Faking a z with the right ratio:
// rv = zo/zg
z.col(pu_.phase_pos[Oil]) = rv.value();
z.col(pu_.phase_pos[Gas]) = V::Ones(n, 1);
}
Block matrix(n, np*np);
Block dmatrix(n, np*np);
props_.matrix(n, pg.value().data(), z.data(), cells.data(), matrix.data(), dmatrix.data());
const int phase_ind = pu_.phase_pos[Gas];
const int column = phase_ind*np + phase_ind; // Index of our sought diagonal column.
ADB::M db_diag = spdiag(dmatrix.col(column));
const int num_blocks = pg.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = db_diag * pg.derivative()[block];
}
return ADB::function(matrix.col(column), jacs);
}
/// Water viscosity.
/// \param[in] pw Array of n water pressure values.
/// \param[in] cells Array of n cell indices to be associated with the pressure values.
/// \return Array of n viscosity values.
ADB BlackoilPropsAd::muWat(const ADB& pw,
const Cells& cells) const
{
#if 1
return ADB::constant(muWat(pw.value(), cells), pw.blockPattern());
#else
if (!pu_.phase_used[Water]) {
OPM_THROW(std::runtime_error, "Cannot call muWat(): water phase not present.");
}
const int n = cells.size();
assert(pw.value().size() == n);
const int np = props_.numPhases();
Block z = Block::Zero(n, np);
Block mu(n, np);
Block dmu(n, np);
props_.viscosity(n, pw.value().data(), z.data(), cells.data(), mu.data(), dmu.data());
ADB::M dmu_diag = spdiag(dmu.col(pu_.phase_pos[Water]));
const int num_blocks = pw.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = dmu_diag * pw.derivative()[block];
}
return ADB::function(mu.col(pu_.phase_pos[Water]), jacs);
#endif
}
std::vector<ADB> BlackoilPropsAd::capPress(const ADB& sw,
const ADB& so,
const ADB& sg,
const Cells& cells) const
{
const int numCells = cells.size();
const int numActivePhases = numPhases();
const int numBlocks = so.numBlocks();
Block activeSat(numCells, numActivePhases);
if (pu_.phase_used[Water]) {
assert(sw.value().size() == numCells);
activeSat.col(pu_.phase_pos[Water]) = sw.value();
}
if (pu_.phase_used[Oil]) {
assert(so.value().size() == numCells);
activeSat.col(pu_.phase_pos[Oil]) = so.value();
} else {
OPM_THROW(std::runtime_error, "BlackoilPropsAdFromDeck::relperm() assumes oil phase is active.");
}
if (pu_.phase_used[Gas]) {
assert(sg.value().size() == numCells);
activeSat.col(pu_.phase_pos[Gas]) = sg.value();
}
Block pc(numCells, numActivePhases);
Block dpc(numCells, numActivePhases*numActivePhases);
props_.capPress(numCells, activeSat.data(), cells.data(), pc.data(), dpc.data());
std::vector<ADB> adbCapPressures;
adbCapPressures.reserve(3);
const ADB* s[3] = { &sw, &so, &sg };
for (int phase1 = 0; phase1 < 3; ++phase1) {
if (pu_.phase_used[phase1]) {
const int phase1_pos = pu_.phase_pos[phase1];
std::vector<ADB::M> jacs(numBlocks);
for (int block = 0; block < numBlocks; ++block) {
jacs[block] = ADB::M(numCells, s[phase1]->derivative()[block].cols());
}
for (int phase2 = 0; phase2 < 3; ++phase2) {
if (!pu_.phase_used[phase2])
continue;
const int phase2_pos = pu_.phase_pos[phase2];
// Assemble dpc1/ds2.
const int column = phase1_pos + numActivePhases*phase2_pos; // Recall: Fortran ordering from props_.relperm()
ADB::M dpc1_ds2_diag = spdiag(dpc.col(column));
for (int block = 0; block < numBlocks; ++block) {
jacs[block] += dpc1_ds2_diag * s[phase2]->derivative()[block];
}
}
adbCapPressures.emplace_back(ADB::function(pc.col(phase1_pos), jacs));
} else {
adbCapPressures.emplace_back(ADB::null());
}
}
return adbCapPressures;
}
/// Relative permeabilities for all phases.
/// \param[in] sw Array of n water saturation values.
/// \param[in] so Array of n oil saturation values.
/// \param[in] sg Array of n gas saturation values.
/// \param[in] cells Array of n cell indices to be associated with the saturation values.
/// \return An std::vector with 3 elements, each an array of n relperm values,
/// containing krw, kro, krg. Use PhaseIndex for indexing into the result.
std::vector<ADB> BlackoilPropsAd::relperm(const ADB& sw,
const ADB& so,
const ADB& sg,
const Cells& cells) const
{
const int n = cells.size();
const int np = props_.numPhases();
Block s_all(n, np);
if (pu_.phase_used[Water]) {
assert(sw.value().size() == n);
s_all.col(pu_.phase_pos[Water]) = sw.value();
}
if (pu_.phase_used[Oil]) {
assert(so.value().size() == n);
s_all.col(pu_.phase_pos[Oil]) = so.value();
} else {
OPM_THROW(std::runtime_error, "BlackoilPropsAd::relperm() assumes oil phase is active.");
}
if (pu_.phase_used[Gas]) {
assert(sg.value().size() == n);
s_all.col(pu_.phase_pos[Gas]) = sg.value();
}
Block kr(n, np);
Block dkr(n, np*np);
props_.relperm(n, s_all.data(), cells.data(), kr.data(), dkr.data());
const int num_blocks = so.numBlocks();
std::vector<ADB> relperms;
relperms.reserve(3);
typedef const ADB* ADBPtr;
ADBPtr s[3] = { &sw, &so, &sg };
for (int phase1 = 0; phase1 < 3; ++phase1) {
if (pu_.phase_used[phase1]) {
const int phase1_pos = pu_.phase_pos[phase1];
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = ADB::M(n, s[phase1]->derivative()[block].cols());
}
for (int phase2 = 0; phase2 < 3; ++phase2) {
if (!pu_.phase_used[phase2]) {
continue;
}
const int phase2_pos = pu_.phase_pos[phase2];
// Assemble dkr1/ds2.
const int column = phase1_pos + np*phase2_pos; // Recall: Fortran ordering from props_.relperm()
ADB::M dkr1_ds2_diag = spdiag(dkr.col(column));
for (int block = 0; block < num_blocks; ++block) {
jacs[block] += dkr1_ds2_diag * s[phase2]->derivative()[block];
}
}
relperms.emplace_back(ADB::function(kr.col(phase1_pos), jacs));
} else {
relperms.emplace_back(ADB::null());
}
}
return relperms;
}
sp_mat build_hallway_P(const uint N,
const double p_stick,
const int action){
// Build the hallway transition matrix associated with
// and action
assert(action == -1 or action == 1);
sp_mat P = p_stick * speye(N,N)
+ spdiag((1-p_stick) * ones<vec>(N-1),action);
if(action < 0)
P(0,N-1) = (1 - p_stick);
else
P(N-1,0) = (1 - p_stick);
return P;
}
/// Bubble point curve for Rs as function of oil pressure.
/// \param[in] po Array of n oil pressure values.
/// \param[in] cells Array of n cell indices to be associated with the pressure values.
/// \return Array of n bubble point values for Rs.
ADB BlackoilPropsAdFromDeck::rsMax(const ADB& po,
const Cells& cells) const
{
if (!phase_usage_.phase_used[Oil]) {
OPM_THROW(std::runtime_error, "Cannot call rsMax(): oil phase not present.");
}
const int n = cells.size();
assert(po.size() == n);
V rbub(n);
V drbubdp(n);
props_[Oil]->rbub(n, po.value().data(), rbub.data(), drbubdp.data());
ADB::M drbubdp_diag = spdiag(drbubdp);
const int num_blocks = po.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = drbubdp_diag * po.derivative()[block];
}
return ADB::function(rbub, jacs);
}
ADB PolymerPropsAd::effectiveInvWaterVisc(const ADB& c,
const double* visc) const
{
const int nc = c.size();
V inv_mu_w_eff(nc);
V dinv_mu_w_eff(nc);
for (int i = 0; i < nc; ++i) {
double im = 0, dim = 0;
polymer_props_.effectiveInvViscWithDer(c.value()(i), visc, im, dim);
inv_mu_w_eff(i) = im;
dinv_mu_w_eff(i) = dim;
}
ADB::M dim_diag = spdiag(dinv_mu_w_eff);
const int num_blocks = c.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = dim_diag * c.derivative()[block];
}
return ADB::function(std::move(inv_mu_w_eff), std::move(jacs));
}
/// Water viscosity.
/// \param[in] pw Array of n water pressure values.
/// \param[in] cells Array of n cell indices to be associated with the pressure values.
/// \return Array of n viscosity values.
ADB BlackoilPropsAdFromDeck::muWat(const ADB& pw,
const Cells& cells) const
{
if (!phase_usage_.phase_used[Water]) {
OPM_THROW(std::runtime_error, "Cannot call muWat(): water phase not present.");
}
const int n = cells.size();
assert(pw.size() == n);
V mu(n);
V dmudp(n);
V dmudr(n);
const double* rs = 0;
props_[phase_usage_.phase_pos[Water]]->mu(n, pw.value().data(), rs,
mu.data(), dmudp.data(), dmudr.data());
ADB::M dmudp_diag = spdiag(dmudp);
const int num_blocks = pw.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = dmudp_diag * pw.derivative()[block];
}
return ADB::function(mu, jacs);
}
/// Water formation volume factor.
/// \param[in] pw Array of n water pressure values.
/// \param[in] cells Array of n cell indices to be associated with the pressure values.
/// \return Array of n formation volume factor values.
ADB BlackoilPropsAd::bWat(const ADB& pw,
const Cells& cells) const
{
if (!pu_.phase_used[Water]) {
OPM_THROW(std::runtime_error, "Cannot call muWat(): water phase not present.");
}
const int n = cells.size();
assert(pw.value().size() == n);
const int np = props_.numPhases();
Block z = Block::Zero(n, np);
Block matrix(n, np*np);
Block dmatrix(n, np*np);
props_.matrix(n, pw.value().data(), z.data(), cells.data(), matrix.data(), dmatrix.data());
const int phase_ind = pu_.phase_pos[Water];
const int column = phase_ind*np + phase_ind; // Index of our sought diagonal column.
ADB::M db_diag = spdiag(dmatrix.col(column));
const int num_blocks = pw.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = db_diag * pw.derivative()[block];
}
return ADB::function(matrix.col(column), jacs);
}
/// Gas formation volume factor.
/// \param[in] pg Array of n gas pressure values.
/// \param[in] cells Array of n cell indices to be associated with the pressure values.
/// \return Array of n formation volume factor values.
ADB BlackoilPropsAd::bGas(const ADB& pg,
const Cells& cells) const
{
if (!pu_.phase_used[Gas]) {
THROW("Cannot call muGas(): gas phase not present.");
}
const int n = cells.size();
ASSERT(pg.value().size() == n);
const int np = props_.numPhases();
Block z = Block::Zero(n, np);
Block matrix(n, np*np);
Block dmatrix(n, np*np);
props_.matrix(n, pg.value().data(), z.data(), cells.data(), matrix.data(), dmatrix.data());
const int phase_ind = pu_.phase_pos[Gas];
const int column = phase_ind*np + phase_ind; // Index of our sought diagonal column.
ADB::M db_diag = spdiag(dmatrix.col(column));
const int num_blocks = pg.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = db_diag * pg.derivative()[block];
}
return ADB::function(matrix.col(column), jacs);
}
ADB PolymerPropsAd::adsorption(const ADB& c, const ADB& cmax_cells) const
{
const int nc = c.value().size();
V ads(nc);
V dads(nc);
for (int i = 0; i < nc; ++i) {
double c_ads = 0;
double dc_ads = 0;
polymer_props_.adsorptionWithDer(c.value()(i), cmax_cells.value()(i), c_ads, dc_ads);
ads(i) = c_ads;
dads(i) = dc_ads;
}
ADB::M dads_diag = spdiag(dads);
int num_blocks = c.numBlocks();
std::vector<ADB::M> jacs(num_blocks);
for (int block = 0; block < num_blocks; ++block) {
jacs[block] = dads_diag * c.derivative()[block];
}
return ADB::function(std::move(ads), std::move(jacs));
}
SolverResult ProjectiveSolver::solve(const PLCP & plcp,
vec & x,
vec & y,
vec & w) const{
sp_mat P = plcp.P;
sp_mat U = plcp.U;
vec q = plcp.q;
uint N = P.n_rows;
uint K = P.n_cols;
assert(size(P.t()) == size(U));
bvec free_vars = plcp.free_vars;
uvec bound_idx = find(0 == free_vars);
uvec free_idx = find(1 == free_vars);
assert(N == bound_idx.n_elem + free_idx.n_elem);
uint NB = bound_idx.n_elem; // number of bound vars
uint NF = free_idx.n_elem; // number of free vars
assert(NF == accu(conv_to<uvec>::from(free_vars)));
assert(N == x.n_elem);
assert(N == y.n_elem);
assert(K == w.n_elem);
assert(ALMOST_ZERO > norm(y(free_idx)));
if(verbose)
cout << "Free variables: \t" << NF << endl
<< "Non-neg variables:\t" << NB << endl;
sp_mat J = join_vert(sp_mat(NF,NB),speye(NB,NB));
assert(size(N,NB) == size(J));
assert(PRETTY_SMALL > norm(y(free_idx)));
if(verbose)
cout << "Forming pre-computed products..." << endl;
mat PtP = mat(P.t() * P);
assert(size(K,K) == size(PtP));
mat PtPU = PtP*U;
assert(size(K,N) == size(PtPU));
mat Pt_PtPU = P.t() - PtPU;
assert(size(K,N) == size(Pt_PtPU));
mat PtPUP = PtPU * P;
assert(size(K,K) == size(PtPUP));
vec Ptq = P.t() * q;
if(verbose)
cout << "Done..." << endl;
double sigma = initial_sigma;
uint iter;
double mean_comp;
for(iter = 0; iter < max_iter; iter++){
if(verbose or iter_verbose)
cout << "---Iteration " << iter << "---" << endl;
// Mean complementarity
vec s = y(bound_idx);
vec b = x(bound_idx);
mean_comp = dot(s,b) / (double) NB;
if(mean_comp < comp_thresh)
break;
// Generate reduced Netwon system
mat C = s+b;
vec g = sigma * mean_comp - s % b;
assert(NB == g.n_elem);
// NB: A,G,and h have opposite sign from python version
mat A = Pt_PtPU * J * spdiag(1.0 / C);
assert(size(K,NB) == size(A));
mat G = PtPUP + (A * spdiag(s)) * J.t() * P;
assert(size(K,K) == size(G));
vec Ptr = P.t() * (J * s) - PtPU*x - Ptq;
vec h = Ptr + A*g;
assert(K == h.n_elem);
// Options don't make much difference
vec dw = arma::solve(G+1e-15*eye(K,K),h,
solve_opts::equilibrate);
assert(K == dw.n_elem);
// Recover dy
vec Pdw = P * dw;
vec JtPdw = J.t() * Pdw;
assert(NB == JtPdw.n_elem);
vec ds = (g - s % JtPdw) / C;
assert(NB == ds.n_elem);
// Recover dx
vec dx = (J * ds) + (Pdw);
assert(N == dx.n_elem);
double steplen = steplen_heuristic(x(bound_idx),s,dx(bound_idx),ds,0.9);
sigma = sigma_heuristic(sigma,steplen);
x += steplen * dx;
s += steplen * ds;
y(bound_idx) = s;
w += steplen * dw;
if(verbose){
cout <<"\tMean complementarity: " << mean_comp
<<"\n\tStep length: " << steplen
<<"\n\tCentering sigma: " << sigma << endl;
}
}
if(verbose){
cout << "Finished"
<<"\n\t Final mean complementarity: " << mean_comp << endl;
}
return SolverResult(x,y,iter);
}
SolverResult KojimaSolver::solve(const LCP & lcp,
vec & x,
vec & y) const{
superlu_opts opts;
opts.equilibrate = true;
opts.permutation = superlu_opts::COLAMD;
opts.refine = superlu_opts::REF_SINGLE;
vec q = lcp.q;
sp_mat M = lcp.M + regularizer * speye(size(lcp.M));
uint N = q.n_elem;
assert(N == x.n_elem);
assert(N == y.n_elem);
// Figure out what is free (-inf,inf)
// and what is bound to be non-negative [0,inf)
bvec free_vars = lcp.free_vars;
uvec bound_idx = find(0 == free_vars);
uvec free_idx = find(1 == free_vars);
assert(N == bound_idx.n_elem + free_idx.n_elem);
uint NB = bound_idx.n_elem; // number of bound vars
uint NF = free_idx.n_elem; // number of free vars
/*
In what follows, the primal variables x are partitioned into
free variables and bound variables x = [f;b]'
Likewise, the dual variables are partitioned into y = [0,s]'
*/
/* The Newton system:
[M_ff M_fb 0][df] [M_f x + q_f]
[M_bf M_bb -I][db] + [M_b x + q_b - s]
[0 S B][dv] [u1 + VBe]
Where "M_ff" is the free-free block
Overwrite the S,B diagonals every iteration*/
// Split M matrix into blocks based on free and bound indicies
block_sp_mat M_part = sp_partition(M,free_idx,bound_idx);
sp_mat M_recon = block_mat(M_part);
assert(PRETTY_SMALL > norm(M_recon - M));
vec qf = q(free_idx);
vec qb = q(bound_idx);
vec b = x(bound_idx);
vec f = x(free_idx);
vec s = y(bound_idx);
// Build the Newton matrix
vector<vector<sp_mat>> block_G;
block_G.push_back(block_sp_vec{sp_mat(),sp_mat(),sp_mat(NB,NB)});
block_G.push_back(block_sp_vec{-M_part[0][0],-M_part[0][1],sp_mat()});
block_G.push_back(block_sp_vec{-M_part[1][0],-M_part[1][1],speye(NB,NB)});
// Start iteration
double mean_comp, steplen;
double sigma = initial_sigma;
uint iter;
for(iter = 0; iter < max_iter; iter++){
if(verbose or iter_verbose)
cout << "---Iteration " << iter << "---" << endl;
assert(all(0 == y(free_idx)));
// Mean complementarity
mean_comp = dot(b,s) / (double) NB;
if(mean_comp < comp_thresh)
break;
block_G[0][1] = spdiag(s);
block_G[0][2] = spdiag(b);
sp_mat G = block_mat(block_G);
assert(size(N + NB,N + NB) == size(G));
// Form RHS from residual and complementarity
vec h = vec(N + NB);
vec res_f = M_part[0][0]*f + M_part[0][1]*b + qf;
vec res_b = M_part[1][0]*f + M_part[1][1]*b + qb - s;
h.head(NB) = sigma * mean_comp - b % s;
h.subvec(NB,size(res_f)) = res_f;
h.tail(NB) = res_b;
//Archiver arch;
//arch.add_sp_mat("G",G);
//arch.add_vec("h",h);
//arch.write("test.sys");
// Solve and extract directions
vec dir = spsolve(G,h,"superlu",opts);
assert((N+NB) == dir.n_elem);
vec df = dir.head(NF);
vec db = dir.subvec(NF,N-1);
assert(NB == db.n_elem);
vec ds = dir.tail(NB);
vec dir_recon = join_vert(df,join_vert(db,ds));
assert(PRETTY_SMALL > norm(dir_recon-dir));
steplen = steplen_heuristic(b,s,db,ds,0.9);
sigma = sigma_heuristic(sigma,steplen);
f += steplen * df;
b += steplen * db;
s += steplen * ds;
if(verbose){
double res = norm(join_vert(res_f,res_b));
cout <<"\t Mean complementarity: " << mean_comp
<<"\n\t Residual norm: " << res
<<"\n\t |df|: " << norm(df)
<<"\n\t |db|: " << norm(db)
<<"\n\t |ds|: " << norm(ds)
<<"\n\t Step length: " << steplen
<<"\n\t Centering sigma: " << sigma << endl;
}
}
if(verbose){
cout << "Finished"
<<"\n\t Final mean complementarity: " << mean_comp << endl;
}
x(free_idx) = f;
x(bound_idx) = b;
y(free_idx).fill(0);
y(bound_idx) = s;
return SolverResult(x,y,iter);
}
