use std::rc::Rc;
use pounce_common::types::Number;
use crate::backsolver::SensBacksolver;
use crate::solver::SolverError;
const TAU: Number = 0.9995;
#[derive(Debug, Clone, PartialEq)]
pub struct CorrectorReport {
pub iterations: usize,
pub residual: Number,
pub initial_residual: Number,
pub converged: bool,
pub released: usize,
pub pinned: usize,
pub stationarity: Number,
pub feasibility: Number,
pub complementarity: Number,
}
impl CorrectorReport {
pub fn improved(&self) -> bool {
self.residual < self.initial_residual
}
}
fn residual_norm(v: &[Number]) -> Number {
let mut worst = 0.0_f64;
for &b in v {
if !b.is_finite() {
return Number::INFINITY;
}
worst = worst.max(b.abs());
}
worst
}
pub(crate) fn residual_at(
bs: &crate::algorithm_backsolver::PdSensBacksolver,
flat: &[Number],
pin_rows: &[usize],
deltas: &[Number],
mu: Number,
out: &mut [Number],
) -> Result<(), SolverError> {
let scaled: Vec<Number> = match bs.natural_units_factor() {
None => flat.to_vec(),
Some(f) => flat
.iter()
.zip(f)
.map(|(&v, &s)| if s == 0.0 { v } else { v / s })
.collect(),
};
let iv = bs
.pack_public(&scaled)
.map_err(|_| SolverError::SensComputationFailed("corrector: pack failed".into()))?;
let (data, cq, _) = bs.activity_handles();
data.borrow_mut().set_trial(iv.freeze());
let off = bs.offsets_public();
let cqb = cq.borrow();
let blocks: [(usize, std::rc::Rc<dyn pounce_linalg::Vector>, Number); 8] = [
(0, cqb.trial_grad_lag_x(), 0.0),
(1, cqb.trial_grad_lag_s(), 0.0),
(2, cqb.trial_c(), 0.0),
(3, cqb.trial_d_minus_s(), 0.0),
(4, cqb.trial_compl_x_l(), mu),
(5, cqb.trial_compl_x_u(), mu),
(6, cqb.trial_compl_s_l(), mu),
(7, cqb.trial_compl_s_u(), mu),
];
for (i, v, shift) in blocks {
let vals = crate::vec_util::dense_to_vec(&*v);
let (a, b) = (off[i], off[i + 1]);
if vals.len() != b - a {
return Err(SolverError::SensComputationFailed(format!(
"corrector: block {i} is {} long, expected {}",
vals.len(),
b - a
)));
}
for (o, val) in out[a..b].iter_mut().zip(vals) {
*o = val - shift;
}
}
drop(cqb);
let (yc_a, yc_b) = (off[2], off[3]);
for (&r, &d) in pin_rows.iter().zip(deltas) {
if r < yc_a || r >= yc_b {
return Err(SolverError::SensComputationFailed(format!(
"corrector: pin row {r} is outside the equality block"
)));
}
out[r] -= d;
}
Ok(())
}
fn fraction_to_boundary(val: &[Number], dir: &[Number], skip: &[usize]) -> Number {
let mut a = 1.0;
for (k, (&v, &d)) in val.iter().zip(dir).enumerate() {
if d < 0.0 && !skip.contains(&k) {
let lim = -TAU * v / d;
if lim < a {
a = lim;
}
}
}
a.max(0.0)
}
fn slacks_and_directions(
rows: &[crate::backsolver::BoundRow],
x: &[Number],
dx: &[Number],
lo: &[Number],
hi: &[Number],
lower: bool,
) -> (Vec<Number>, Vec<Number>) {
let mut s = Vec::new();
let mut ds = Vec::new();
for b in rows.iter().filter(|b| b.lower == lower) {
let i = b.var_row;
if lower {
s.push(x[i] - lo[i]);
ds.push(dx[i]);
} else {
s.push(hi[i] - x[i]);
ds.push(-dx[i]);
}
}
(s, ds)
}
fn released_rows(
rows: &[crate::backsolver::BoundRow],
base: &[Number],
end: &[Number],
lo: &[Number],
hi: &[Number],
) -> Vec<usize> {
let mut out = Vec::new();
for b in rows {
let i = b.var_row;
let (s_base, s_end) = if b.lower {
(base[i] - lo[i], end[i] - lo[i])
} else {
(hi[i] - base[i], hi[i] - end[i])
};
let z_base = base[b.row];
if s_base <= 0.0 || z_base / s_base <= 1.0 {
continue; }
if s_end > 10.0 * s_base && s_end > 1e-9 * (1.0 + base[i].abs()) {
out.push(b.row);
}
}
out
}
fn pinned_rows(
rows: &[crate::backsolver::BoundRow],
base: &[Number],
end: &[Number],
lo: &[Number],
hi: &[Number],
mu: Number,
) -> Vec<(usize, Number)> {
let mut out = Vec::new();
for b in rows {
let i = b.var_row;
let (s_base, s_end) = if b.lower {
(base[i] - lo[i], end[i] - lo[i])
} else {
(hi[i] - base[i], hi[i] - end[i])
};
let z_base = base[b.row];
if s_base <= 0.0 || z_base / s_base > 1.0 {
continue; }
if s_end > 0.0 && s_end < 0.1 * s_base && s_end < 1e-6 * (1.0 + base[i].abs()) {
out.push((i, mu / (s_end * s_end)));
}
}
out
}
fn clamp_multipliers(
rows: &[crate::backsolver::BoundRow],
iterate: &mut [Number],
lo: &[Number],
hi: &[Number],
mu: Number,
) {
const KAPPA_SIGMA: Number = 1e10;
for b in rows {
let i = b.var_row;
let s = if b.lower {
iterate[i] - lo[i]
} else {
hi[i] - iterate[i]
};
if s <= 0.0 || !s.is_finite() {
continue;
}
let z = &mut iterate[b.row];
if *z <= 0.0 {
continue; }
*z = z.clamp(mu / (KAPPA_SIGMA * s), KAPPA_SIGMA * mu / s);
}
}
#[allow(clippy::too_many_arguments)]
pub(crate) fn run(
bs: &crate::algorithm_backsolver::PdSensBacksolver,
base_scaled: &[Number],
start: &[Number],
pin_rows: &[usize],
deltas: &[Number],
lo: &[Number],
hi: &[Number],
mu: Number,
max_iter: usize,
exact_hessian: bool,
) -> Result<(Vec<Number>, CorrectorReport), SolverError> {
let dim = bs.dim();
let off = bs.offsets_public();
let n_x = bs.block_dims()[0];
let n_p = n_x + bs.block_dims()[1];
if lo.len() != n_p || hi.len() != n_p {
return Err(SolverError::SensComputationFailed(format!(
"corrector: the box spans {} entries, but the primal prefix (x then s) \
is {n_p}. The bound rows carry primal KKT rows, so the box must cover \
both blocks.",
lo.len()
)));
}
let rows = bs
.bound_rows()
.ok_or_else(|| SolverError::SensComputationFailed("corrector: no bound rows".into()))?
.to_vec();
let base: Vec<Number> = match bs.natural_units_factor() {
None => base_scaled.to_vec(),
Some(f) => base_scaled.iter().zip(f).map(|(&v, &s)| v * s).collect(),
};
let base = &base[..];
let mut iterate: Vec<Number> = base.iter().zip(start).map(|(&b, &s)| b + s).collect();
for b in &rows {
let i = b.var_row;
let margin = 1e-10 * (1.0 + base[i].abs());
if b.lower {
iterate[i] = iterate[i].max(lo[i] + margin);
} else {
iterate[i] = iterate[i].min(hi[i] - margin);
}
}
for z in iterate[off[4]..off[8]].iter_mut() {
*z = z.max(1e-12);
}
clamp_multipliers(&rows, &mut iterate, lo, hi, mu);
let released = released_rows(&rows, base, &iterate, lo, hi);
let pinned = pinned_rows(&rows, base, &iterate, lo, hi, mu);
for &r in &released {
iterate[r] = 0.0;
}
let (data_h, cq_h, _) = bs.activity_handles();
let predicted = bs.pack_natural(&iterate).ok_or_else(|| {
SolverError::SensComputationFailed("corrector: packing the predicted point failed".into())
})?;
struct RestoreCurr<'a> {
data: &'a pounce_algorithm::ipopt_data::IpoptDataHandle,
saved: Option<pounce_algorithm::iterates_vector::IteratesVector>,
saved_w: Option<Rc<dyn pounce_linalg::SymMatrix>>,
restore_w: bool,
}
impl Drop for RestoreCurr<'_> {
fn drop(&mut self) {
if let Some(c) = self.saved.take() {
self.data.borrow_mut().set_curr(c);
}
if self.restore_w {
self.data.borrow_mut().w = self.saved_w.take();
}
}
}
let mut restore = RestoreCurr {
data: data_h,
saved: data_h.borrow().curr.clone(),
saved_w: None,
restore_w: false,
};
data_h.borrow_mut().set_curr(predicted);
if exact_hessian {
restore.saved_w = data_h.borrow().w.clone();
restore.restore_w = true;
let w = cq_h.borrow().curr_exact_hessian();
data_h.borrow_mut().w = Some(w);
}
let _restore = restore;
let op = bs.corrector_sigma(&pinned).ok_or_else(|| {
SolverError::SensComputationFailed("corrector: operator diagonal unavailable".into())
})?;
let solve = |rhs: &[Number], lhs: &mut [Number]| -> bool {
bs.solve_released_prebuilt(
&released,
Rc::clone(&op.sigma_x),
Some(Rc::clone(&op.sigma_s)),
Some((&op.ratio_x, &op.ratio_s)),
rhs,
lhs,
false,
)
};
let clear = |v: &mut [Number]| {
for &r in &released {
v[r] = 0.0;
}
};
let mut resid = vec![0.0; dim];
residual_at(bs, &iterate, pin_rows, deltas, mu, &mut resid)?;
clear(&mut resid);
let norm = residual_norm;
let initial_residual = norm(&resid);
if !initial_residual.is_finite() {
return Err(SolverError::SensComputationFailed(
"corrector: the barrier residual is not finite at the point the \
iterations start from, so the predicted point is outside the \
domain of the model's functions (an unbounded variable driven \
into a log, a sqrt or a reciprocal, say). Bound the variable or \
take a smaller perturbation."
.into(),
));
}
let mut best = iterate.clone();
let mut best_residual = initial_residual;
let mut iterations = 0usize;
let mut converged = false;
let released_dual: Vec<usize> = released.iter().map(|&r| r - off[4]).collect();
let mut rhs = vec![0.0; dim];
let mut dir = vec![0.0; dim];
while iterations < max_iter {
match bs.scaled_rhs_factor() {
None => {
for (r, s) in rhs.iter_mut().zip(&resid) {
*r = -s;
}
}
Some(e) => {
for ((r, s), &ev) in rhs.iter_mut().zip(&resid).zip(e) {
*r = if ev == 0.0 { -s } else { -s / ev };
}
}
}
if !solve(&rhs, &mut dir) {
return Err(SolverError::BacksolveFailed);
}
iterations += 1;
let (sl, dsl) = slacks_and_directions(&rows, &iterate[..n_p], &dir[..n_p], lo, hi, true);
let (su, dsu) = slacks_and_directions(&rows, &iterate[..n_p], &dir[..n_p], lo, hi, false);
let alpha_p =
fraction_to_boundary(&sl, &dsl, &[]).min(fraction_to_boundary(&su, &dsu, &[]));
let alpha_d = fraction_to_boundary(
&iterate[off[4]..off[8]],
&dir[off[4]..off[8]],
&released_dual,
);
for i in 0..off[4] {
iterate[i] += alpha_p * dir[i];
}
for i in off[4]..off[8] {
iterate[i] = (iterate[i] + alpha_d * dir[i]).max(1e-14);
}
for &r in &released {
iterate[r] = 0.0;
}
clamp_multipliers(&rows, &mut iterate, lo, hi, mu);
residual_at(bs, &iterate, pin_rows, deltas, mu, &mut resid)?;
clear(&mut resid);
let now = norm(&resid);
if now < best_residual {
best_residual = now;
best.copy_from_slice(&iterate);
} else {
converged = true;
break;
}
}
residual_at(bs, &best, pin_rows, deltas, mu, &mut resid)?;
clear(&mut resid);
let part = |a: usize, b: usize| norm(&resid[off[a]..off[b]]);
let (stationarity, feasibility, complementarity) = (part(0, 2), part(2, 4), part(4, 8));
let step: Vec<Number> = best.iter().zip(base).map(|(&v, &b)| v - b).collect();
if !step.iter().all(|v| v.is_finite()) {
return Err(SolverError::SensComputationFailed(
"corrector: the corrected step is not finite. The predicted \
point is outside the domain of the model's functions (an \
unbounded variable driven into a log, a sqrt or a reciprocal, \
say). Bound the variable or take a smaller perturbation."
.into(),
));
}
Ok((
step,
CorrectorReport {
iterations,
residual: best_residual,
initial_residual,
converged,
released: released.len(),
pinned: pinned.len(),
stationarity,
feasibility,
complementarity,
},
))
}
#[cfg(test)]
mod tests {
use super::*;
use crate::backsolver::BoundRow;
const MU: Number = 1e-8;
#[test]
fn a_non_finite_entry_norms_to_infinity_rather_than_being_swallowed() {
assert_eq!(residual_norm(&[Number::NAN; 4]), Number::INFINITY);
assert_eq!(
residual_norm(&[1e-3, Number::NAN, 2e-3]),
Number::INFINITY,
"one NaN among finite entries is still no residual"
);
assert_eq!(residual_norm(&[Number::INFINITY, 0.0]), Number::INFINITY);
assert_eq!(residual_norm(&[Number::NEG_INFINITY]), Number::INFINITY);
assert_eq!(residual_norm(&[]), 0.0);
assert_eq!(residual_norm(&[-3.0, 1.0, 2.0]), 3.0);
let beats_the_best_seen = residual_norm(&[Number::NAN]) < 1e-30;
assert!(!beats_the_best_seen);
}
fn rows() -> Vec<BoundRow> {
vec![
BoundRow {
row: 2,
var_row: 0,
lower: true,
},
BoundRow {
row: 3,
var_row: 1,
lower: false,
},
]
}
fn iterate(z0: Number, z1: Number) -> Vec<Number> {
vec![1.0, 9.0, z0, z1]
}
const LO: [Number; 2] = [0.0, 0.0];
const HI: [Number; 2] = [10.0, 10.0];
#[test]
fn a_multiplier_inside_the_band_is_left_alone() {
let mut it = iterate(1.0, 2.0);
clamp_multipliers(&rows(), &mut it, &LO, &HI, MU);
assert_eq!((it[2], it[3]), (1.0, 2.0));
}
#[test]
fn a_multiplier_above_the_band_comes_down_to_it() {
let ceiling = 1e10 * MU;
let mut it = iterate(1e6, 1e6);
clamp_multipliers(&rows(), &mut it, &LO, &HI, MU);
assert_eq!(
(it[2], it[3]),
(ceiling, ceiling),
"both should land on the ceiling {ceiling}",
);
}
#[test]
fn a_multiplier_below_the_band_comes_up_to_it() {
let floor = MU / 1e10;
let mut it = iterate(1e-30, 1e-30);
clamp_multipliers(&rows(), &mut it, &LO, &HI, MU);
assert_eq!((it[2], it[3]), (floor, floor));
}
#[test]
fn the_band_moves_with_the_slack() {
let mut it = vec![0.01, 9.99, 1e6, 1e6];
clamp_multipliers(&rows(), &mut it, &LO, &HI, MU);
let ceiling = 1e10 * MU / 0.01;
assert!(
(it[2] - ceiling).abs() < 1e-9 * ceiling && (it[3] - ceiling).abs() < 1e-9 * ceiling,
"want {ceiling} on both, got {} and {}",
it[2],
it[3],
);
}
#[test]
fn a_released_multiplier_stays_at_zero() {
let mut it = iterate(0.0, -1.0);
clamp_multipliers(&rows(), &mut it, &LO, &HI, MU);
assert_eq!((it[2], it[3]), (0.0, -1.0));
}
#[test]
fn a_coordinate_outside_its_bound_is_skipped() {
let mut it = vec![-1.0, 11.0, 1e6, 1e6];
clamp_multipliers(&rows(), &mut it, &LO, &HI, MU);
assert_eq!((it[2], it[3]), (1e6, 1e6));
}
#[test]
fn the_fraction_rule_stops_short_of_zero() {
assert_eq!(fraction_to_boundary(&[1.0], &[-1.0], &[]), TAU);
assert_eq!(fraction_to_boundary(&[1.0], &[1.0], &[]), 1.0);
assert_eq!(fraction_to_boundary(&[2.0, 1.0], &[-1.0, -1.0], &[]), TAU);
assert_eq!(fraction_to_boundary(&[1.0], &[-4.0], &[]), TAU / 4.0);
assert_eq!(fraction_to_boundary(&[0.0, 1.0], &[-1.0, -1.0], &[]), 0.0);
assert_eq!(fraction_to_boundary(&[0.0, 1.0], &[-1.0, -1.0], &[0]), TAU);
}
}