1#![allow(
2 clippy::type_complexity,
3 clippy::manual_clamp,
4 clippy::manual_range_contains,
5 clippy::needless_range_loop
6)]
7#![forbid(unsafe_code)]
57#![warn(missing_docs)]
58
59use tpt_math_linalg_dense::{DMatrix, DVector, Lu};
60
61pub use tpt_math_linalg_dense;
64
65#[derive(Debug, Clone, PartialEq)]
67#[non_exhaustive]
68pub enum ConvexError {
69 DimensionMismatch {
72 what: String,
74 },
75 NotFinite {
77 what: String,
79 },
80 Solver {
84 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#[derive(Debug, Clone, PartialEq)]
104#[non_exhaustive]
105pub enum QpStatus {
106 Solved,
108 AlmostSolved,
110 Failed(String),
112}
113
114#[derive(Debug, Clone)]
116#[non_exhaustive]
117pub struct QpSolution {
118 pub x: DVector<f64>,
120 pub objective: f64,
122 pub status: QpStatus,
124 pub iterations: usize,
126}
127
128pub 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
153pub 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 let (a, b) = build_inequality_system(a_ineq, b_ineq, bounds, n);
219
220 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#[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 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 pub fn objective(mut self, p: DMatrix<f64>) -> Self {
286 self.p = p;
287 self
288 }
289
290 pub fn linear_cost(mut self, q: DVector<f64>) -> Self {
292 self.q = q;
293 self
294 }
295
296 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 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 pub fn bounds(mut self, bounds: &[(f64, f64)]) -> Self {
312 self.bounds = bounds.to_vec();
313 self
314 }
315
316 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
330fn 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
382fn 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 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 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
421fn 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
432fn 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 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#[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
544fn 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 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 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 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 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 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 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 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 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 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]); 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 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 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 let n = 5;
852 let mut rng_state: u64 = 0x1234_5678_9abc_def0;
853 let mut rng = || {
854 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 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 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 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 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 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 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}