1use thiserror::Error;
4
5use crate::matrix::{dot, Matrix};
6
7#[derive(Clone, Debug, PartialEq)]
9#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
10pub enum FactorCovariance {
11 Diagonal(Vec<f64>),
13 Dense(Matrix),
15}
16
17impl FactorCovariance {
18 #[must_use]
20 pub fn dimension(&self) -> usize {
21 match self {
22 Self::Diagonal(diagonal) => diagonal.len(),
23 Self::Dense(matrix) => matrix.rows(),
24 }
25 }
26}
27
28#[derive(Clone, Debug, PartialEq)]
31#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
32pub struct FactorQuad {
33 pub factors: Matrix,
35 pub omega: FactorCovariance,
37 pub diagonal: Vec<f64>,
39}
40
41impl FactorQuad {
42 pub fn new(
52 factors: Matrix,
53 omega: FactorCovariance,
54 diagonal: Vec<f64>,
55 ) -> Result<Self, ProblemError> {
56 let quadratic = Self {
57 factors,
58 omega,
59 diagonal,
60 };
61 quadratic.validate()?;
62 Ok(quadratic)
63 }
64
65 #[must_use]
67 pub fn dimension(&self) -> usize {
68 self.factors.rows()
69 }
70
71 #[must_use]
73 pub fn factor_count(&self) -> usize {
74 self.factors.cols()
75 }
76
77 #[must_use]
79 pub fn apply(&self, x: &[f64]) -> Vec<f64> {
80 debug_assert_eq!(x.len(), self.dimension());
81 let n = self.dimension();
82 let k = self.factor_count();
83 let mut factor_projection = vec![0.0; k];
84 self.factors.transpose_mul_add(x, &mut factor_projection);
85
86 let weighted = match &self.omega {
87 FactorCovariance::Diagonal(diagonal) => factor_projection
88 .iter()
89 .zip(diagonal)
90 .map(|(value, weight)| value * weight)
91 .collect(),
92 FactorCovariance::Dense(matrix) => matrix.mul_vec(&factor_projection),
93 };
94
95 let mut result: Vec<f64> = self
96 .diagonal
97 .iter()
98 .zip(x)
99 .map(|(diagonal, value)| diagonal * value)
100 .collect();
101 for (row, value) in result.iter_mut().enumerate().take(n) {
102 *value += dot(self.factors.row(row), &weighted);
103 }
104 result
105 }
106
107 fn validate(&self) -> Result<(), ProblemError> {
108 let n = self.factors.rows();
109 let k = self.factors.cols();
110 if self.diagonal.len() != n {
111 return Err(ProblemError::Dimension {
112 field: "quadratic.diagonal",
113 expected: n,
114 actual: self.diagonal.len(),
115 });
116 }
117 if self.omega.dimension() != k {
118 return Err(ProblemError::Dimension {
119 field: "quadratic.omega",
120 expected: k,
121 actual: self.omega.dimension(),
122 });
123 }
124 if self
125 .factors
126 .as_slice()
127 .iter()
128 .any(|value| !value.is_finite())
129 {
130 return Err(ProblemError::NonFinite("quadratic.factors"));
131 }
132 if self
133 .diagonal
134 .iter()
135 .any(|value| !value.is_finite() || *value < 0.0)
136 {
137 return Err(ProblemError::NonConvex(
138 "quadratic diagonal must be finite and non-negative",
139 ));
140 }
141 match &self.omega {
142 FactorCovariance::Diagonal(diagonal) => {
143 if diagonal
144 .iter()
145 .any(|value| !value.is_finite() || *value < 0.0)
146 {
147 return Err(ProblemError::NonConvex(
148 "factor covariance diagonal must be finite and non-negative",
149 ));
150 }
151 }
152 FactorCovariance::Dense(matrix) => {
153 if matrix.rows() != matrix.cols() {
154 return Err(ProblemError::NotSquare("quadratic.omega"));
155 }
156 if matrix.as_slice().iter().any(|value| !value.is_finite()) {
157 return Err(ProblemError::NonFinite("quadratic.omega"));
158 }
159 for row in 0..k {
160 for col in 0..row {
161 let scale = 1.0_f64
162 .max(matrix[(row, col)].abs())
163 .max(matrix[(col, row)].abs());
164 if (matrix[(row, col)] - matrix[(col, row)]).abs() > 1.0e-12 * scale {
165 return Err(ProblemError::NotSymmetric("quadratic.omega"));
166 }
167 }
168 }
169 }
170 }
171 Ok(())
172 }
173}
174
175#[derive(Clone, Debug, PartialEq)]
184#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
185pub struct L1Term {
186 pub costs: Vec<f64>,
188 pub anchor: Vec<f64>,
190}
191
192impl L1Term {
193 #[must_use]
195 pub fn evaluate(&self, x: &[f64]) -> f64 {
196 self.costs
197 .iter()
198 .zip(&self.anchor)
199 .zip(x)
200 .map(|((cost, anchor), value)| cost * (value - anchor).abs())
201 .sum()
202 }
203
204 fn validate(&self, dimension: usize) -> Result<(), ProblemError> {
205 if self.costs.len() != dimension {
206 return Err(ProblemError::Dimension {
207 field: "l1.costs",
208 expected: dimension,
209 actual: self.costs.len(),
210 });
211 }
212 if self.anchor.len() != dimension {
213 return Err(ProblemError::Dimension {
214 field: "l1.anchor",
215 expected: dimension,
216 actual: self.anchor.len(),
217 });
218 }
219 if self
220 .costs
221 .iter()
222 .any(|value| !value.is_finite() || *value < 0.0)
223 {
224 return Err(ProblemError::NonConvex(
225 "l1 costs must be finite and non-negative",
226 ));
227 }
228 if self.anchor.iter().any(|value| !value.is_finite()) {
229 return Err(ProblemError::NonFinite("l1.anchor"));
230 }
231 Ok(())
232 }
233}
234
235#[derive(Clone, Debug, PartialEq)]
237#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
238pub struct LinearConstraints {
239 pub matrix: Matrix,
241 pub rhs: Vec<f64>,
243}
244
245impl LinearConstraints {
246 pub fn new(matrix: Matrix, rhs: Vec<f64>) -> Result<Self, ProblemError> {
252 if matrix.rows() != rhs.len() {
253 return Err(ProblemError::Dimension {
254 field: "constraints.rhs",
255 expected: matrix.rows(),
256 actual: rhs.len(),
257 });
258 }
259 Ok(Self { matrix, rhs })
260 }
261
262 #[must_use]
264 pub fn empty(dimension: usize) -> Self {
265 Self {
266 matrix: Matrix::zeros(0, dimension),
267 rhs: Vec::new(),
268 }
269 }
270
271 #[must_use]
273 pub fn len(&self) -> usize {
274 self.rhs.len()
275 }
276
277 #[must_use]
279 pub fn is_empty(&self) -> bool {
280 self.rhs.is_empty()
281 }
282}
283
284#[derive(Clone, Debug, PartialEq)]
290#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
291pub struct QpProblem {
292 pub quadratic: FactorQuad,
294 pub linear: Vec<f64>,
296 pub l1: Option<L1Term>,
298 pub equalities: LinearConstraints,
300 pub inequalities: LinearConstraints,
302 #[cfg_attr(feature = "serde", serde(with = "crate::serde_support::lower_bounds"))]
308 pub lower_bounds: Vec<f64>,
309 #[cfg_attr(feature = "serde", serde(with = "crate::serde_support::upper_bounds"))]
315 pub upper_bounds: Vec<f64>,
316}
317
318impl QpProblem {
319 pub fn validate(&self) -> Result<(), ProblemError> {
325 self.quadratic.validate()?;
326 let n = self.quadratic.dimension();
327 for (field, actual) in [
328 ("linear", self.linear.len()),
329 ("lower_bounds", self.lower_bounds.len()),
330 ("upper_bounds", self.upper_bounds.len()),
331 ] {
332 if actual != n {
333 return Err(ProblemError::Dimension {
334 field,
335 expected: n,
336 actual,
337 });
338 }
339 }
340 for (field, constraints) in [
341 ("equalities", &self.equalities),
342 ("inequalities", &self.inequalities),
343 ] {
344 if constraints.matrix.cols() != n {
345 return Err(ProblemError::Dimension {
346 field,
347 expected: n,
348 actual: constraints.matrix.cols(),
349 });
350 }
351 if constraints.matrix.rows() != constraints.rhs.len() {
352 return Err(ProblemError::Dimension {
353 field,
354 expected: constraints.matrix.rows(),
355 actual: constraints.rhs.len(),
356 });
357 }
358 if constraints
359 .matrix
360 .as_slice()
361 .iter()
362 .chain(&constraints.rhs)
363 .any(|value| !value.is_finite())
364 {
365 return Err(ProblemError::NonFinite(field));
366 }
367 }
368 if self.linear.iter().any(|value| !value.is_finite()) {
369 return Err(ProblemError::NonFinite("linear"));
370 }
371 if let Some(l1) = &self.l1 {
372 l1.validate(n)?;
373 }
374 for index in 0..n {
375 let lower = self.lower_bounds[index];
376 let upper = self.upper_bounds[index];
377 if lower.is_nan() || upper.is_nan() {
378 return Err(ProblemError::NonFinite("bounds"));
379 }
380 if lower > upper {
381 return Err(ProblemError::InvalidBounds {
382 index,
383 lower,
384 upper,
385 });
386 }
387 }
388 Ok(())
389 }
390
391 #[must_use]
393 pub fn objective(&self, x: &[f64]) -> f64 {
394 let qx = self.quadratic.apply(x);
395 let smooth = 0.5 * dot(x, &qx) + dot(&self.linear, x);
396 smooth + self.l1.as_ref().map_or(0.0, |term| term.evaluate(x))
397 }
398}
399
400#[derive(Debug, Error, Clone, PartialEq)]
402pub enum ProblemError {
403 #[error("{field} has dimension {actual}; expected {expected}")]
405 Dimension {
406 field: &'static str,
408 expected: usize,
410 actual: usize,
412 },
413 #[error("{0} contains a non-finite coefficient")]
415 NonFinite(&'static str),
416 #[error("non-convex quadratic data: {0}")]
418 NonConvex(&'static str),
419 #[error("{0} must be square")]
421 NotSquare(&'static str),
422 #[error("{0} must be symmetric")]
424 NotSymmetric(&'static str),
425 #[error("invalid bounds at index {index}: lower {lower} exceeds upper {upper}")]
427 InvalidBounds {
428 index: usize,
430 lower: f64,
432 upper: f64,
434 },
435}