qualia_core_db/solvers/
polynomial.rs1use crate::solvers::SolversError;
10
11#[derive(Debug, Clone, Copy, PartialEq)]
13pub struct Complex {
14 pub re: f64,
15 pub im: f64,
16}
17
18impl Complex {
19 pub const fn new(re: f64, im: f64) -> Self {
21 Self { re, im }
22 }
23 pub const fn real(re: f64) -> Self {
25 Self { re, im: 0.0 }
26 }
27
28 #[inline]
29 pub fn add(self, o: Complex) -> Complex {
31 Complex::new(self.re + o.re, self.im + o.im)
32 }
33 #[inline]
34 pub fn sub(self, o: Complex) -> Complex {
36 Complex::new(self.re - o.re, self.im - o.im)
37 }
38 #[inline]
39 pub fn mul(self, o: Complex) -> Complex {
41 Complex::new(
42 self.re * o.re - self.im * o.im,
43 self.re * o.im + self.im * o.re,
44 )
45 }
46 #[inline]
47 pub fn div(self, o: Complex) -> Complex {
49 let d = o.re * o.re + o.im * o.im;
50 Complex::new(
51 (self.re * o.re + self.im * o.im) / d,
52 (self.im * o.re - self.re * o.im) / d,
53 )
54 }
55 #[inline]
57 pub fn abs(self) -> f64 {
58 self.re.hypot(self.im)
59 }
60 #[inline]
62 pub fn is_real(self, tol: f64) -> bool {
63 self.im.abs() <= tol
64 }
65}
66
67#[derive(Debug, Clone, Copy, PartialEq)]
69pub enum QuadraticRoots {
70 TwoReal(f64, f64),
72 DoubleReal(f64),
74 ComplexPair { re: f64, im: f64 },
76 Linear(f64),
78}
79
80pub fn solve_quadratic(a: f64, b: f64, c: f64) -> Result<QuadraticRoots, SolversError> {
87 if !(a.is_finite() && b.is_finite() && c.is_finite()) {
88 return Err(SolversError::ComputationError);
89 }
90 let scale = a.abs().max(b.abs()).max(c.abs()).max(1.0);
91
92 if a.abs() <= f64::EPSILON * scale {
94 if b.abs() <= f64::EPSILON * scale {
95 return Err(SolversError::ComputationError);
96 }
97 return Ok(QuadraticRoots::Linear(-c / b));
98 }
99
100 let disc = b * b - 4.0 * a * c;
101 let disc_scale = (b * b).max((4.0 * a * c).abs()).max(1.0);
102 if disc.abs() <= 1e-12 * disc_scale {
103 return Ok(QuadraticRoots::DoubleReal(-b / (2.0 * a)));
104 }
105
106 if disc > 0.0 {
107 let sqrt_d = disc.sqrt();
108 let sign_b = if b >= 0.0 { 1.0 } else { -1.0 };
109 let q = -0.5 * (b + sign_b * sqrt_d);
110 let r1 = q / a;
111 let r2 = c / q;
112 let (lo, hi) = if r1 <= r2 { (r1, r2) } else { (r2, r1) };
113 Ok(QuadraticRoots::TwoReal(lo, hi))
114 } else {
115 let re = -b / (2.0 * a);
116 let im = (-disc).sqrt() / (2.0 * a.abs());
117 Ok(QuadraticRoots::ComplexPair { re, im })
118 }
119}
120
121fn poly_eval_complex(coeffs: &[f64], x: Complex) -> Complex {
124 let mut acc = Complex::real(0.0);
125 for &c in coeffs {
126 acc = acc.mul(x).add(Complex::real(c));
127 }
128 acc
129}
130
131pub fn polynomial_roots(coeffs: &[f64]) -> Result<Vec<Complex>, SolversError> {
138 let start = coeffs
140 .iter()
141 .position(|c| c.abs() > 0.0)
142 .ok_or(SolversError::ComputationError)?;
143 let coeffs = &coeffs[start..];
144 if coeffs.len() == 1 {
145 return Ok(Vec::new()); }
147 if coeffs.iter().any(|c| !c.is_finite()) {
148 return Err(SolversError::ComputationError);
149 }
150
151 let lead = coeffs[0];
153 let monic: Vec<f64> = coeffs.iter().map(|c| c / lead).collect();
154 let degree = monic.len() - 1;
155
156 let seed = Complex::new(0.4, 0.9);
158 let mut roots: Vec<Complex> = (0..degree)
159 .map(|k| {
160 let mut z = Complex::real(1.0);
161 for _ in 0..k {
162 z = z.mul(seed);
163 }
164 z
165 })
166 .collect();
167
168 const MAX_ITERS: usize = 500;
169 const TOL: f64 = 1e-14;
170 for _ in 0..MAX_ITERS {
171 let mut max_delta = 0.0_f64;
172 for i in 0..degree {
173 let zi = roots[i];
174 let mut denom = Complex::real(1.0);
176 for j in 0..degree {
177 if j != i {
178 denom = denom.mul(zi.sub(roots[j]));
179 }
180 }
181 if denom.abs() == 0.0 {
182 continue; }
184 let delta = poly_eval_complex(&monic, zi).div(denom);
185 roots[i] = zi.sub(delta);
186 max_delta = max_delta.max(delta.abs());
187 }
188 if max_delta < TOL {
189 break;
190 }
191 }
192
193 for r in roots.iter_mut() {
195 if r.im.abs() < 1e-9 * (1.0 + r.re.abs()) {
196 r.im = 0.0;
197 }
198 }
199 Ok(roots)
200}
201
202#[cfg(test)]
203mod tests {
204 use super::*;
205
206 #[test]
207 fn quadratic_two_real() {
208 assert_eq!(
210 solve_quadratic(1.0, -3.0, 2.0).unwrap(),
211 QuadraticRoots::TwoReal(1.0, 2.0)
212 );
213 }
214
215 #[test]
216 fn quadratic_double_and_complex_and_linear() {
217 assert_eq!(
219 solve_quadratic(1.0, -2.0, 1.0).unwrap(),
220 QuadraticRoots::DoubleReal(1.0)
221 );
222 match solve_quadratic(1.0, 0.0, 1.0).unwrap() {
224 QuadraticRoots::ComplexPair { re, im } => {
225 assert!(re.abs() < 1e-12 && (im - 1.0).abs() < 1e-12);
226 }
227 other => panic!("expected complex pair, got {other:?}"),
228 }
229 assert_eq!(
231 solve_quadratic(0.0, 2.0, 4.0).unwrap(),
232 QuadraticRoots::Linear(-2.0)
233 );
234 }
235
236 #[test]
237 fn quadratic_rejects_degenerate() {
238 assert!(matches!(
239 solve_quadratic(0.0, 0.0, 1.0),
240 Err(SolversError::ComputationError)
241 ));
242 assert!(matches!(
243 solve_quadratic(f64::NAN, 1.0, 1.0),
244 Err(SolversError::ComputationError)
245 ));
246 }
247
248 #[test]
249 fn roots_of_known_polynomial() {
250 let roots = polynomial_roots(&[1.0, -6.0, 11.0, -6.0]).unwrap();
252 assert_eq!(roots.len(), 3);
253 let mut reals: Vec<f64> = roots.iter().map(|r| r.re).collect();
254 reals.sort_by(|a, b| a.partial_cmp(b).unwrap());
255 for (got, want) in reals.iter().zip([1.0, 2.0, 3.0]) {
256 assert!((got - want).abs() < 1e-6, "{got} != {want}");
257 }
258 assert!(roots.iter().all(|r| r.is_real(1e-6)));
259 }
260
261 #[test]
262 fn roots_complex_pair() {
263 let roots = polynomial_roots(&[1.0, 0.0, 1.0]).unwrap();
265 assert_eq!(roots.len(), 2);
266 assert!(roots.iter().any(|r| (r.im.abs() - 1.0).abs() < 1e-6));
267 }
268
269 #[test]
270 fn roots_reject_zero_polynomial() {
271 assert!(matches!(
272 polynomial_roots(&[0.0, 0.0]),
273 Err(SolversError::ComputationError)
274 ));
275 }
276}