1use super::{CgResult, CgStatus, SparseMatrix};
4
5pub struct SparseLu {
19 n: usize,
20 l_rows: Vec<Vec<(usize, f64)>>,
22 u_rows: Vec<Vec<(usize, f64)>>,
24 perm: Vec<usize>,
26}
27
28impl SparseLu {
29 pub fn perm(&self) -> &[usize] {
31 &self.perm
32 }
33
34 pub fn l_rows(&self) -> &[Vec<(usize, f64)>] {
36 &self.l_rows
37 }
38
39 pub fn u_rows(&self) -> &[Vec<(usize, f64)>] {
41 &self.u_rows
42 }
43
44 pub fn n(&self) -> usize {
46 self.n
47 }
48
49 pub fn verify_pa_eq_lu(&self, a: &SparseMatrix) -> f64 {
52 let n = self.n;
53 let mut a_dense = vec![vec![0.0f64; n]; n];
55 for i in 0..n {
56 for k in a.row_ptr()[i]..a.row_ptr()[i + 1] {
57 let j = a.csr_cols()[k];
58 a_dense[i][j] = a.csr_vals()[k];
59 }
60 }
61
62 let mut pa = vec![vec![0.0f64; n]; n];
64 for i in 0..n {
65 let orig_i = self.perm[i];
66 for j in 0..n {
67 pa[i][j] = a_dense[orig_i][j];
68 }
69 }
70
71 let mut lu = vec![vec![0.0f64; n]; n];
73 for i in 0..n {
74 for j in 0..n {
75 let mut sum = 0.0;
76 for k in 0..n {
77 let l_ik = if i == k {
78 1.0
79 } else if k < i {
80 self.l_rows[i]
81 .iter()
82 .find(|(c, _)| *c == k)
83 .map(|(_, v)| *v)
84 .unwrap_or(0.0)
85 } else {
86 0.0
87 };
88 let u_kj = if k <= j {
89 self.u_rows[k]
90 .iter()
91 .find(|(c, _)| *c == j)
92 .map(|(_, v)| *v)
93 .unwrap_or(0.0)
94 } else {
95 0.0
96 };
97 sum += l_ik * u_kj;
98 }
99 lu[i][j] = sum;
100 }
101 }
102
103 let mut max_diff = 0.0;
105 for i in 0..n {
106 for j in 0..n {
107 let diff = (pa[i][j] - lu[i][j]).abs();
108 if diff > max_diff {
109 max_diff = diff;
110 }
111 }
112 }
113 max_diff
114 }
115
116 pub fn factor(a: &SparseMatrix) -> Result<SparseLu, String> {
119 let n = a.n;
120 let ac = if a.compressed {
121 None
122 } else {
123 let mut m = a.clone();
124 m.compress();
125 Some(m)
126 };
127 let a_ref = ac.as_ref().unwrap_or(a);
128 let (row_ptr, cols, vals) = a_ref.csr_data();
129
130 let mut a_dense = vec![vec![0.0f64; n]; n];
134 for i in 0..n {
135 for k in row_ptr[i]..row_ptr[i + 1] {
136 let j = cols[k];
137 a_dense[i][j] = vals[k];
138 }
139 }
140
141 let mut perm = (0..n).collect::<Vec<usize>>();
143
144 let mut l = vec![vec![0.0f64; n]; n]; let mut u = vec![vec![0.0f64; n]; n];
147
148 let mut col_scales = vec![0.0f64; n];
151 for j in 0..n {
152 let mut max_abs = 0.0f64;
153 for i in 0..n {
154 max_abs = max_abs.max(a_dense[i][j].abs());
155 }
156 col_scales[j] = max_abs;
157 }
158 let _matrix_scale = col_scales.iter().fold(0.0f64, |a, &v| a.max(v));
160
161 const PIVOT_SAFETY_FACTOR: f64 = 100.0;
166
167 for k in 0..n {
169 let mut pivot_row = k;
171 let mut max_val = a_dense[k][k].abs();
172 for i in (k + 1)..n {
173 let v = a_dense[i][k].abs();
174 if v > max_val {
175 max_val = v;
176 pivot_row = i;
177 }
178 }
179
180 let col_scale = col_scales[k];
184 if !col_scale.is_finite() || col_scale == 0.0 {
185 return Err(format!(
186 "Singular matrix at column {}: column is all zeros or contains NaN/Inf (col_scale = {:.2e})",
187 k, col_scale
188 ));
189 }
190 let pivot_tol = f64::EPSILON * n as f64 * col_scale * PIVOT_SAFETY_FACTOR;
191
192 if max_val <= pivot_tol {
193 return Err(format!(
194 "Singular or near-singular matrix at column {}: max pivot = {:.2e}, tolerance = {:.2e} (col_scale = {:.2e})",
195 k, max_val, pivot_tol, col_scale
196 ));
197 }
198
199 if pivot_row != k {
201 a_dense.swap(k, pivot_row);
202 let (l_first, l_second) = if k < pivot_row {
205 l.split_at_mut(pivot_row)
206 } else {
207 l.split_at_mut(k)
208 };
209 let (row_k, row_pivot) = if k < pivot_row {
210 (&mut l_first[k], &mut l_second[0])
211 } else {
212 (&mut l_second[0], &mut l_first[k])
213 };
214 for j in 0..k {
215 std::mem::swap(&mut row_k[j], &mut row_pivot[j]);
216 }
217 perm.swap(k, pivot_row);
218 }
219
220 u[k][k] = a_dense[k][k];
222
223 for j in (k + 1)..n {
225 u[k][j] = a_dense[k][j];
226 }
227
228 let pivot = u[k][k];
230 for i in (k + 1)..n {
231 l[i][k] = a_dense[i][k] / pivot;
232 }
233
234 for i in (k + 1)..n {
236 let lik = l[i][k];
237 if lik != 0.0 {
238 for j in (k + 1)..n {
239 a_dense[i][j] -= lik * u[k][j];
240 }
241 a_dense[i][k] = 0.0; }
243 }
244 }
245
246 let mut l_rows: Vec<Vec<(usize, f64)>> = vec![Vec::new(); n];
250 let mut u_rows: Vec<Vec<(usize, f64)>> = vec![Vec::new(); n];
251
252 let mut l_row_scale = vec![0.0f64; n];
254 let mut u_row_scale = vec![0.0f64; n];
255 for i in 0..n {
256 for j in 0..i {
257 l_row_scale[i] = l_row_scale[i].max(l[i][j].abs());
258 }
259 for j in i..n {
260 u_row_scale[i] = u_row_scale[i].max(u[i][j].abs());
261 }
262 }
263
264 for i in 0..n {
265 let l_tol = l_row_scale[i] * f64::EPSILON * n as f64;
267 for j in 0..i {
268 if l[i][j].abs() > l_tol {
269 l_rows[i].push((j, l[i][j]));
270 }
271 }
272 let u_tol = u_row_scale[i] * f64::EPSILON * n as f64;
274 for j in i..n {
275 if u[i][j].abs() > u_tol {
276 u_rows[i].push((j, u[i][j]));
277 }
278 }
279 l_rows[i].sort_unstable_by_key(|e| e.0);
280 u_rows[i].sort_unstable_by_key(|e| e.0);
281 }
282
283 Ok(SparseLu {
284 n,
285 l_rows,
286 u_rows,
287 perm,
288 })
289 }
290
291 pub fn solve(&self, b: &[f64]) -> Result<Vec<f64>, String> {
294 let n = self.n;
295 if b.len() != n {
296 return Err(format!(
297 "Solve failed: RHS length {} does not match matrix size {}",
298 b.len(),
299 n
300 ));
301 }
302 for (i, &val) in b.iter().enumerate() {
304 if !val.is_finite() {
305 return Err(format!(
306 "Solve failed: RHS contains non-finite value at index {}: {}",
307 i, val
308 ));
309 }
310 }
311 let mut y = vec![0.0f64; n];
313 for i in 0..n {
314 y[i] = b[self.perm[i]];
315 }
316
317 for i in 0..n {
319 let mut sum = 0.0;
320 for &(c, v) in &self.l_rows[i] {
321 sum += v * y[c];
322 }
323 y[i] -= sum;
324 }
325
326 let mut x = vec![0.0f64; n];
328 for i in (0..n).rev() {
329 let mut sum = 0.0;
330 let mut diag = 1.0;
332 for &(c, v) in &self.u_rows[i] {
333 if c == i {
334 diag = v;
335 } else if c > i {
336 sum += v * x[c];
337 }
338 }
339 if !diag.is_finite() || diag == 0.0 {
340 return Err(format!(
341 "Solve failed: near-zero or invalid diagonal at row {}: diag = {:.2e}",
342 i, diag
343 ));
344 }
345 x[i] = (y[i] - sum) / diag;
346 }
347 Ok(x)
348 }
349}
350
351pub struct Ic0Factor {
358 n: usize,
359 ptr: Vec<usize>,
361 flat_cols: Vec<usize>,
362 flat_vals: Vec<f64>,
363}
364
365impl Ic0Factor {
366 pub fn factor(a: &SparseMatrix) -> Result<Ic0Factor, String> {
367 let n = a.n;
368 let ac = if a.compressed {
369 None
370 } else {
371 let mut m = a.clone();
372 m.compress();
373 Some(m)
374 };
375 let a_ref = ac.as_ref().unwrap_or(a);
376 let (row_ptr, cols, vals) = a_ref.csr_data();
377
378 let mut l_rows: Vec<Vec<(usize, f64)>> = vec![Vec::new(); n];
380 for i in 0..n {
381 for k in row_ptr[i]..row_ptr[i + 1] {
382 if cols[k] <= i {
383 l_rows[i].push((cols[k], vals[k]));
384 }
385 }
386 l_rows[i].sort_unstable_by_key(|e| e.0);
387 }
388
389 for i in 0..n {
394 let len_i = l_rows[i].len();
395 for m in 0..len_i {
396 let (k, _) = l_rows[i][m];
397 if k >= i {
398 break;
399 }
400 let dk = l_rows[k].last().map(|&(_, v)| v).unwrap_or(1.0);
401 if dk.abs() < 1e-300 {
402 return Err(format!("IC(0): zero pivot at row {k}"));
403 }
404 l_rows[i][m].1 /= dk;
405 let l_ik = l_rows[i][m].1;
406 if l_ik == 0.0 {
407 continue;
408 }
409 let krow: Vec<(usize, f64)> = l_rows[k].clone();
410 for &(cj, lj) in krow.iter() {
411 if cj <= k || cj > i {
412 continue;
413 }
414 if let Some(pos) = l_rows[i].iter().position(|&(cc, _)| cc == cj) {
416 l_rows[i][pos].1 -= l_ik * lj;
417 }
418 }
419 }
420
421 let diag_val = l_rows[i].last().map(|&(_, v)| v).unwrap_or(0.0);
423 let d = diag_val;
424 if !(d > 0.0) || !d.is_finite() {
425 return Err(format!("IC(0) breakdown at row {i}: non-positive diagonal"));
426 }
427 let last = l_rows[i].len() - 1;
428 debug_assert_eq!(l_rows[i][last].0, i);
429 l_rows[i][last].1 = d.sqrt();
430 }
431
432 let mut ptr = vec![0usize; n + 1];
434 let mut flat_cols = Vec::new();
435 let mut flat_vals = Vec::new();
436 for i in 0..n {
437 ptr[i + 1] = ptr[i] + l_rows[i].len();
438 for &(c, v) in &l_rows[i] {
439 flat_cols.push(c);
440 flat_vals.push(v);
441 }
442 }
443
444 Ok(Ic0Factor {
445 n,
446 ptr,
447 flat_cols,
448 flat_vals,
449 })
450 }
451
452 pub fn solve(&self, b: &[f64]) -> Vec<f64> {
454 let n = self.n;
455 let mut y = b.to_vec();
456 for i in 0..n {
458 let mut s = y[i];
459 for k in self.ptr[i]..self.ptr[i + 1] {
460 let c = self.flat_cols[k];
461 if c < i {
462 s -= self.flat_vals[k] * y[c];
463 }
464 }
465 let d = self.flat_vals[self.ptr[i + 1] - 1];
466 y[i] = s / d;
467 }
468 let mut x = y;
470 for i in (0..n).rev() {
471 let d = self.flat_vals[self.ptr[i + 1] - 1];
472 let xi = x[i] / d;
473 x[i] = xi;
474 for k in self.ptr[i]..self.ptr[i + 1] - 1 {
475 let c = self.flat_cols[k];
476 x[c] -= self.flat_vals[k] * xi;
477 }
478 }
479 x
480 }
481}
482
483pub fn iccg_solve(a: &SparseMatrix, b: &[f64], max_iter: usize, tol: f64) -> CgResult {
485 let ic = match Ic0Factor::factor(a) {
486 Ok(f) => f,
487 Err(_) => return super::cg_solve(a, b, max_iter, tol),
488 };
489
490 let n = b.len();
491 let ac = if a.compressed {
492 None
493 } else {
494 let mut m = a.clone();
495 m.compress();
496 Some(m)
497 };
498 let a_ref = ac.as_ref().unwrap_or(a);
499 let (rptr, ccols, cvals) = a_ref.csr_data();
500 let matvec = |p: &[f64], out: &mut [f64]| {
501 for row in 0..n {
502 let mut sum = 0.0;
503 for k in rptr[row]..rptr[row + 1] {
504 sum += cvals[k] * p[ccols[k]];
505 }
506 out[row] = sum;
507 }
508 };
509
510 let b_norm = b.iter().map(|v| v * v).sum::<f64>().sqrt();
511 let mut x = vec![0.0f64; n];
512 if b_norm == 0.0 {
513 return CgResult {
514 x,
515 iterations: 0,
516 residual: 0.0,
517 status: CgStatus::InvalidInput,
518 };
519 }
520
521 let mut r = b.to_vec();
522 let mut z = ic.solve(&r);
523 let mut p = z.clone();
524 let mut rz: f64 = r.iter().zip(z.iter()).map(|(&a, &b2)| a * b2).sum();
525 let mut iterations = 0;
526
527 let mut ap = vec![0.0f64; n];
528 for iter in 0..max_iter {
529 matvec(&p, &mut ap);
530 let pap: f64 = p.iter().zip(ap.iter()).map(|(&a, &b2)| a * b2).sum();
531 if pap.abs() <= 0.0 {
532 return CgResult {
533 x,
534 iterations,
535 residual: r.iter().map(|v| v * v).sum::<f64>().sqrt(),
536 status: CgStatus::Breakdown,
537 };
538 }
539 let alpha = rz / pap;
540 if !alpha.is_finite() {
541 return CgResult {
542 x,
543 iterations,
544 residual: r.iter().map(|v| v * v).sum::<f64>().sqrt(),
545 status: CgStatus::Breakdown,
546 };
547 }
548 for i in 0..n {
549 x[i] += alpha * p[i];
550 r[i] -= alpha * ap[i];
551 }
552
553 let rn = r.iter().map(|v| v * v).sum::<f64>().sqrt();
554 iterations = iter + 1;
555 if rn < tol * b_norm {
556 return CgResult {
557 x,
558 iterations,
559 residual: rn,
560 status: CgStatus::Converged,
561 };
562 }
563
564 z = ic.solve(&r);
565 let rz_new: f64 = r.iter().zip(z.iter()).map(|(&a, &b2)| a * b2).sum();
566 if rz == 0.0 || !rz.is_finite() {
567 return CgResult {
568 x,
569 iterations,
570 residual: r.iter().map(|v| v * v).sum::<f64>().sqrt(),
571 status: CgStatus::Breakdown,
572 };
573 }
574 let beta = rz_new / rz;
575 if !beta.is_finite() {
576 return CgResult {
577 x,
578 iterations,
579 residual: r.iter().map(|v| v * v).sum::<f64>().sqrt(),
580 status: CgStatus::Breakdown,
581 };
582 }
583 for i in 0..n {
584 p[i] = z[i] + beta * p[i];
585 }
586 rz = rz_new;
587 }
588
589 CgResult {
590 x,
591 iterations,
592 residual: r.iter().map(|v| v * v).sum::<f64>().sqrt(),
593 status: CgStatus::MaxIterations,
594 }
595}