Skip to main content

tpt_math_optimize_convex/
lib.rs

1#![allow(
2    clippy::type_complexity,
3    clippy::manual_clamp,
4    clippy::manual_range_contains,
5    clippy::needless_range_loop
6)]
7//! Convex / quadratic optimization (in-house dense interior-point solver).
8//!
9//! This crate solves convex quadratic programs (QPs) of the form
10//!
11//! ```text
12//! minimize    ½ xᵀ P x + qᵀ x
13//! subject to  A_eq x = b_eq
14//!             A_ineq x ≤ b_ineq
15//!             l ≤ x ≤ u            (per-variable bounds)
16//! ```
17//!
18//! with a self-contained dense primal-dual interior-point method
19//! (Mehrotra predictor-corrector). The KKT linear systems are inverted with
20//! [`tpt_math_linalg_dense`]'s in-house dense solver, so there is no
21//! Apache-2.0-only dependency anywhere in the tree (the old `clarabel` backend
22//! was removed for that reason — see `spec.txt` / ADR-0007).
23//!
24//! The problem is converted into conic form
25//!
26//! ```text
27//! minimize    qᵀ x + ½ xᵀ P x
28//! subject to  A_eq x        = b_eq
29//!             A x + s       = b,   s ≥ 0
30//! ```
31//!
32//! where the equality constraints use the zero cone (no slack) and the
33//! inequality constraints plus per-variable bounds use the nonnegative cone
34//! (one slack `s_j ≥ 0` each). Bounds `l ≤ x ≤ u` are mapped to two
35//! nonnegative slack inequalities each (`x - l ≥ 0` and `u - x ≥ 0`); a
36//! non-finite bound is skipped (treated as "unbounded" on that side).
37//!
38//! # Examples
39//!
40//! ```
41//! use tpt_math_linalg_dense::{DMatrix, DVector};
42//! use tpt_math_optimize_convex::solve_qp;
43//!
44//! // minimize x² + y²  subject to  x + y = 1
45//! // optimum is (0.5, 0.5)
46//! let p = DMatrix::from_row_slice(2, 2, &[2.0, 0.0, 0.0, 2.0]);
47//! let q = DVector::from_vec(vec![0.0, 0.0]);
48//! let a_eq = DMatrix::from_row_slice(1, 2, &[1.0, 1.0]);
49//! let b_eq = DVector::from_vec(vec![1.0]);
50//! let x = solve_qp(&p, &q, &a_eq, &b_eq, &[]).unwrap();
51//! assert!((x[0] - 0.5).abs() < 1e-6 && (x[1] - 0.5).abs() < 1e-6);
52//! ```
53//!
54//! [`tpt_math_linalg_dense`]: crate::tpt_math_linalg_dense
55
56#![forbid(unsafe_code)]
57#![warn(missing_docs)]
58
59use tpt_math_linalg_dense::{DMatrix, DVector, Lu};
60
61/// Re-export of [`tpt_math_linalg_dense`] (in-house dense storage) for
62/// constructing dense inputs such as [`DMatrix`] and [`DVector`].
63pub use tpt_math_linalg_dense;
64
65/// Errors returned by the QP solvers in this crate.
66#[derive(Debug, Clone, PartialEq)]
67#[non_exhaustive]
68pub enum ConvexError {
69    /// A matrix or vector had an incompatible dimension (e.g. `A_eq` column
70    /// count does not match the number of variables `n`).
71    DimensionMismatch {
72        /// Human-readable description of the conflict.
73        what: String,
74    },
75    /// An input contained a non-finite (`NaN` or `infinite`) entry.
76    NotFinite {
77        /// Which input was non-finite.
78        what: String,
79    },
80    /// The interior-point solver did not return a (near) optimal solution —
81    /// non-convergence, infeasibility, or unboundedness. The `status` string
82    /// carries the reason.
83    Solver {
84        /// Human-readable solver outcome.
85        status: String,
86    },
87}
88
89impl std::fmt::Display for ConvexError {
90    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
91        match self {
92            ConvexError::DimensionMismatch { what } => write!(f, "dimension mismatch: {what}"),
93            ConvexError::NotFinite { what } => write!(f, "non-finite input: {what}"),
94            ConvexError::Solver { status } => write!(f, "solver failed: {status}"),
95        }
96    }
97}
98
99impl std::error::Error for ConvexError {}
100
101/// Outcome of a single interior-point solve, used by the status field of
102/// [`QpSolution`].
103#[derive(Debug, Clone, PartialEq)]
104#[non_exhaustive]
105pub enum QpStatus {
106    /// The optimizer converged to a solution within tolerance.
107    Solved,
108    /// The optimizer converged to a solution at reduced accuracy.
109    AlmostSolved,
110    /// The solve terminated without an optimum; the string explains why.
111    Failed(String),
112}
113
114/// A successful solution to a quadratic program.
115#[derive(Debug, Clone)]
116#[non_exhaustive]
117pub struct QpSolution {
118    /// The optimal primal variable vector `x`.
119    pub x: DVector<f64>,
120    /// The optimal objective value `½ xᵀ P x + qᵀ x`.
121    pub objective: f64,
122    /// The solve outcome. Usually [`QpStatus::Solved`].
123    pub status: QpStatus,
124    /// Interior-point iterations performed before the solve was accepted.
125    pub iterations: usize,
126}
127
128/// Solve a QP with equality constraints and per-variable bounds (the most
129/// common convex QP form). See the crate-level docs for the formulation.
130///
131/// See [`solve_qp_internal`] for the full form including inequality
132/// constraints.
133///
134/// A solution accepted at reduced accuracy ([`QpStatus::AlmostSolved`]) is
135/// returned as `Ok`. Use [`solve_qp_internal`] to read the status.
136///
137/// # Errors
138///
139/// Returns [`ConvexError`] on dimension mismatch, non-finite input, or if the
140/// solver fails to converge to an optimum.
141pub fn solve_qp(
142    p: &DMatrix<f64>,
143    q: &DVector<f64>,
144    a_eq: &DMatrix<f64>,
145    b_eq: &DVector<f64>,
146    bounds: &[(f64, f64)],
147) -> Result<DVector<f64>, ConvexError> {
148    let a_ineq = DMatrix::<f64>::zeros(0, q.len());
149    let b_ineq = DVector::<f64>::zeros(0);
150    solve_qp_internal(p, q, a_eq, b_eq, &a_ineq, &b_ineq, bounds).map(|s| s.x)
151}
152
153/// Full QP solver: equality constraints, inequality constraints, and
154/// per-variable bounds.
155///
156/// # Errors
157///
158/// Returns [`ConvexError`] on dimension mismatch, non-finite input, or if the
159/// solver fails to converge to an optimum.
160pub fn solve_qp_internal(
161    p: &DMatrix<f64>,
162    q: &DVector<f64>,
163    a_eq: &DMatrix<f64>,
164    b_eq: &DVector<f64>,
165    a_ineq: &DMatrix<f64>,
166    b_ineq: &DVector<f64>,
167    bounds: &[(f64, f64)],
168) -> Result<QpSolution, ConvexError> {
169    let n = q.len();
170
171    if p.nrows() != n || p.ncols() != n {
172        return Err(ConvexError::DimensionMismatch {
173            what: format!("P is {}x{}, expected {}x{}", p.nrows(), p.ncols(), n, n),
174        });
175    }
176    if !a_eq.is_empty() && a_eq.ncols() != n {
177        return Err(ConvexError::DimensionMismatch {
178            what: format!("A_eq has {} columns, expected {n}", a_eq.ncols()),
179        });
180    }
181    if a_eq.nrows() != b_eq.len() {
182        return Err(ConvexError::DimensionMismatch {
183            what: format!(
184                "A_eq has {} rows but b_eq has {} entries",
185                a_eq.nrows(),
186                b_eq.len()
187            ),
188        });
189    }
190    if !a_ineq.is_empty() && a_ineq.ncols() != n {
191        return Err(ConvexError::DimensionMismatch {
192            what: format!("A_ineq has {} columns, expected {n}", a_ineq.ncols()),
193        });
194    }
195    if a_ineq.nrows() != b_ineq.len() {
196        return Err(ConvexError::DimensionMismatch {
197            what: format!(
198                "A_ineq has {} rows but b_ineq has {} entries",
199                a_ineq.nrows(),
200                b_ineq.len()
201            ),
202        });
203    }
204    if bounds.len() != n && !bounds.is_empty() {
205        return Err(ConvexError::DimensionMismatch {
206            what: format!("bounds has {} entries, expected {n} or empty", bounds.len()),
207        });
208    }
209
210    check_finite_matrix(p, "P")?;
211    check_finite_vector(q, "q")?;
212    check_finite_matrix(a_eq, "A_eq")?;
213    check_finite_vector(b_eq, "b_eq")?;
214    check_finite_matrix(a_ineq, "A_ineq")?;
215    check_finite_vector(b_ineq, "b_ineq")?;
216
217    // Assemble the nonnegative-cone system (inequalities + finite bounds).
218    let (a, b) = build_inequality_system(a_ineq, b_ineq, bounds, n);
219
220    // Work in plain `Vec`/`Vec<Vec>` form for the iteration; the KKT solve is
221    // delegated to `tpt-math-linalg-dense`'s in-house `DMatrix::solve`.
222    let p_sym = symmetrize(p);
223    let qv = q.iter().copied().collect::<Vec<f64>>();
224    let a_eqv = matrix_to_vec(a_eq);
225    let b_eqv = b_eq.iter().copied().collect::<Vec<f64>>();
226
227    let (x, status, iterations) = interior_point(&p_sym, &qv, &a_eqv, &b_eqv, &a, &b)?;
228
229    let xv = DVector::from_vec(x);
230    let objective = qp_objective(&p_sym, &qv, &xv.iter().copied().collect::<Vec<f64>>());
231    Ok(QpSolution {
232        x: xv,
233        objective,
234        status,
235        iterations,
236    })
237}
238
239/// A builder for quadratic programs supporting equality constraints, inequality
240/// constraints, and per-variable bounds.
241///
242/// # Examples
243///
244/// ```
245/// use tpt_math_linalg_dense::{DMatrix, DVector};
246/// use tpt_math_optimize_convex::{QuadraticProgram, solve_qp};
247///
248/// // minimize x² + y²  subject to  x + y = 1, x ≥ 0, y ≥ 0
249/// let qp = QuadraticProgram::new(DVector::from_vec(vec![0.0, 0.0]))
250///     .objective(DMatrix::from_row_slice(2, 2, &[2.0, 0.0, 0.0, 2.0]))
251///     .equality(DMatrix::from_row_slice(1, 2, &[1.0, 1.0]), DVector::from_vec(vec![1.0]))
252///     .bounds(&[(0.0, f64::INFINITY), (0.0, f64::INFINITY)]);
253/// let sol = qp.solve().unwrap();
254/// assert!((sol.x[0] - 0.5).abs() < 1e-6 && (sol.x[1] - 0.5).abs() < 1e-6);
255/// ```
256#[derive(Debug, Clone)]
257pub struct QuadraticProgram {
258    p: DMatrix<f64>,
259    q: DVector<f64>,
260    a_eq: DMatrix<f64>,
261    b_eq: DVector<f64>,
262    a_ineq: DMatrix<f64>,
263    b_ineq: DVector<f64>,
264    bounds: Vec<(f64, f64)>,
265}
266
267impl QuadraticProgram {
268    /// Create a new QP with `n` variables and zero quadratic cost.
269    ///
270    /// The linear cost `q` must have length `n`.
271    pub fn new(q: DVector<f64>) -> Self {
272        let n = q.len();
273        QuadraticProgram {
274            p: DMatrix::zeros(n, n),
275            q,
276            a_eq: DMatrix::zeros(0, n),
277            b_eq: DVector::zeros(0),
278            a_ineq: DMatrix::zeros(0, n),
279            b_ineq: DVector::zeros(0),
280            bounds: Vec::new(),
281        }
282    }
283
284    /// Set the quadratic cost matrix `P` (`n×n`). It will be symmetrized.
285    pub fn objective(mut self, p: DMatrix<f64>) -> Self {
286        self.p = p;
287        self
288    }
289
290    /// Set the linear cost vector `q`.
291    pub fn linear_cost(mut self, q: DVector<f64>) -> Self {
292        self.q = q;
293        self
294    }
295
296    /// Set equality constraints `A_eq x = b_eq`.
297    pub fn equality(mut self, a_eq: DMatrix<f64>, b_eq: DVector<f64>) -> Self {
298        self.a_eq = a_eq;
299        self.b_eq = b_eq;
300        self
301    }
302
303    /// Set linear inequality constraints `A_ineq x ≤ b_ineq`.
304    pub fn inequality(mut self, a_ineq: DMatrix<f64>, b_ineq: DVector<f64>) -> Self {
305        self.a_ineq = a_ineq;
306        self.b_ineq = b_ineq;
307        self
308    }
309
310    /// Set per-variable bounds `(lower, upper)`; length must equal `n`.
311    pub fn bounds(mut self, bounds: &[(f64, f64)]) -> Self {
312        self.bounds = bounds.to_vec();
313        self
314    }
315
316    /// Solve the program and return a [`QpSolution`].
317    pub fn solve(self) -> Result<QpSolution, ConvexError> {
318        solve_qp_internal(
319            &self.p,
320            &self.q,
321            &self.a_eq,
322            &self.b_eq,
323            &self.a_ineq,
324            &self.b_ineq,
325            &self.bounds,
326        )
327    }
328}
329
330// ---------------------------------------------------------------------------
331// Internal helpers operating on `Vec` / `Vec<Vec>` form.
332// ---------------------------------------------------------------------------
333
334fn matrix_to_vec(m: &DMatrix<f64>) -> Vec<Vec<f64>> {
335    (0..m.nrows())
336        .map(|i| (0..m.ncols()).map(|j| m[(i, j)]).collect())
337        .collect()
338}
339
340fn symmetrize(m: &DMatrix<f64>) -> Vec<Vec<f64>> {
341    let n = m.nrows();
342    (0..n)
343        .map(|i| {
344            (0..m.ncols())
345                .map(|j| 0.5 * (m[(i, j)] + m[(j, i)]))
346                .collect()
347        })
348        .collect()
349}
350
351fn dot(a: &[f64], b: &[f64]) -> f64 {
352    a.iter().zip(b).map(|(x, y)| x * y).sum()
353}
354
355fn qp_objective(p: &[Vec<f64>], q: &[f64], x: &[f64]) -> f64 {
356    let n = x.len();
357    let mut quad = 0.0;
358    for i in 0..n {
359        let mut row = 0.0;
360        for k in 0..n {
361            row += p[i][k] * x[k];
362        }
363        quad += row * x[i];
364    }
365    0.5 * quad + dot(q, x)
366}
367
368fn check_finite_matrix(m: &DMatrix<f64>, what: &str) -> Result<(), ConvexError> {
369    if !m.iter().all(|v| v.is_finite()) {
370        return Err(ConvexError::NotFinite { what: what.into() });
371    }
372    Ok(())
373}
374
375fn check_finite_vector(v: &DVector<f64>, what: &str) -> Result<(), ConvexError> {
376    if !v.iter().all(|x| x.is_finite()) {
377        return Err(ConvexError::NotFinite { what: what.into() });
378    }
379    Ok(())
380}
381
382/// Build the nonnegative-cone system `A x + s = b, s ≥ 0` from the inequality
383/// constraints `A_ineq x ≤ b_ineq` and the per-variable bounds. Each inequality
384/// becomes one row; each finite bound becomes one row (`x - l ≥ 0` and/or
385/// `u - x ≥ 0`).
386fn build_inequality_system(
387    a_ineq: &DMatrix<f64>,
388    b_ineq: &DVector<f64>,
389    bounds: &[(f64, f64)],
390    n: usize,
391) -> (Vec<Vec<f64>>, Vec<f64>) {
392    let mut rows: Vec<Vec<f64>> = Vec::new();
393    let mut rhs: Vec<f64> = Vec::new();
394
395    for r in 0..a_ineq.nrows() {
396        let row: Vec<f64> = (0..n).map(|j| a_ineq[(r, j)]).collect();
397        rows.push(row);
398        rhs.push(b_ineq[r]);
399    }
400
401    for (j, &(l, u)) in bounds.iter().enumerate() {
402        if l.is_finite() {
403            // x_j ≥ l  =>  -x_j + s = -l
404            let mut row = vec![0.0; n];
405            row[j] = -1.0;
406            rows.push(row);
407            rhs.push(-l);
408        }
409        if u.is_finite() {
410            // x_j ≤ u  =>   x_j + s = u
411            let mut row = vec![0.0; n];
412            row[j] = 1.0;
413            rows.push(row);
414            rhs.push(u);
415        }
416    }
417
418    (rows, rhs)
419}
420
421/// Maximum step length `α ∈ [0, cap]` keeping `v + α dv ≥ 0`.
422fn step_length(v: &[f64], dv: &[f64], cap: f64) -> f64 {
423    let mut alpha = cap;
424    for i in 0..v.len() {
425        if dv[i] < 0.0 {
426            alpha = alpha.min(-v[i] / dv[i]);
427        }
428    }
429    alpha.max(0.0)
430}
431
432/// Solve the reduced KKT system `K [dx; dy] = rhs` and return the concatenated
433/// step. The matrix `K` is `(n + m_eq) × (n + m_eq)`:
434///
435/// ```text
436/// K = [ P + Aᵀ Σ A    A_eqᵀ ]
437///     [ A_eq            0    ]
438/// ```
439///
440/// with `Σ = diag(z / s)`. The solve uses `tpt-math-linalg-dense`'s in-house
441/// dense LU.
442///
443/// `a` is the nonnegative-cone constraint matrix (`m_c × n`), `a_eq` the
444/// equality matrix (`m_eq × n`).
445fn solve_kkt(
446    p: &[Vec<f64>],
447    a_eq: &[Vec<f64>],
448    a: &[Vec<f64>],
449    sigma: &[f64],
450    rhs: &[f64],
451) -> Result<Vec<f64>, ConvexError> {
452    let n = p.len();
453    let m_eq = a_eq.len();
454    let m_c = a.len();
455    let dim = n + m_eq;
456
457    let k = DMatrix::<f64>::from_fn(dim, dim, |i, j| {
458        if i < n && j < n {
459            let mut v = p[i][j];
460            for k in 0..m_c {
461                v += a[k][i] * sigma[k] * a[k][j];
462            }
463            v
464        } else if i < n && j >= n {
465            let e = j - n;
466            if e < m_eq {
467                a_eq[e][i]
468            } else {
469                0.0
470            }
471        } else if i >= n && j < n {
472            let e = i - n;
473            if e < m_eq {
474                a_eq[e][j]
475            } else {
476                0.0
477            }
478        } else {
479            0.0
480        }
481    });
482
483    let rh = DVector::from_vec(rhs.to_vec());
484    // Absolute 1e-12 pivot threshold, pinned explicitly: KKT pivots shrink as the
485    // barrier parameter goes to zero, so the relative default would change the
486    // solver's behaviour near the end of a solve.
487    let delta = Lu::with_pivot_tolerance(&k, 1e-12)
488        .and_then(|lu| lu.solve(&rh))
489        .map_err(|_| ConvexError::Solver {
490            status: "singular KKT system (infeasible or ill-posed)".into(),
491        })?;
492    Ok(delta.iter().copied().collect())
493}
494
495/// One interior-point iteration step (affine when `w` is zero, corrector
496/// otherwise). Returns `(dx, dy, ds, dz)`.
497#[allow(clippy::too_many_arguments)]
498fn compute_step(
499    p: &[Vec<f64>],
500    a_eq: &[Vec<f64>],
501    a: &[Vec<f64>],
502    _b: &[f64],
503    x: &[f64],
504    s: &[f64],
505    z: &[f64],
506    sigma: &[f64],
507    r_dual: &[f64],
508    r_pri_eq: &[f64],
509    r_pri2: &[f64],
510    w: &[f64],
511) -> Result<(Vec<f64>, Vec<f64>, Vec<f64>, Vec<f64>), ConvexError> {
512    let n = x.len();
513    let m_eq = a_eq.len();
514    let m_c = a.len();
515
516    let mut rhs = vec![0.0; n + m_eq];
517    for i in 0..n {
518        let mut v = -r_dual[i];
519        for j in 0..m_c {
520            v += a[j][i] * z[j];
521            v -= a[j][i] * sigma[j] * r_pri2[j];
522            v += a[j][i] * (w[j] / s[j]);
523        }
524        rhs[i] = v;
525    }
526    for e in 0..m_eq {
527        rhs[n + e] = -r_pri_eq[e];
528    }
529
530    let delta = solve_kkt(p, a_eq, a, sigma, &rhs)?;
531    let dx = delta[0..n].to_vec();
532    let dy = delta[n..n + m_eq].to_vec();
533
534    let mut ds = vec![0.0; m_c];
535    let mut dz = vec![0.0; m_c];
536    for j in 0..m_c {
537        let ax = dot(&a[j], &dx);
538        ds[j] = -r_pri2[j] - ax;
539        dz[j] = -z[j] + w[j] / s[j] - sigma[j] * ds[j];
540    }
541    Ok((dx, dy, ds, dz))
542}
543
544/// The Mehrotra predictor-corrector interior-point loop.
545fn interior_point(
546    p: &[Vec<f64>],
547    q: &[f64],
548    a_eq: &[Vec<f64>],
549    b_eq: &[f64],
550    a: &[Vec<f64>],
551    b: &[f64],
552) -> Result<(Vec<f64>, QpStatus, usize), ConvexError> {
553    let n = q.len();
554    let m_eq = a_eq.len();
555    let m_c = a.len();
556
557    let tol = 1e-8;
558    let max_iter = 200;
559    let tau = 0.99;
560    let huge = 1e14;
561    // Data scales used to make the residual tolerances relative (the "1 +"
562    // keeps them meaningful when the data are all zero).
563    let max_abs_p = p.iter().flatten().fold(0.0_f64, |acc, v| acc.max(v.abs()));
564    let scale_p = 1.0 + norm(b_eq) + norm(b);
565    let scale_d = 1.0 + norm(q) + max_abs_p;
566
567    let mut x = vec![0.0; n];
568    let mut y = vec![0.0; m_eq];
569    let mut s = vec![0.0; m_c];
570    let mut z = vec![0.0; m_c];
571    for j in 0..m_c {
572        let ax = dot(&a[j], &x);
573        let mut sj = b[j] - ax;
574        if !sj.is_finite() || sj <= 0.0 {
575            sj = 1.0;
576        }
577        s[j] = sj;
578        z[j] = 1.0;
579    }
580
581    for iter in 0..max_iter {
582        let r_dual = resid_dual(p, q, a_eq, &y, a, &z, &x);
583        let r_pri_eq = resid_pri_eq(a_eq, &x, b_eq);
584        let r_pri2 = resid_pri2(a, &x, &s, b);
585        let mu = if m_c > 0 {
586            dot(&s, &z) / (m_c as f64)
587        } else {
588            0.0
589        };
590
591        let prim = norm(&r_pri_eq) + norm(&r_pri2);
592        let dual = norm(&r_dual);
593        // Residual tolerances scale with the problem data so that a uniformly
594        // scaled QP converges the same way as the original.
595        // The complementarity gap m·mu is compared with the objective magnitude.
596        let obj_scale = 1.0 + qp_objective(p, q, &x).abs();
597        let gap = m_c as f64 * mu;
598        if prim < tol * scale_p && dual < tol * scale_d && (m_c == 0 || gap < tol * obj_scale) {
599            return Ok((x, QpStatus::Solved, iter + 1));
600        }
601        if prim < tol * 10.0 * scale_p
602            && dual < tol * 10.0 * scale_d
603            && (m_c == 0 || gap < tol * 10.0 * obj_scale)
604        {
605            return Ok((x, QpStatus::AlmostSolved, iter + 1));
606        }
607
608        let sigma_vec: Vec<f64> = (0..m_c).map(|j| z[j] / s[j]).collect();
609
610        // Predictor (affine) step.
611        let w_zero = vec![0.0; m_c];
612        let (_dx_aff, _dy_aff, ds_aff, dz_aff) = compute_step(
613            p, a_eq, a, b, &x, &s, &z, &sigma_vec, &r_dual, &r_pri_eq, &r_pri2, &w_zero,
614        )?;
615
616        let a_aff_pri = step_length(&s, &ds_aff, 1.0);
617        let a_aff_dual = step_length(&z, &dz_aff, 1.0);
618        let a_aff = a_aff_pri.min(a_aff_dual).min(1.0);
619        let mu_aff = if m_c > 0 {
620            let sp: Vec<f64> = (0..m_c).map(|j| s[j] + a_aff * ds_aff[j]).collect();
621            let zp: Vec<f64> = (0..m_c).map(|j| z[j] + a_aff * dz_aff[j]).collect();
622            dot(&sp, &zp) / (m_c as f64)
623        } else {
624            0.0
625        };
626        let sigma_cent = if m_c > 0 {
627            let ratio = mu_aff / mu.max(1e-300);
628            (ratio.max(0.0).min(1.0)).powi(3)
629        } else {
630            0.0
631        };
632
633        // Corrector step.
634        let w = (0..m_c)
635            .map(|j| sigma_cent * ds_aff[j] * dz_aff[j])
636            .collect::<Vec<f64>>();
637        let (dx, dy, ds, dz) = compute_step(
638            p, a_eq, a, b, &x, &s, &z, &sigma_vec, &r_dual, &r_pri_eq, &r_pri2, &w,
639        )?;
640
641        let a_pri = tau * step_length(&s, &ds, 1.0);
642        let a_dual = tau * step_length(&z, &dz, 1.0);
643
644        for i in 0..n {
645            x[i] += a_pri * dx[i];
646        }
647        for e in 0..m_eq {
648            y[e] += a_dual * dy[e];
649        }
650        for j in 0..m_c {
651            s[j] += a_pri * ds[j];
652            z[j] += a_dual * dz[j];
653        }
654
655        if x.iter().any(|v| !v.is_finite()) || norm(&x) > huge {
656            return Err(ConvexError::Solver {
657                status: "primal iterate diverged (problem is unbounded)".into(),
658            });
659        }
660        if s.iter().chain(z.iter()).any(|v| !v.is_finite()) {
661            return Err(ConvexError::Solver {
662                status: "dual iterate diverged (problem is infeasible)".into(),
663            });
664        }
665    }
666
667    Err(ConvexError::Solver {
668        status: "did not converge within the iteration budget".into(),
669    })
670}
671
672fn resid_dual(
673    p: &[Vec<f64>],
674    q: &[f64],
675    a_eq: &[Vec<f64>],
676    y: &[f64],
677    a: &[Vec<f64>],
678    z: &[f64],
679    x: &[f64],
680) -> Vec<f64> {
681    let n = x.len();
682    (0..n)
683        .map(|i| {
684            let mut v = q[i];
685            for k in 0..n {
686                v += p[i][k] * x[k];
687            }
688            for e in 0..a_eq.len() {
689                v += a_eq[e][i] * y[e];
690            }
691            for j in 0..a.len() {
692                v += a[j][i] * z[j];
693            }
694            v
695        })
696        .collect()
697}
698
699fn resid_pri_eq(a_eq: &[Vec<f64>], x: &[f64], b_eq: &[f64]) -> Vec<f64> {
700    a_eq.iter()
701        .enumerate()
702        .map(|(e, row)| dot(row, x) - b_eq[e])
703        .collect()
704}
705
706fn resid_pri2(a: &[Vec<f64>], x: &[f64], s: &[f64], b: &[f64]) -> Vec<f64> {
707    a.iter()
708        .enumerate()
709        .map(|(j, row)| dot(row, x) + s[j] - b[j])
710        .collect()
711}
712
713fn norm(v: &[f64]) -> f64 {
714    v.iter().map(|x| x * x).sum::<f64>().sqrt()
715}
716
717#[cfg(test)]
718mod tests {
719    use super::*;
720    use tpt_math_linalg_dense::{DMatrix, DVector};
721
722    fn dv(xs: &[f64]) -> DVector<f64> {
723        DVector::from_vec(xs.to_vec())
724    }
725
726    fn dm(rows: usize, cols: usize, xs: &[f64]) -> DMatrix<f64> {
727        DMatrix::from_row_slice(rows, cols, xs)
728    }
729
730    #[test]
731    fn test_minimize_x2_eq_1() {
732        // minimize x²  s.t. x ≥ 1  ->  x = 1
733        let p = dm(1, 1, &[2.0]);
734        let q = dv(&[0.0]);
735        let a_eq = dm(0, 1, &[]);
736        let b_eq = dv(&[]);
737        let x = solve_qp(&p, &q, &a_eq, &b_eq, &[(1.0, f64::INFINITY)]).unwrap();
738        assert!((x[0] - 1.0).abs() < 1e-6, "got x = {}", x[0]);
739    }
740
741    #[test]
742    fn test_minimize_x2_y2_eq_sum() {
743        // minimize x² + y²  s.t. x + y = 1  ->  (0.5, 0.5)
744        let p = dm(2, 2, &[2.0, 0.0, 0.0, 2.0]);
745        let q = dv(&[0.0, 0.0]);
746        let a_eq = dm(1, 2, &[1.0, 1.0]);
747        let b_eq = dv(&[1.0]);
748        let x = solve_qp(&p, &q, &a_eq, &b_eq, &[]).unwrap();
749        assert!(
750            (x[0] - 0.5).abs() < 1e-6 && (x[1] - 0.5).abs() < 1e-6,
751            "got x = {:?}",
752            x
753        );
754    }
755
756    #[test]
757    fn test_minimize_with_inequality() {
758        // minimize x² + y²  s.t. x ≥ 1  ->  x = 1, y = 0
759        let p = dm(2, 2, &[2.0, 0.0, 0.0, 2.0]);
760        let q = dv(&[0.0, 0.0]);
761        let a_eq = dm(0, 2, &[]);
762        let b_eq = dv(&[]);
763        let x = solve_qp(
764            &p,
765            &q,
766            &a_eq,
767            &b_eq,
768            &[(1.0, f64::INFINITY), (0.0, f64::INFINITY)],
769        )
770        .unwrap();
771        assert!(
772            (x[0] - 1.0).abs() < 1e-4 && (x[1] - 0.0).abs() < 1e-4,
773            "got x = {:?}",
774            x
775        );
776    }
777
778    #[test]
779    fn test_builder_twoside_bounds() {
780        // minimize x²  s.t. 2 ≤ x ≤ 3  ->  x = 2
781        let qp = QuadraticProgram::new(dv(&[0.0]))
782            .objective(dm(1, 1, &[2.0]))
783            .bounds(&[(2.0, 3.0)]);
784        let sol = qp.solve().unwrap();
785        assert!((sol.x[0] - 2.0).abs() < 1e-6, "got x = {}", sol.x[0]);
786    }
787
788    #[test]
789    fn test_inequality_constraints() {
790        // minimize x² + y²  s.t. x + y ≤ 1, x ≥ 0, y ≥ 0
791        // unconstrained min (0,0) already feasible -> (0, 0)
792        let p = dm(2, 2, &[2.0, 0.0, 0.0, 2.0]);
793        let q = dv(&[0.0, 0.0]);
794        let a_ineq = dm(1, 2, &[1.0, 1.0]);
795        let b_ineq = dv(&[1.0]);
796        let sol =
797            solve_qp_internal(&p, &q, &dm(0, 2, &[]), &dv(&[]), &a_ineq, &b_ineq, &[]).unwrap();
798        assert!(
799            sol.x[0].abs() < 1e-6 && sol.x[1].abs() < 1e-6,
800            "got x = {:?}",
801            sol.x
802        );
803    }
804
805    #[test]
806    fn test_dimension_error() {
807        let p = dm(2, 2, &[2.0, 0.0, 0.0, 2.0]);
808        let q = dv(&[0.0]); // wrong length
809        let a_eq = dm(1, 2, &[1.0, 1.0]);
810        let b_eq = dv(&[1.0]);
811        let err = solve_qp(&p, &q, &a_eq, &b_eq, &[]).unwrap_err();
812        assert!(matches!(err, ConvexError::DimensionMismatch { .. }));
813    }
814
815    #[test]
816    fn test_nonfinite_error() {
817        let p = dm(2, 2, &[2.0, 0.0, 0.0, 2.0]);
818        let q = dv(&[f64::NAN, 0.0]);
819        let a_eq = dm(1, 2, &[1.0, 1.0]);
820        let b_eq = dv(&[1.0]);
821        let err = solve_qp(&p, &q, &a_eq, &b_eq, &[]).unwrap_err();
822        assert!(matches!(err, ConvexError::NotFinite { .. }));
823    }
824
825    #[test]
826    fn test_infeasible_qp() {
827        // x ≥ 2 and x ≤ 1: empty feasible set.
828        let p = dm(1, 1, &[2.0]);
829        let q = dv(&[0.0]);
830        let a_eq = dm(0, 1, &[]);
831        let b_eq = dv(&[]);
832        let x = solve_qp(&p, &q, &a_eq, &b_eq, &[(2.0, 1.0)]);
833        assert!(x.is_err(), "expected infeasible error, got {:?}", x);
834    }
835
836    #[test]
837    fn test_unbounded_qp() {
838        // minimize -x  s.t. x ≥ 1: objective unbounded below as x → +∞.
839        let p = dm(1, 1, &[0.0]);
840        let q = dv(&[-1.0]);
841        let a_eq = dm(0, 1, &[]);
842        let b_eq = dv(&[]);
843        let x = solve_qp(&p, &q, &a_eq, &b_eq, &[(1.0, f64::INFINITY)]);
844        assert!(x.is_err(), "expected unbounded error, got {:?}", x);
845    }
846
847    #[test]
848    fn test_random_qp_kkt_conditions() {
849        // Random PSD P, random q, box bounds. Verify KKT conditions at the
850        // returned optimum (a cross-check that the solve is correct).
851        let n = 5;
852        let mut rng_state: u64 = 0x1234_5678_9abc_def0;
853        let mut rng = || {
854            // xorshift64*
855            rng_state ^= rng_state >> 12;
856            rng_state ^= rng_state << 25;
857            rng_state ^= rng_state >> 27;
858            ((rng_state.wrapping_mul(0x2545_f491_4f6c_dd1d)) >> 11) as f64 / (1u64 << 53) as f64
859        };
860
861        // Build PSD P = Mᵀ M + I.
862        let mut m = vec![vec![0.0_f64; n]; n];
863        for i in 0..n {
864            for k in 0..n {
865                m[i][k] = rng() * 2.0 - 1.0;
866            }
867        }
868        let mut p = vec![vec![0.0_f64; n]; n];
869        for i in 0..n {
870            for j in 0..n {
871                let mut v = if i == j { 1.0 } else { 0.0 };
872                for k in 0..n {
873                    v += m[k][i] * m[k][j];
874                }
875                p[i][j] = v;
876            }
877        }
878        let q: Vec<f64> = (0..n).map(|_| rng() * 2.0 - 1.0).collect();
879        let p_mat = dm(n, n, &p.iter().flatten().copied().collect::<Vec<_>>());
880        let q_vec = dv(&q);
881
882        let bounds: Vec<(f64, f64)> = (0..n).map(|_| (-5.0, 5.0)).collect();
883        let sol = solve_qp(&p_mat, &q_vec, &dm(0, n, &[]), &dv(&[]), &bounds).unwrap();
884        let x = sol.iter().copied().collect::<Vec<_>>();
885
886        // Stationarity: P x + q + Aᵀ z = 0 on the interior of active bounds.
887        // For each variable, if strictly inside the box, the gradient must be 0.
888        for i in 0..n {
889            let mut grad = q[i];
890            for k in 0..n {
891                grad += p[i][k] * x[k];
892            }
893            let inside = x[i] > -5.0 + 1e-6 && x[i] < 5.0 - 1e-6;
894            if inside {
895                assert!(
896                    grad.abs() < 1e-4,
897                    "variable {i} interior but gradient = {grad} (x = {})",
898                    x[i]
899                );
900            } else {
901                // At a bound: sign of gradient must push back into the feasible
902                // region (complementarity).
903                let at_lower = (x[i] + 5.0_f64).abs() < 1e-4;
904                let at_upper = (5.0_f64 - x[i]).abs() < 1e-4;
905                if at_lower {
906                    assert!(grad > -1e-4, "at lower bound but gradient = {grad}");
907                }
908                if at_upper {
909                    assert!(grad < 1e-4, "at upper bound but gradient = {grad}");
910                }
911            }
912        }
913        // Feasibility.
914        for &xi in &x {
915            assert!(
916                (-5.0 - 1e-6..=5.0 + 1e-6).contains(&xi),
917                "infeasible x = {xi}"
918            );
919        }
920    }
921
922    #[test]
923    fn badly_scaled_problem_reports_solved_status() {
924        // min 1e6 (x - 1)² + 1e6 (y + 2)² with bounds. The data are large, so
925        // absolute residual tolerances were never met with the scaled problem.
926        let p = DMatrix::from_row_slice(2, 2, &[2e6, 0.0, 0.0, 2e6]);
927        let q = dv(&[-2e6, 4e6]);
928        let a_ineq = DMatrix::<f64>::zeros(0, 2);
929        let b_ineq = DVector::<f64>::zeros(0);
930        let a_eq = DMatrix::<f64>::zeros(0, 2);
931        let b_eq = DVector::<f64>::zeros(0);
932        let bounds = [(-10.0, 10.0), (-10.0, 10.0)];
933        let sol = solve_qp_internal(&p, &q, &a_eq, &b_eq, &a_ineq, &b_ineq, &bounds).unwrap();
934        assert_eq!(sol.status, QpStatus::Solved);
935        assert!((sol.x[0] - 1.0).abs() < 1e-6, "x = {}", sol.x[0]);
936        assert!((sol.x[1] + 2.0).abs() < 1e-6, "y = {}", sol.x[1]);
937    }
938}
939
940#[cfg(test)]
941mod iteration_count_tests {
942    use super::*;
943
944    #[test]
945    fn solution_reports_iterations_within_budget() {
946        // min ½(x₀² + x₁²) − x₀ − x₁ with x ≥ 0 has the optimum (1, 1).
947        let p = DMatrix::from_row_slice(2, 2, &[1.0, 0.0, 0.0, 1.0]);
948        let q = DVector::from_vec(vec![-1.0, -1.0]);
949        let a_eq = DMatrix::<f64>::zeros(0, 2);
950        let b_eq = DVector::<f64>::zeros(0);
951        let a_ineq = DMatrix::<f64>::zeros(0, 2);
952        let b_ineq = DVector::<f64>::zeros(0);
953        let bounds = [(0.0, f64::INFINITY), (0.0, f64::INFINITY)];
954        let sol = solve_qp_internal(&p, &q, &a_eq, &b_eq, &a_ineq, &b_ineq, &bounds).unwrap();
955        assert!(sol.iterations > 0 && sol.iterations <= 200);
956        assert!((sol.x[0] - 1.0).abs() < 1e-6 && (sol.x[1] - 1.0).abs() < 1e-6);
957    }
958}