1use std::ops::{Add, Div, Mul, Neg, Sub};
9
10use num_complex::Complex64;
11
12pub trait DenseScalar:
15 Copy
16 + PartialEq
17 + Add<Output = Self>
18 + Sub<Output = Self>
19 + Mul<Output = Self>
20 + Div<Output = Self>
21 + Neg<Output = Self>
22{
23 const ZERO: Self;
25 const ONE: Self;
27 fn magnitude(self) -> f64;
29 fn is_finite(self) -> bool;
31 fn unscaled_magnitude(self) -> f64;
35 #[must_use]
38 fn reciprocal(self) -> Self;
39 #[must_use]
43 fn ratio_quotient(self, divisor: Self) -> Self;
44}
45
46impl DenseScalar for f64 {
47 const ZERO: Self = 0.0;
48 const ONE: Self = 1.0;
49 fn magnitude(self) -> f64 {
50 self.abs()
51 }
52 fn is_finite(self) -> bool {
53 f64::is_finite(self)
54 }
55 fn unscaled_magnitude(self) -> f64 {
56 self.abs()
57 }
58 fn reciprocal(self) -> Self {
59 1.0 / self
60 }
61 fn ratio_quotient(self, divisor: Self) -> Self {
62 self / divisor
63 }
64}
65
66impl DenseScalar for Complex64 {
67 const ZERO: Self = Complex64::new(0.0, 0.0);
68 const ONE: Self = Complex64::new(1.0, 0.0);
69 fn magnitude(self) -> f64 {
70 self.norm()
71 }
72 fn is_finite(self) -> bool {
73 self.re.is_finite() && self.im.is_finite()
74 }
75 fn unscaled_magnitude(self) -> f64 {
76 self.norm_sqr().sqrt()
77 }
78 fn reciprocal(self) -> Self {
79 self.finv()
80 }
81 fn ratio_quotient(self, divisor: Self) -> Self {
82 if divisor.re.abs() > divisor.im.abs() {
83 let ratio = divisor.im / divisor.re;
84 let scale = divisor.re + divisor.im * ratio;
85 Complex64::new(
86 (self.re + self.im * ratio) / scale,
87 (self.im - self.re * ratio) / scale,
88 )
89 } else {
90 let ratio = divisor.re / divisor.im;
91 let scale = divisor.im + divisor.re * ratio;
92 Complex64::new(
93 (self.im + self.re * ratio) / scale,
94 (-self.re + self.im * ratio) / scale,
95 )
96 }
97 }
98}
99
100#[derive(Clone, Copy, Debug, PartialEq, Eq)]
102#[non_exhaustive]
103pub enum DensePivot {
104 Partial,
109 OpenDssDiagonal,
126}
127
128#[derive(Clone, Copy, Debug, PartialEq, Eq)]
130#[non_exhaustive]
131pub enum DenseRefusal {
132 Singular,
136 NonFinite,
139}
140
141#[must_use]
168pub fn invert_dense_matrix<T: DenseScalar>(
169 matrix: &[Vec<T>],
170 pivot: DensePivot,
171 singular_pivot: f64,
172 relative_pivot: f64,
173) -> Option<Vec<Vec<T>>> {
174 eliminate_dense_matrix(matrix, pivot, singular_pivot, relative_pivot).ok()
175}
176
177pub fn eliminate_dense_matrix<T: DenseScalar>(
185 matrix: &[Vec<T>],
186 pivot: DensePivot,
187 singular_pivot: f64,
188 relative_pivot: f64,
189) -> Result<Vec<Vec<T>>, DenseRefusal> {
190 let n = matrix.len();
191 if matrix.iter().any(|row| row.len() != n) {
192 return Err(DenseRefusal::Singular);
193 }
194 let floors: Vec<f64> = (0..n)
195 .map(|column| {
196 let scale = matrix
197 .iter()
198 .fold(0.0_f64, |largest, row| largest.max(row[column].magnitude()));
199 singular_pivot.max(relative_pivot * scale)
200 })
201 .collect();
202 let check = |magnitude: f64, column: usize| {
203 if !magnitude.is_finite() && (pivot == DensePivot::Partial || magnitude.is_nan()) {
206 Err(DenseRefusal::NonFinite)
207 } else if magnitude <= floors[column] {
208 Err(DenseRefusal::Singular)
209 } else {
210 Ok(())
211 }
212 };
213 if let [row] = matrix
214 && let [value] = row.as_slice()
215 {
216 let magnitude = match pivot {
217 DensePivot::Partial => value.magnitude(),
218 DensePivot::OpenDssDiagonal => value.unscaled_magnitude(),
219 };
220 if magnitude.is_nan() {
221 return Err(DenseRefusal::Singular);
222 }
223 check(magnitude, 0)?;
224 return Ok(vec![vec![value.reciprocal()]]);
225 }
226 match pivot {
227 DensePivot::Partial => eliminate_partial(matrix, check),
228 DensePivot::OpenDssDiagonal => eliminate_diagonal(matrix, check),
229 }
230}
231
232fn eliminate_partial<T: DenseScalar>(
233 matrix: &[Vec<T>],
234 check: impl Fn(f64, usize) -> Result<(), DenseRefusal>,
235) -> Result<Vec<Vec<T>>, DenseRefusal> {
236 let n = matrix.len();
237 let mut work: Vec<Vec<T>> = matrix.to_vec();
238 let mut inverse: Vec<Vec<T>> = (0..n)
239 .map(|row| {
240 (0..n)
241 .map(|column| if row == column { T::ONE } else { T::ZERO })
242 .collect()
243 })
244 .collect();
245 for pivot in 0..n {
246 let mut best = pivot;
247 for row in pivot + 1..n {
248 if work[row][pivot].magnitude() > work[best][pivot].magnitude() {
249 best = row;
250 }
251 }
252 check(work[best][pivot].magnitude(), pivot)?;
253 work.swap(pivot, best);
254 inverse.swap(pivot, best);
255 let lead = work[pivot][pivot];
256 for column in 0..n {
257 work[pivot][column] = work[pivot][column] / lead;
258 inverse[pivot][column] = inverse[pivot][column] / lead;
259 }
260 for row in 0..n {
261 if row == pivot {
262 continue;
263 }
264 let factor = work[row][pivot];
265 if factor == T::ZERO {
266 continue;
267 }
268 for column in 0..n {
269 let (lead_work, lead_inverse) = (work[pivot][column], inverse[pivot][column]);
270 work[row][column] = work[row][column] - factor * lead_work;
271 inverse[row][column] = inverse[row][column] - factor * lead_inverse;
272 }
273 }
274 }
275 Ok(inverse)
276}
277
278fn eliminate_diagonal<T: DenseScalar>(
280 matrix: &[Vec<T>],
281 check: impl Fn(f64, usize) -> Result<(), DenseRefusal>,
282) -> Result<Vec<Vec<T>>, DenseRefusal> {
283 let n = matrix.len();
284 let mut a: Vec<Vec<T>> = matrix.to_vec();
285 let mut used = vec![false; n];
286 for _ in 0..n {
287 let mut selected = None;
291 let mut largest = 0.0;
292 for (index, done) in used.iter().enumerate() {
293 let magnitude = a[index][index].unscaled_magnitude();
294 if !done && magnitude > largest {
295 largest = magnitude;
296 selected = Some(index);
297 }
298 }
299 let k = selected.ok_or(DenseRefusal::Singular)?;
300 check(largest, k)?;
301 used[k] = true;
302 for i in (0..n).filter(|&i| i != k) {
303 for j in (0..n).filter(|&j| j != k) {
304 let correction = (a[i][k] * a[k][j]).ratio_quotient(a[k][k]);
305 a[i][j] = a[i][j] - correction;
306 if !used[i] && !used[j] && !a[i][j].is_finite() {
308 return Err(DenseRefusal::NonFinite);
309 }
310 }
311 }
312 let lead = -a[k][k].reciprocal();
313 a[k][k] = lead;
314 for i in (0..n).filter(|&i| i != k) {
315 a[i][k] = a[i][k] * lead;
316 a[k][i] = a[k][i] * lead;
317 }
318 }
319 for row in &mut a {
320 for value in row.iter_mut() {
321 *value = -*value;
322 }
323 }
324 Ok(a)
325}
326
327#[cfg(test)]
328mod tests {
329 use super::*;
330
331 #[test]
332 fn inverts_real_and_complex_matrices_and_refuses_singular_pivots() {
333 let real = invert_dense_matrix(
334 &[vec![0.0, 2.0], vec![4.0, 0.0]],
335 DensePivot::Partial,
336 0.0,
337 0.0,
338 )
339 .unwrap();
340 assert_eq!(real, vec![vec![0.0, 0.25], vec![0.5, 0.0]]);
341 let z = Complex64::new(1.0, 2.0);
342 let complex = invert_dense_matrix(&[vec![z]], DensePivot::Partial, 0.0, 0.0).unwrap();
343 assert_eq!(complex[0][0], z.finv());
344 assert!(
345 invert_dense_matrix(
346 &[vec![1.0, 2.0], vec![2.0, 4.0]],
347 DensePivot::Partial,
348 0.0,
349 0.0
350 )
351 .is_none()
352 );
353 assert!(invert_dense_matrix(&[vec![1e-15]], DensePivot::Partial, 1e-14, 0.0).is_none());
354 assert!(invert_dense_matrix(&[vec![1.0, 2.0]], DensePivot::Partial, 0.0, 0.0).is_none());
355 assert_eq!(
356 invert_dense_matrix::<f64>(&[], DensePivot::Partial, 0.0, 0.0),
357 Some(Vec::new())
358 );
359 assert!(
361 invert_dense_matrix(&[vec![f64::INFINITY]], DensePivot::Partial, 0.0, 0.0).is_none()
362 );
363 assert!(invert_dense_matrix(&[vec![f64::NAN]], DensePivot::Partial, 0.0, 0.0).is_none());
364 let noisy = [vec![1.0, 1.0], vec![1.0, 1.0 + f64::EPSILON]];
367 assert!(invert_dense_matrix(&noisy, DensePivot::Partial, 0.0, 0.0).is_some());
368 assert!(
369 invert_dense_matrix(&noisy, DensePivot::Partial, 0.0, 2.0 * f64::EPSILON).is_none()
370 );
371 }
372
373 #[test]
374 fn the_opendss_diagonal_sweep_inverts_and_refuses_as_the_engine_does() {
375 let c = Complex64::new;
376 let matrix = vec![
377 vec![c(0.3, 1.1), c(0.1, 0.4), c(0.05, 0.3)],
378 vec![c(0.1, 0.4), c(0.35, 1.0), c(0.1, 0.35)],
379 vec![c(0.05, 0.3), c(0.1, 0.35), c(0.4, 1.2)],
380 ];
381 let partial = invert_dense_matrix(&matrix, DensePivot::Partial, 0.0, 0.0).unwrap();
382 let diagonal = invert_dense_matrix(&matrix, DensePivot::OpenDssDiagonal, 0.0, 0.0).unwrap();
383 for (p, d) in partial.iter().flatten().zip(diagonal.iter().flatten()) {
384 assert!((p - d).norm() < 1e-12, "{p} {d}");
385 }
386 let crossed = vec![
390 vec![c(0.0, 0.0), c(1.0, 1.0)],
391 vec![c(1.0, 1.0), c(0.0, 0.0)],
392 ];
393 assert!(invert_dense_matrix(&crossed, DensePivot::Partial, 0.0, 0.0).is_some());
394 assert_eq!(
395 eliminate_dense_matrix(&crossed, DensePivot::OpenDssDiagonal, 0.0, 0.0),
396 Err(DenseRefusal::Singular)
397 );
398 let noisy = [vec![1.0, 1.0], vec![1.0, 1.0 + f64::EPSILON]];
400 assert!(invert_dense_matrix(&noisy, DensePivot::OpenDssDiagonal, 0.0, 0.0).is_some());
401 assert_eq!(
404 eliminate_dense_matrix(
405 &[vec![f64::INFINITY]],
406 DensePivot::OpenDssDiagonal,
407 0.0,
408 0.0
409 ),
410 Ok(vec![vec![0.0]])
411 );
412 let huge = c(1e200, 1.0);
413 assert!(huge.unscaled_magnitude().is_infinite());
414 assert!(
415 invert_dense_matrix(&[vec![huge]], DensePivot::OpenDssDiagonal, 0.0, 0.0).is_some()
416 );
417 let two = vec![vec![huge, c(1.0, 0.0)], vec![c(1.0, 0.0), c(1.0, 1.0)]];
418 assert!(invert_dense_matrix(&two, DensePivot::OpenDssDiagonal, 0.0, 0.0).is_some());
419 assert!(invert_dense_matrix(&two, DensePivot::Partial, 0.0, 0.0).is_some());
420 assert_eq!(
421 eliminate_dense_matrix(&[vec![f64::NAN]], DensePivot::OpenDssDiagonal, 0.0, 0.0),
422 Err(DenseRefusal::Singular)
423 );
424 assert_eq!(
425 eliminate_dense_matrix(&[vec![2.0]], DensePivot::OpenDssDiagonal, 0.0, 0.0),
426 Ok(vec![vec![0.5]])
427 );
428 }
429}