use crate::sqp::bfgs::DampedBfgs;
use crate::sqp::filter::{SqpFilter, filter_line_search};
use crate::sqp::iterates::SqpIterates;
use crate::sqp::lbfgs::LBfgs;
use crate::sqp::line_search::l1_merit_line_search;
use crate::sqp::options::{SqpGlobalization, SqpHessianSource, SqpOptions};
use crate::sqp::problem::SqpProblemSpec;
use crate::sqp::qp_assembly::{SqpQpData, Triplet};
use crate::sqp::result::{SqpError, SqpResult, SqpStatus};
use pounce_common::types::{NLP_LOWER_BOUND_INF, NLP_UPPER_BOUND_INF, Number};
use pounce_linalg::triplet::GenTMatrix;
use pounce_qp::{
HessianInertia, ParametricActiveSetSolver, QpOptions, QpProblem, QpSolver, QpStatus, WorkingSet,
};
pub struct SqpAlgorithm {
qp_solver: ParametricActiveSetSolver,
qp_opts: QpOptions,
opts: SqpOptions,
iterates: Option<SqpIterates>,
filter: SqpFilter,
}
impl SqpAlgorithm {
pub fn new(qp_solver: ParametricActiveSetSolver, opts: SqpOptions) -> Self {
Self {
qp_solver,
qp_opts: QpOptions::sqp_subproblem(),
opts,
iterates: None,
filter: SqpFilter::new(),
}
}
pub fn with_qp_options(mut self, qp_opts: QpOptions) -> Self {
self.qp_opts = qp_opts;
self
}
pub fn options(&self) -> &SqpOptions {
&self.opts
}
pub fn iterates(&self) -> Option<&SqpIterates> {
self.iterates.as_ref()
}
pub fn optimize<N: SqpProblemSpec>(&mut self, nlp: &mut N) -> Result<SqpResult, SqpError> {
self.optimize_with_warm_start(nlp, None)
}
pub fn optimize_with_warm_start<N: SqpProblemSpec>(
&mut self,
nlp: &mut N,
warm: Option<SqpIterates>,
) -> Result<SqpResult, SqpError> {
let n = nlp.n();
let m = nlp.m();
let (xl, xu) = nlp.variable_bounds();
let (bl_c, bu_c) = nlp.constraint_bounds();
if xl.len() != n || xu.len() != n {
return Err(SqpError::DimensionMismatch(format!(
"variable_bounds length must be n = {n}"
)));
}
if bl_c.len() != m || bu_c.len() != m {
return Err(SqpError::DimensionMismatch(format!(
"constraint_bounds length must be m = {m}"
)));
}
let mut iter = match warm {
Some(w) => {
if w.x.len() != n {
return Err(SqpError::DimensionMismatch(format!(
"warm.x length {} must equal n = {n}",
w.x.len()
)));
}
if w.lambda_g.len() != m {
return Err(SqpError::DimensionMismatch(format!(
"warm.lambda_g length {} must equal m = {m}",
w.lambda_g.len()
)));
}
if w.lambda_x.len() != n {
return Err(SqpError::DimensionMismatch(format!(
"warm.lambda_x length {} must equal n = {n}",
w.lambda_x.len()
)));
}
if let Some(ws) = w.working.as_ref() {
ws.validate_dims(n, m).map_err(SqpError::QpFailure)?;
}
w
}
None => {
let mut cold = SqpIterates::cold(n, m);
let x_init = nlp.x_init();
if x_init.len() != n {
return Err(SqpError::DimensionMismatch(format!(
"x_init length must be n = {n}"
)));
}
cold.x = x_init;
cold
}
};
let mut n_qp_solves: u32 = 0;
let mut n_qp_working_set_changes: u32 = 0;
let mut final_stationarity = 0.0;
let mut final_constr_viol = 0.0;
let mut nu = self.opts.l1_penalty;
self.filter = SqpFilter::new();
let mut f_cached: Option<Number> = None;
let mut c_cached: Option<Vec<Number>> = None;
let mut prev_point: Option<(Vec<Number>, Vec<Number>, Triplet)> = None;
let mut bfgs: Option<DampedBfgs> =
if matches!(self.opts.hessian, SqpHessianSource::DampedBfgs) {
Some(DampedBfgs::new(n))
} else {
None
};
let mut lbfgs: Option<LBfgs> = if matches!(self.opts.hessian, SqpHessianSource::Lbfgs) {
Some(LBfgs::new(n, self.opts.lbfgs_max_history.max(1) as usize))
} else {
None
};
if let Some(b) = bfgs.as_mut() {
let g0 = nlp.eval_grad_f(&iter.x);
let g_norm = g0.iter().map(|v| v * v).sum::<Number>().sqrt();
if g_norm.is_finite() && g_norm > 0.0 {
let x_scale = iter.x.iter().map(|v| v.abs()).fold(1.0, f64::max);
let eps = 1e-7 * x_scale;
let step: Vec<Number> = g0.iter().map(|gi| -eps * gi / g_norm).collect();
let x_probe: Vec<Number> =
iter.x.iter().zip(step.iter()).map(|(a, d)| a + d).collect();
let linear_constraints = m == 0 || {
let j0 = nlp.eval_jac_c(&iter.x);
let j1 = nlp.eval_jac_c(&x_probe);
j0.vals.len() == j1.vals.len()
&& j0.vals.iter().zip(j1.vals.iter()).all(|(a, c)| {
let scale = a.abs().max(c.abs()).max(1.0);
(a - c).abs() <= 1e-12 * scale
})
};
if linear_constraints {
let g1 = nlp.eval_grad_f(&x_probe);
let s_y: Number = step
.iter()
.zip(g1.iter().zip(g0.iter()))
.map(|(si, (a, bg))| si * (a - bg))
.sum();
let s_s: Number = step.iter().map(|v| v * v).sum();
if s_s > 0.0 && s_y.is_finite() {
b.seed_scale(s_y / s_s);
}
}
}
}
const MAX_SECOND_ORDER_ESCAPES: u32 = 8;
let mut escapes: u32 = 0;
for outer in 0..self.opts.max_iter {
let grad_f = nlp.eval_grad_f(&iter.x);
let c_vals = c_cached.take().unwrap_or_else(|| nlp.eval_c(&iter.x));
let f_curr = f_cached.take().unwrap_or_else(|| nlp.eval_f(&iter.x));
let jac_c = nlp.eval_jac_c(&iter.x);
let hess_lag = match self.opts.hessian {
SqpHessianSource::Exact => nlp.eval_hess_lag(&iter.x, &iter.lambda_g),
SqpHessianSource::DampedBfgs => {
let bfgs = bfgs.as_mut().expect("DampedBfgs state initialized above");
if let Some((s, y)) =
curvature_pair(prev_point.as_ref(), &iter, &grad_f, &jac_c, n)
{
bfgs.update_sy(&s, &y);
}
bfgs.as_triplet()
}
SqpHessianSource::Lbfgs => {
let lb = lbfgs.as_mut().expect("LBfgs state initialized above");
if let Some((s, y)) =
curvature_pair(prev_point.as_ref(), &iter, &grad_f, &jac_c, n)
{
lb.update_sy(&s, &y);
}
lb.as_triplet()
}
};
prev_point = Some((iter.x.clone(), grad_f.clone(), jac_c.clone()));
let kkt = check_kkt(
n, m, &iter, &grad_f, &c_vals, &bl_c, &bu_c, &xl, &xu, &jac_c,
);
final_stationarity = kkt.stationarity;
final_constr_viol = kkt.constr_viol;
if !kkt.stationarity.is_finite() || !kkt.constr_viol.is_finite() {
let obj = nlp.eval_f(&iter.x);
self.iterates = Some(iter.clone());
return Ok(SqpResult {
x: iter.x,
lambda_g: iter.lambda_g,
lambda_x: iter.lambda_x,
obj,
status: SqpStatus::InvalidNumber,
n_iter: outer,
n_qp_solves,
n_qp_working_set_changes,
final_stationarity,
final_constr_viol,
working_set: iter.working,
});
}
#[cfg(test)]
if self.opts.print_level >= 1 {
tracing::debug!(target: "pounce::sqp",
"[sqp k={outer:3}] x={:?} f={:.4e} ‖c‖={:.2e} stat={:.2e} ν={:.2e}",
iter.x.iter().map(|v| format!("{v:.3}")).collect::<Vec<_>>(),
f_curr,
kkt.constr_viol,
kkt.stationarity,
nu,
);
}
let stationarity_tol = self.opts.tol.min(self.opts.dual_inf_tol);
if kkt.stationarity <= stationarity_tol && kkt.constr_viol <= self.opts.constr_viol_tol
{
let exact_hess = if matches!(self.opts.hessian, SqpHessianSource::Exact) {
None
} else {
Some(nlp.eval_hess_lag(&iter.x, &iter.lambda_g))
};
if escapes < MAX_SECOND_ORDER_ESCAPES
&& let Some(d) = negative_curvature_at_kkt_point(
n,
m,
&iter.x,
exact_hess.as_ref().unwrap_or(&hess_lag),
&jac_c,
&c_vals,
&bl_c,
&bu_c,
&xl,
&xu,
self.opts.constr_viol_tol,
)
&& let Some(next) = exhibit_better_point(
nlp,
&iter.x,
&d,
f_curr,
&xl,
&xu,
&bl_c,
&bu_c,
self.opts.constr_viol_tol,
)
{
tracing::debug!(target: "pounce::sqp",
"first-order KKT point refuted at second order; stepping \
along negative curvature to a strictly better feasible \
point (gh #856)");
escapes += 1;
iter.x = next;
iter.working = None;
f_cached = None;
c_cached = None;
prev_point = None;
continue;
}
self.iterates = Some(iter.clone());
return Ok(SqpResult {
x: iter.x,
lambda_g: iter.lambda_g,
lambda_x: iter.lambda_x,
obj: f_curr,
status: SqpStatus::Optimal,
n_iter: outer,
n_qp_solves,
n_qp_working_set_changes,
final_stationarity,
final_constr_viol,
working_set: iter.working,
});
}
let qp_data = SqpQpData::build(
&iter.x,
&grad_f,
&c_vals,
&bl_c,
&bu_c,
&xl,
&xu,
jac_c,
hess_lag,
self.hessian_inertia(),
);
let qp = qp_data.as_qp();
const SCALE_MAX: Number = 1e6;
let g_inf = grad_f.iter().map(|v| v.abs()).fold(0.0, f64::max);
let b_inf = qp_data
.h
.values()
.iter()
.map(|v| v.abs())
.fold(0.0, f64::max);
let base = self.qp_opts.clone();
let scale = g_inf.max(b_inf).clamp(1.0, SCALE_MAX);
if scale > 1.0 {
self.qp_opts.opt_tol = base.opt_tol * scale;
self.qp_opts.feas_tol = base.feas_tol * scale;
}
let warm_started = iter.working.is_some();
let mut sol = if let Some(prev_w) = iter.working.as_ref() {
match self
.qp_solver
.solve_with_working_set(&qp, prev_w, &self.qp_opts)
{
Ok(v) => v,
Err(e) => {
tracing::debug!(target: "pounce::sqp",
"warm-started step QP failed hard ({e:?}); re-solving \
from cold, since the carried working set is what a \
cold start rebuilds (gh #855)");
n_qp_solves += 1;
self.qp_solver.solve(&qp, None, &self.qp_opts)?
}
}
} else {
self.qp_solver.solve(&qp, None, &self.qp_opts)?
};
n_qp_solves += 1;
n_qp_working_set_changes += sol.stats.n_working_set_changes;
if warm_started && matches!(sol.status, QpStatus::MaxIter | QpStatus::NumericalError) {
let cold = self.qp_solver.solve(&qp, None, &self.qp_opts)?;
n_qp_solves += 1;
n_qp_working_set_changes += cold.stats.n_working_set_changes;
if matches!(cold.status, QpStatus::Optimal | QpStatus::Unbounded) {
sol = cold;
}
}
let mut qp_data = qp_data;
if matches!(sol.status, QpStatus::MaxIter | QpStatus::NumericalError)
&& let Some(b) = bfgs.as_mut()
{
b.reset_to_scale();
qp_data = SqpQpData::build(
&iter.x,
&grad_f,
&c_vals,
&bl_c,
&bu_c,
&xl,
&xu,
nlp.eval_jac_c(&iter.x),
b.as_triplet(),
self.hessian_inertia(),
);
let retry = self
.qp_solver
.solve(&qp_data.as_qp(), None, &self.qp_opts)?;
n_qp_solves += 1;
n_qp_working_set_changes += retry.stats.n_working_set_changes;
if matches!(retry.status, QpStatus::Optimal | QpStatus::Unbounded) {
sol = retry;
}
}
let mut ray_certified = false;
if sol.status == QpStatus::Unbounded {
ray_certified = sol.unbounded_ray.as_ref().is_some_and(|d| {
ray_certifies_unbounded(
nlp,
&iter.x,
d,
f_curr,
&grad_f,
&bl_c,
&bu_c,
&xl,
&xu,
self.opts.constr_viol_tol,
)
});
if !ray_certified {
tracing::debug!(target: "pounce::sqp",
"unbounded step QP whose recession ray does not survive \
re-testing against the NLP — re-solving for the δ-shifted \
proximal step (gh #423)");
let prox_opts = QpOptions {
certify_recession_ray: false,
..self.qp_opts.clone()
};
let prox = self.qp_solver.solve(&qp_data.as_qp(), None, &prox_opts)?;
n_qp_solves += 1;
if prox.status == QpStatus::Optimal {
sol = prox;
}
}
}
self.qp_opts = base;
match sol.status {
QpStatus::Optimal => {}
QpStatus::Infeasible => {
let obj = nlp.eval_f(&iter.x);
self.iterates = Some(iter.clone());
return Ok(SqpResult {
x: iter.x,
lambda_g: iter.lambda_g,
lambda_x: iter.lambda_x,
obj,
status: SqpStatus::InfeasibleSubproblem,
n_iter: outer,
n_qp_solves,
n_qp_working_set_changes,
final_stationarity,
final_constr_viol,
working_set: iter.working,
});
}
QpStatus::MaxIter | QpStatus::TimeLimit | QpStatus::NumericalError => {
let obj = nlp.eval_f(&iter.x);
self.iterates = Some(iter.clone());
let status = if matches!(sol.status, QpStatus::MaxIter | QpStatus::TimeLimit) {
SqpStatus::QpIterationLimit
} else {
SqpStatus::QpStepFailed
};
return Ok(SqpResult {
x: iter.x,
lambda_g: iter.lambda_g,
lambda_x: iter.lambda_x,
obj,
status,
n_iter: outer,
n_qp_solves,
n_qp_working_set_changes,
final_stationarity,
final_constr_viol,
working_set: iter.working,
});
}
QpStatus::Unbounded => {
let certified = ray_certified;
let obj = nlp.eval_f(&iter.x);
self.iterates = Some(iter.clone());
return Ok(SqpResult {
x: iter.x,
lambda_g: iter.lambda_g,
lambda_x: iter.lambda_x,
obj,
status: if certified {
SqpStatus::Unbounded
} else {
SqpStatus::QpStepFailed
},
n_iter: outer,
n_qp_solves,
n_qp_working_set_changes,
final_stationarity,
final_constr_viol,
working_set: iter.working,
});
}
}
#[cfg(test)]
if self.opts.print_level >= 1 {
let p_inf = sol.x.iter().map(|v| v.abs()).fold(0.0_f64, f64::max);
tracing::debug!(target: "pounce::sqp",
" qp: ‖p‖_inf={:.3e} ‖λ_g_qp‖_inf={:.3e}",
p_inf,
sol.lambda_g.iter().map(|v| v.abs()).fold(0.0_f64, f64::max)
);
}
let a_p = if m > 0 {
mat_vec_gen(&qp_data.a, &sol.x, m)
} else {
Vec::new()
};
let mut n_soc_solves: u32 = 0;
let mut soc_working: Option<WorkingSet> = None;
let ls = {
let qp_solver = &mut self.qp_solver;
let qp_opts = &self.qp_opts;
let qp_data_ref = &qp_data;
let c_curr_ref = &c_vals;
let a_p_ref = &a_p;
let sol_working = &sol.working;
let n_soc = &mut n_soc_solves;
let soc_working_slot = &mut soc_working;
let mut soc = |c_trial: &[Number]| -> Option<crate::sqp::line_search::SocStep> {
let mm = qp_data_ref.m;
let mut bl_soc = qp_data_ref.bl.clone();
let mut bu_soc = qp_data_ref.bu.clone();
for i in 0..mm {
let delta = c_curr_ref[i] - c_trial[i] + a_p_ref[i];
if qp_data_ref.bl[i] > NLP_LOWER_BOUND_INF {
bl_soc[i] = qp_data_ref.bl[i] + delta;
}
if qp_data_ref.bu[i] < NLP_UPPER_BOUND_INF {
bu_soc[i] = qp_data_ref.bu[i] + delta;
}
}
let qp_soc = QpProblem {
n: qp_data_ref.n,
m: qp_data_ref.m,
h: &qp_data_ref.h,
g: &qp_data_ref.g,
a: &qp_data_ref.a,
bl: &bl_soc,
bu: &bu_soc,
xl: &qp_data_ref.xl,
xu: &qp_data_ref.xu,
hessian_inertia: qp_data_ref.hessian_inertia,
};
let sol_soc = qp_solver
.solve_with_working_set(&qp_soc, sol_working, qp_opts)
.ok()?;
*n_soc += 1;
if sol_soc.status == QpStatus::Optimal {
*soc_working_slot = Some(sol_soc.working);
Some(crate::sqp::line_search::SocStep {
p: sol_soc.x,
lambda_g: sol_soc.lambda_g,
lambda_x: sol_soc.lambda_x,
})
} else {
None
}
};
let soc_ref: Option<crate::sqp::line_search::SocProvider<'_>> =
if m > 0 { Some(&mut soc) } else { None };
match self.opts.globalization {
SqpGlobalization::L1Elastic => l1_merit_line_search(
nlp,
&iter.x,
&sol.x,
&sol.lambda_g,
&grad_f,
f_curr,
&c_vals,
&bl_c,
&bu_c,
&xl,
&xu,
nu,
&self.opts,
soc_ref,
),
SqpGlobalization::Filter => filter_line_search(
nlp,
&mut self.filter,
&iter.x,
&sol.x,
f_curr,
&c_vals,
&bl_c,
&bu_c,
&xl,
&xu,
nu,
&self.opts,
soc_ref,
),
}
};
n_qp_solves += n_soc_solves;
#[cfg(test)]
if self.opts.print_level >= 1 {
tracing::debug!(target: "pounce::sqp",
" ls: α={:.3e} ν={:.3e} ok={} f_new={:.3e}",
ls.alpha, ls.nu, ls.success, ls.f_new
);
}
if !ls.success {
self.iterates = Some(iter.clone());
return Ok(SqpResult {
x: iter.x,
lambda_g: iter.lambda_g,
lambda_x: iter.lambda_x,
obj: f_curr,
status: SqpStatus::LineSearchFailed,
n_iter: outer,
n_qp_solves,
n_qp_working_set_changes,
final_stationarity,
final_constr_viol,
working_set: Some(sol.working),
});
}
iter.x = ls.x_new;
match ls.soc_duals {
Some((soc_lg, soc_lx)) => {
iter.lambda_g = soc_lg;
iter.lambda_x = soc_lx;
iter.working = soc_working.take().or(Some(sol.working));
}
None => {
for (l, &lq) in iter.lambda_g.iter_mut().zip(sol.lambda_g.iter()) {
*l = (1.0 - ls.alpha) * *l + ls.alpha * lq;
}
for (l, &lq) in iter.lambda_x.iter_mut().zip(sol.lambda_x.iter()) {
*l = (1.0 - ls.alpha) * *l + ls.alpha * lq;
}
iter.working = Some(sol.working);
}
}
nu = ls.nu;
f_cached = Some(ls.f_new);
c_cached = Some(ls.c_new);
}
let obj = nlp.eval_f(&iter.x);
self.iterates = Some(iter.clone());
Ok(SqpResult {
x: iter.x,
lambda_g: iter.lambda_g,
lambda_x: iter.lambda_x,
obj,
status: SqpStatus::MaxIter,
n_iter: self.opts.max_iter,
n_qp_solves,
n_qp_working_set_changes,
final_stationarity,
final_constr_viol,
working_set: iter.working,
})
}
fn hessian_inertia(&self) -> HessianInertia {
match self.opts.hessian {
crate::sqp::SqpHessianSource::Exact => HessianInertia::Indefinite,
crate::sqp::SqpHessianSource::DampedBfgs => HessianInertia::Psd,
crate::sqp::SqpHessianSource::Lbfgs => HessianInertia::Psd,
}
}
}
#[allow(clippy::too_many_arguments)]
fn ray_certifies_unbounded<N: SqpProblemSpec>(
nlp: &mut N,
x: &[Number],
dir: &[Number],
f_x: Number,
grad_f: &[Number],
bl_c: &[Number],
bu_c: &[Number],
xl: &[Number],
xu: &[Number],
constr_viol_tol: Number,
) -> bool {
const PROBES: [Number; 7] = [1e0, 1e2, 1e4, 1e6, 1e8, 1e10, 1e12];
const ROUNDOFF_REL: Number = 1e-12;
let n = x.len();
if dir.len() != n || grad_f.len() != n || !f_x.is_finite() {
return false;
}
let scale = dir.iter().map(|v| v.abs()).fold(0.0, f64::max);
if !scale.is_finite() || scale <= 0.0 {
return false;
}
let d: Vec<Number> = dir.iter().map(|v| v / scale).collect();
let slope: Number = grad_f.iter().zip(d.iter()).map(|(g, di)| g * di).sum();
let g_norm = grad_f.iter().map(|v| v * v).sum::<Number>().sqrt();
let descent_bar = -1e-9 * g_norm.max(1.0);
if !slope.is_finite() || slope >= descent_bar {
return false;
}
let m = bl_c.len();
let mut row_scale: Vec<Number> = vec![0.0; m];
{
let jac = nlp.eval_jac_c(x);
for k in 0..jac.vals.len() {
let i = (jac.irow[k] - 1) as usize;
row_scale[i] = row_scale[i].max(jac.vals[k].abs());
}
}
for &t in PROBES.iter() {
let xt: Vec<Number> = x.iter().zip(d.iter()).map(|(xi, di)| xi + t * di).collect();
if xt.iter().any(|v| !v.is_finite()) {
return false;
}
for i in 0..n {
let tol = 1e-9 * (1.0 + xt[i].abs());
if xl[i] > NLP_LOWER_BOUND_INF && xt[i] < xl[i] - tol {
return false;
}
if xu[i] < NLP_UPPER_BOUND_INF && xt[i] > xu[i] + tol {
return false;
}
}
let x_inf = xt.iter().map(|v| v.abs()).fold(0.0, f64::max);
let c = nlp.eval_c(&xt);
if c.len() != m {
return false;
}
for i in 0..m {
if c[i].is_nan() {
return false;
}
let tol =
constr_viol_tol.max(0.0) * (1.0 + c[i].abs()) + ROUNDOFF_REL * row_scale[i] * x_inf;
if bl_c[i] > NLP_LOWER_BOUND_INF && c[i] < bl_c[i] - tol {
return false;
}
if bu_c[i] < NLP_UPPER_BOUND_INF && c[i] > bu_c[i] + tol {
return false;
}
}
let f_t = nlp.eval_f(&xt);
if f_t.is_nan() || f_t > f_x + 0.5 * slope * t {
return false;
}
}
true
}
#[derive(Debug, Clone, Copy)]
pub(crate) struct KktError {
pub stationarity: Number,
pub constr_viol: Number,
}
fn mat_vec_gen(a: &GenTMatrix, p: &[Number], m: usize) -> Vec<Number> {
let mut out = vec![0.0; m];
let irows = a.irows();
let jcols = a.jcols();
let vals = a.values();
for k in 0..vals.len() {
let i = (irows[k] - 1) as usize;
let j = (jcols[k] - 1) as usize;
out[i] += vals[k] * p[j];
}
out
}
fn curvature_pair(
prev: Option<&(Vec<Number>, Vec<Number>, Triplet)>,
iter: &SqpIterates,
grad_f: &[Number],
jac_c: &Triplet,
n: usize,
) -> Option<(Vec<Number>, Vec<Number>)> {
let (prev_x, prev_grad_f, prev_jac) = prev?;
let s: Vec<Number> = iter
.x
.iter()
.zip(prev_x.iter())
.map(|(a, b)| a - b)
.collect();
let lag_curr = compute_grad_lag(grad_f, jac_c, &iter.lambda_g, n);
let lag_prev = compute_grad_lag(prev_grad_f, prev_jac, &iter.lambda_g, n);
let y: Vec<Number> = lag_curr
.iter()
.zip(lag_prev.iter())
.map(|(a, b)| a - b)
.collect();
Some((s, y))
}
fn compute_grad_lag(
grad_f: &[Number],
jac_c: &Triplet,
lambda_g: &[Number],
n: usize,
) -> Vec<Number> {
let mut out = grad_f.to_vec();
debug_assert_eq!(out.len(), n);
for k in 0..jac_c.irow.len() {
let row_i = (jac_c.irow[k] - 1) as usize;
let col_j = (jac_c.jcol[k] - 1) as usize;
out[col_j] += jac_c.vals[k] * lambda_g[row_i];
}
out
}
#[allow(clippy::too_many_arguments)]
fn exhibit_better_point<N: SqpProblemSpec>(
nlp: &mut N,
x: &[Number],
d: &[Number],
f_curr: Number,
xl: &[Number],
xu: &[Number],
bl_c: &[Number],
bu_c: &[Number],
constr_viol_tol: Number,
) -> Option<Vec<Number>> {
let n = x.len();
let dn = d.iter().fold(0.0_f64, |a, v| a.max(v.abs()));
if !(dn > 0.0) || !dn.is_finite() {
return None;
}
let viol_at = |nlp: &mut N, v: &[Number]| -> (Number, Number) {
let c = nlp.eval_c(v);
let mut viol = 0.0_f64;
let mut scale = 0.0_f64;
for (j, &cj) in c.iter().enumerate() {
viol = viol
.max((bl_c[j] - cj).max(0.0))
.max((cj - bu_c[j]).max(0.0));
scale = scale.max(cj.abs());
}
(viol, scale)
};
let (viol_curr, _) = viol_at(nlp, x);
let mut best: Option<(Number, Vec<Number>)> = None;
for sign in [1.0_f64, -1.0] {
let mut alpha = f64::INFINITY;
for i in 0..n {
let di = sign * d[i];
if di > 1e-12 * dn && xu[i] < f64::INFINITY {
alpha = alpha.min((xu[i] - x[i]) / di);
}
if di < -1e-12 * dn && xl[i] > f64::NEG_INFINITY {
alpha = alpha.min((x[i] - xl[i]) / -di);
}
}
if !alpha.is_finite() {
alpha = 1.0 / dn;
}
for _ in 0..24 {
if !(alpha > 0.0) {
break;
}
let trial: Vec<Number> = (0..n)
.map(|i| (x[i] + sign * alpha * d[i]).clamp(xl[i], xu[i]))
.collect();
let (viol, c_scale) = viol_at(nlp, &trial);
let feas_bar = viol_curr.max(1e-12 * c_scale).min(constr_viol_tol);
if viol <= feas_bar {
let f_trial = nlp.eval_f(&trial);
if f_trial.is_finite() && best.as_ref().is_none_or(|(b, _)| f_trial < *b) {
best = Some((f_trial, trial));
}
}
alpha *= 0.5;
}
}
let (f_best, x_best) = best?;
let f_scale = f_curr.abs().max(f_best.abs());
let bar = 1e-10 * (f_scale.min(1.0) + f_curr.abs());
(f_best < f_curr - bar).then_some(x_best)
}
fn active_tol(tol: Number, scale: &[Number]) -> Number {
let s = scale
.iter()
.filter(|v| v.is_finite())
.fold(0.0_f64, |a, v| a.max(v.abs()));
tol * s.min(1.0)
}
#[allow(clippy::too_many_arguments)]
fn negative_curvature_at_kkt_point(
n: usize,
m: usize,
x: &[Number],
hess_lag: &Triplet,
jac_c: &Triplet,
c_vals: &[Number],
bl_c: &[Number],
bu_c: &[Number],
xl: &[Number],
xu: &[Number],
tol: Number,
) -> Option<Vec<Number>> {
const MAX_N: usize = 512;
if n == 0 || n > MAX_N {
return None;
}
let mut rows: Vec<Vec<Number>> = Vec::new();
for j in 0..m {
let t = active_tol(tol, &[c_vals[j], bl_c[j], bu_c[j]]);
let lo_active = bl_c[j] > f64::NEG_INFINITY && (c_vals[j] - bl_c[j]).abs() <= t;
let hi_active = bu_c[j] < f64::INFINITY && (bu_c[j] - c_vals[j]).abs() <= t;
if lo_active || hi_active {
let mut r = vec![0.0; n];
for k in 0..jac_c.vals.len() {
if jac_c.irow[k] as usize == j + 1 {
r[jac_c.jcol[k] as usize - 1] += jac_c.vals[k];
}
}
rows.push(r);
}
}
for i in 0..n {
let t = active_tol(tol, &[x[i], xl[i], xu[i]]);
let on_lo = xl[i] > f64::NEG_INFINITY && (x[i] - xl[i]).abs() <= t;
let on_hi = xu[i] < f64::INFINITY && (xu[i] - x[i]).abs() <= t;
if on_lo || on_hi {
let mut r = vec![0.0; n];
r[i] = 1.0;
rows.push(r);
}
}
let mut z: Vec<Number> = Vec::new();
let n_dof;
if rows.is_empty() {
n_dof = n;
z = vec![0.0; n * n];
for i in 0..n {
z[i * n + i] = 1.0;
}
} else {
let mut btb = vec![0.0; n * n];
for r in &rows {
for a in 0..n {
if r[a] == 0.0 {
continue;
}
for c in 0..n {
btb[c * n + a] += r[a] * r[c];
}
}
}
let (mut ev, mut evec) = (vec![0.0; n], vec![0.0; n * n]);
if !pounce_linalg::symmetric_eigen(&btb, n, &mut ev, &mut evec) {
return None;
}
let lam_max = ev.iter().fold(0.0_f64, |a, v| a.max(v.abs()));
let cut = if lam_max > 0.0 { 1e-9 * lam_max } else { 0.0 };
if lam_max == 0.0 {
z.extend_from_slice(&evec[..n * n]);
n_dof = n;
} else {
for (j, &lam) in ev.iter().enumerate() {
if lam.abs() <= cut {
z.extend_from_slice(&evec[j * n..(j + 1) * n]);
}
}
n_dof = z.len() / n;
}
}
if n_dof == 0 {
return None;
}
let mut hz = vec![0.0; n * n_dof];
for k in 0..n_dof {
let (zc, out) = (&z[k * n..(k + 1) * n], &mut hz[k * n..(k + 1) * n]);
for e in 0..hess_lag.vals.len() {
let (r, c, v) = (
hess_lag.irow[e] as usize - 1,
hess_lag.jcol[e] as usize - 1,
hess_lag.vals[e],
);
out[r] += v * zc[c];
if r != c {
out[c] += v * zc[r];
}
}
}
let mut rh = vec![0.0; n_dof * n_dof];
for a in 0..n_dof {
for b in 0..n_dof {
rh[b * n_dof + a] = (0..n).map(|i| z[a * n + i] * hz[b * n + i]).sum();
}
}
let (mut ev, mut evec) = (vec![0.0; n_dof], vec![0.0; n_dof * n_dof]);
if !pounce_linalg::symmetric_eigen(&rh, n_dof, &mut ev, &mut evec) {
return None;
}
let h_scale = hess_lag.vals.iter().fold(0.0_f64, |a, v| a.max(v.abs()));
if ev[0] >= -1e-8 * h_scale {
return None;
}
let mut d = vec![0.0; n];
for (i, di) in d.iter_mut().enumerate() {
*di = (0..n_dof).map(|k| z[k * n + i] * evec[k]).sum();
}
Some(d)
}
pub(crate) fn check_kkt(
n: usize,
m: usize,
iter: &SqpIterates,
grad_f: &[Number],
c_vals: &[Number],
bl_c: &[Number],
bu_c: &[Number],
xl: &[Number],
xu: &[Number],
jac_c: &crate::sqp::qp_assembly::Triplet,
) -> KktError {
let mut viol = 0.0_f64;
let mut nonfinite = false;
for i in 0..m {
let lo = if bl_c[i] > NLP_LOWER_BOUND_INF {
(bl_c[i] - c_vals[i]).max(0.0)
} else {
0.0
};
let hi = if bu_c[i] < NLP_UPPER_BOUND_INF {
(c_vals[i] - bu_c[i]).max(0.0)
} else {
0.0
};
nonfinite |= !c_vals[i].is_finite();
viol = viol.max(lo).max(hi);
}
for i in 0..n {
nonfinite |= !iter.x[i].is_finite();
let lo = if xl[i] > NLP_LOWER_BOUND_INF {
(xl[i] - iter.x[i]).max(0.0)
} else {
0.0
};
let hi = if xu[i] < NLP_UPPER_BOUND_INF {
(iter.x[i] - xu[i]).max(0.0)
} else {
0.0
};
viol = viol.max(lo).max(hi);
}
let mut stat = vec![0.0; n];
for (s, &g) in stat.iter_mut().zip(grad_f.iter()) {
*s = g;
}
for k in 0..jac_c.irow.len() {
let i = (jac_c.irow[k] - 1) as usize; let j = (jac_c.jcol[k] - 1) as usize; stat[j] += jac_c.vals[k] * iter.lambda_g[i];
}
for (s, &lx) in stat.iter_mut().zip(iter.lambda_x.iter()) {
*s -= lx;
}
let stat_max = crate::sqp::line_search::inf_norm(&stat);
if nonfinite {
return KktError {
stationarity: Number::NAN,
constr_viol: Number::NAN,
};
}
KktError {
stationarity: stat_max,
constr_viol: viol,
}
}
#[cfg(test)]
mod kkt_nan_tests {
use super::*;
use crate::sqp::qp_assembly::Triplet;
use pounce_common::types::Index;
fn setup(x: Vec<Number>, c: Vec<Number>, lam_g: Vec<Number>) -> KktError {
let n = x.len();
let m = c.len();
let iter = SqpIterates {
x,
lambda_g: lam_g,
lambda_x: vec![0.0; n],
working: None,
};
let jac = Triplet {
n_rows: m,
n_cols: n,
irow: (1..=m as i32).map(|i| i as Index).collect(),
jcol: vec![1 as Index; m],
vals: vec![1.0; m],
};
check_kkt(
n,
m,
&iter,
&vec![1.0; n],
&c,
&vec![0.0; m],
&vec![0.0; m],
&vec![NLP_LOWER_BOUND_INF; n],
&vec![NLP_UPPER_BOUND_INF; n],
&jac,
)
}
#[test]
fn a_finite_iterate_is_measured_exactly_as_before() {
let k = setup(vec![2.0], vec![2.0], vec![-1.0]);
assert_eq!(k.constr_viol, 2.0);
assert_eq!(k.stationarity, 0.0);
}
#[test]
fn a_nan_constraint_value_is_not_perfect_feasibility() {
let k = setup(vec![0.0], vec![Number::NAN], vec![-1.0]);
assert!(
k.constr_viol.is_nan(),
"a NaN constraint value reported constr_viol = {}, which clears \
every tolerance the caller compares it against",
k.constr_viol
);
assert!(k.stationarity.is_nan());
}
#[test]
fn a_nan_iterate_is_not_perfect_feasibility() {
let k = setup(vec![Number::NAN], vec![0.0], vec![-1.0]);
assert!(k.constr_viol.is_nan());
assert!(k.stationarity.is_nan());
}
#[test]
fn a_nan_dual_poisons_only_stationarity() {
let k = setup(vec![0.0], vec![0.0], vec![Number::NAN]);
assert!(
k.stationarity.is_nan(),
"a NaN multiplier reduced to stationarity = {}",
k.stationarity
);
assert_eq!(k.constr_viol, 0.0);
}
}
#[cfg(test)]
mod inf_norm_tests {
use crate::sqp::line_search::inf_norm;
#[test]
fn a_nan_entry_propagates_rather_than_reducing_away() {
assert!(inf_norm(&[1.0, f64::NAN, 2.0]).is_nan());
assert!(inf_norm(&[f64::NAN]).is_nan());
}
#[test]
fn finite_vectors_are_unchanged() {
assert_eq!(inf_norm(&[]), 0.0);
assert_eq!(inf_norm(&[-3.0, 1.0, 2.0]), 3.0);
assert_eq!(inf_norm(&[f64::INFINITY, 1.0]), f64::INFINITY);
}
}