conspire/math/matrix/square/ldl/
mod.rs

1#[cfg(test)]
2mod test;
3
4use super::{SquareMatrix, SquareMatrixError};
5use crate::{
6    ABS_TOL, REL_TOL,
7    math::{Scalar, Tensor, Vector, simd},
8};
9
10impl SquareMatrix {
11    /// Factorize the symmetric matrix using the LDLᵀ decomposition.
12    pub fn factorize_ldl(&self) -> Result<LdlDecomposition, SquareMatrixError> {
13        let mut decomposition = LdlDecomposition {
14            ldl: self.clone(),
15            permutation: (0..self.len()).collect(),
16            pair: vec![false; self.len()],
17            column: vec![0.0; self.len()],
18            other: vec![0.0; self.len()],
19        };
20        decomposition.factorize()?;
21        Ok(decomposition)
22    }
23    /// Factorize the symmetric matrix into an existing LDLᵀ decomposition of the same size.
24    pub fn factorize_ldl_into(
25        &self,
26        decomposition: &mut LdlDecomposition,
27    ) -> Result<(), SquareMatrixError> {
28        let LdlDecomposition {
29            ldl, permutation, ..
30        } = decomposition;
31        ldl.iter_mut().zip(self.iter()).for_each(|(ldl_i, self_i)| {
32            ldl_i
33                .iter_mut()
34                .zip(self_i.iter())
35                .for_each(|(a, b)| *a = *b)
36        });
37        permutation
38            .iter_mut()
39            .enumerate()
40            .for_each(|(i, permutation_i)| *permutation_i = i);
41        decomposition.factorize()
42    }
43    /// Solve a system of linear equations using the LDLᵀ decomposition.
44    pub fn solve_ldl(&self, b: &Vector) -> Result<Vector, SquareMatrixError> {
45        Ok(self.factorize_ldl()?.solve(b))
46    }
47}
48
49/// The LDLᵀ decomposition of a symmetric matrix.
50pub struct LdlDecomposition {
51    ldl: SquareMatrix,
52    permutation: Vec<usize>,
53    pair: Vec<bool>,
54    column: Vec<Scalar>,
55    other: Vec<Scalar>,
56}
57
58/// The Bunch-Kaufman threshold, balancing the growth a one-by-one pivot
59/// admits against that of a two-by-two.
60const BUNCH_KAUFMAN: Scalar = 0.640_388_203_202_207_8;
61
62impl LdlDecomposition {
63    /// An unfactorized decomposition sized to hold that of a matrix of the given length.
64    pub fn zero(len: usize) -> Self {
65        Self {
66            ldl: SquareMatrix::zero(len),
67            permutation: (0..len).collect(),
68            pair: vec![false; len],
69            column: vec![0.0; len],
70            other: vec![0.0; len],
71        }
72    }
73    /// The entry of the symmetric matrix held only in the lower triangle.
74    fn at(&self, i: usize, j: usize) -> Scalar {
75        if i >= j {
76            self.ldl[i][j]
77        } else {
78            self.ldl[j][i]
79        }
80    }
81    /// Symmetrically permute two indices, the lower triangle making each of the
82    /// four affected stretches its own.
83    fn exchange(&mut self, p: usize, q: usize) {
84        if p == q {
85            return;
86        }
87        let (p, q) = (p.min(q), p.max(q));
88        for j in 0..p {
89            let temp = self.ldl[p][j];
90            self.ldl[p][j] = self.ldl[q][j];
91            self.ldl[q][j] = temp
92        }
93        for i in p + 1..q {
94            let temp = self.ldl[i][p];
95            self.ldl[i][p] = self.ldl[q][i];
96            self.ldl[q][i] = temp
97        }
98        for i in q + 1..self.ldl.len() {
99            let temp = self.ldl[i][p];
100            self.ldl[i][p] = self.ldl[i][q];
101            self.ldl[i][q] = temp
102        }
103        let temp = self.ldl[p][p];
104        self.ldl[p][p] = self.ldl[q][q];
105        self.ldl[q][q] = temp;
106        self.permutation.swap(p, q)
107    }
108    fn factorize(&mut self) -> Result<(), SquareMatrixError> {
109        let n = self.ldl.len();
110        self.pair.iter_mut().for_each(|paired| *paired = false);
111        let mut largest = 0.0;
112        let mut k = 0;
113        while k < n {
114            let mut omega = 0.0;
115            let mut r = k;
116            for i in k + 1..n {
117                let candidate = self.ldl[i][k].abs();
118                if candidate > omega {
119                    omega = candidate;
120                    r = i
121                }
122            }
123            let mut block = 1;
124            if omega > 0.0 && self.ldl[k][k].abs() < BUNCH_KAUFMAN * omega {
125                let mut omega_r = 0.0;
126                for i in k..n {
127                    if i != r {
128                        omega_r = self.at(i, r).abs().max(omega_r)
129                    }
130                }
131                if self.ldl[k][k].abs() * omega_r < BUNCH_KAUFMAN * omega * omega {
132                    if self.at(r, r).abs() >= BUNCH_KAUFMAN * omega_r {
133                        self.exchange(k, r)
134                    } else {
135                        self.exchange(k + 1, r);
136                        block = 2
137                    }
138                }
139            }
140            if block == 1 {
141                let pivot = self.ldl[k][k];
142                largest = pivot.abs().max(largest);
143                if pivot.abs() < ABS_TOL && pivot.abs() <= REL_TOL * largest {
144                    return Err(SquareMatrixError::Singular);
145                }
146                (k + 1..n).for_each(|i| self.column[i] = self.ldl[i][k]);
147                (k + 1..n).for_each(|i| self.ldl[i][k] /= pivot);
148                self.update_one(k, n)
149            } else {
150                let (a, b, c) = (self.ldl[k][k], self.at(k + 1, k), self.ldl[k + 1][k + 1]);
151                let determinant = a * c - b * b;
152                largest = a.abs().max(c.abs()).max(largest);
153                if determinant.abs() < ABS_TOL && determinant.abs() <= (REL_TOL * largest).powi(2) {
154                    return Err(SquareMatrixError::Singular);
155                }
156                (k + 2..n).for_each(|i| {
157                    self.column[i] = self.ldl[i][k];
158                    self.other[i] = self.ldl[i][k + 1]
159                });
160                (k + 2..n).for_each(|i| {
161                    let (w_0, w_1) = (self.column[i], self.other[i]);
162                    self.ldl[i][k] = (c * w_0 - b * w_1) / determinant;
163                    self.ldl[i][k + 1] = (a * w_1 - b * w_0) / determinant
164                });
165                self.ldl[k + 1][k] = b;
166                self.pair[k] = true;
167                self.update_two(k, n)
168            }
169            k += block
170        }
171        Ok(())
172    }
173    /// Applies a one-by-one pivot to the trailing lower triangle.
174    fn update_one(&mut self, k: usize, n: usize) {
175        let Self { ldl, column, .. } = self;
176        let source = &column[k + 1..n];
177        let back = &mut ldl.0[k + 1..];
178        back.chunks_mut(4).enumerate().for_each(|(chunk, rows)| {
179            let base = 4 * chunk;
180            if let [a, b, c, d] = rows {
181                let u = [-a[k], -b[k], -c[k], -d[k]];
182                let common = base + 1;
183                simd::rank_one_quad(
184                    &mut a.as_mut_slice()[k + 1..k + 1 + common],
185                    &mut b.as_mut_slice()[k + 1..k + 1 + common],
186                    &mut c.as_mut_slice()[k + 1..k + 1 + common],
187                    &mut d.as_mut_slice()[k + 1..k + 1 + common],
188                    &source[..common],
189                    u,
190                );
191                b[k + 1 + common] += u[1] * source[common];
192                c[k + 1 + common] += u[2] * source[common];
193                c[k + 2 + common] += u[2] * source[common + 1];
194                d[k + 1 + common] += u[3] * source[common];
195                d[k + 2 + common] += u[3] * source[common + 1];
196                d[k + 3 + common] += u[3] * source[common + 2]
197            } else {
198                rows.iter_mut().enumerate().for_each(|(row, entries)| {
199                    let factor = entries[k];
200                    simd::axpy(
201                        &mut entries.as_mut_slice()[k + 1..k + 2 + base + row],
202                        &source[..base + row + 1],
203                        factor,
204                    )
205                })
206            }
207        })
208    }
209    /// Applies a two-by-two pivot to the trailing lower triangle.
210    fn update_two(&mut self, k: usize, n: usize) {
211        let Self {
212            ldl, column, other, ..
213        } = self;
214        let source = &column[k + 2..n];
215        let paired = &other[k + 2..n];
216        let back = &mut ldl.0[k + 2..];
217        back.chunks_mut(4).enumerate().for_each(|(chunk, rows)| {
218            let base = 4 * chunk;
219            if let [a, b, c, d] = rows {
220                let u = [-a[k], -b[k], -c[k], -d[k]];
221                let w = [-a[k + 1], -b[k + 1], -c[k + 1], -d[k + 1]];
222                let common = base + 1;
223                simd::rank_two_quad(
224                    &mut a.as_mut_slice()[k + 2..k + 2 + common],
225                    &mut b.as_mut_slice()[k + 2..k + 2 + common],
226                    &mut c.as_mut_slice()[k + 2..k + 2 + common],
227                    &mut d.as_mut_slice()[k + 2..k + 2 + common],
228                    &source[..common],
229                    &paired[..common],
230                    u,
231                    w,
232                );
233                b[k + 2 + common] += u[1] * source[common] + w[1] * paired[common];
234                c[k + 2 + common] += u[2] * source[common] + w[2] * paired[common];
235                c[k + 3 + common] += u[2] * source[common + 1] + w[2] * paired[common + 1];
236                d[k + 2 + common] += u[3] * source[common] + w[3] * paired[common];
237                d[k + 3 + common] += u[3] * source[common + 1] + w[3] * paired[common + 1];
238                d[k + 4 + common] += u[3] * source[common + 2] + w[3] * paired[common + 2]
239            } else {
240                rows.iter_mut().enumerate().for_each(|(row, entries)| {
241                    let (u, w) = (entries[k], entries[k + 1]);
242                    entries.as_mut_slice()[k + 2..k + 3 + base + row]
243                        .iter_mut()
244                        .zip(source[..base + row + 1].iter())
245                        .zip(paired[..base + row + 1].iter())
246                        .for_each(|((entry, source_j), paired_j)| {
247                            *entry -= u * source_j + w * paired_j
248                        })
249                })
250            }
251        })
252    }
253    /// Solve a system of linear equations for another right-hand side.
254    pub fn solve(&self, b: &Vector) -> Vector {
255        let mut x = Vector::zero(self.permutation.len());
256        self.solve_into(b, &mut x);
257        x
258    }
259    /// Solve a system of linear equations into an existing vector.
260    pub fn solve_into(&self, b: &Vector, x: &mut Vector) {
261        let n = self.permutation.len();
262        let mut y = Vector::zero(n);
263        self.permutation
264            .iter()
265            .zip(y.iter_mut())
266            .for_each(|(&p_i, y_i)| *y_i = b[p_i]);
267        for i in 0..n {
268            let stop = if i > 0 && self.pair[i - 1] { i - 1 } else { i };
269            let sum: Scalar = (0..stop).map(|j| self.ldl[i][j] * y[j]).sum();
270            y[i] -= sum
271        }
272        let mut k = 0;
273        while k < n {
274            if self.pair[k] {
275                let (a, b_off, c) = (self.ldl[k][k], self.ldl[k + 1][k], self.ldl[k + 1][k + 1]);
276                let determinant = a * c - b_off * b_off;
277                let (y_0, y_1) = (y[k], y[k + 1]);
278                y[k] = (c * y_0 - b_off * y_1) / determinant;
279                y[k + 1] = (a * y_1 - b_off * y_0) / determinant;
280                k += 2
281            } else {
282                y[k] /= self.ldl[k][k];
283                k += 1
284            }
285        }
286        for i in (0..n).rev() {
287            let start = if self.pair[i] { i + 2 } else { i + 1 };
288            let sum: Scalar = (start..n).map(|j| self.ldl[j][i] * y[j]).sum();
289            y[i] -= sum
290        }
291        self.permutation
292            .iter()
293            .zip(y.iter())
294            .for_each(|(&p_i, y_i)| x[p_i] = *y_i)
295    }
296}
conspire/math/matrix/square/ldl/mod.rs

conspire/math/matrix/square/ldl/
mod.rs
