Skip to main content

section_properties/
fea.rs

1//! Finite element analysis core — Tri6 (six-noded quadratic triangular) element.
2//!
3//! Mirrors Python `sectionproperties.analysis.fea`:
4//! shape functions, Gaussian quadrature, stress extrapolation, and element
5//! integration methods for section property computation.
6
7use crate::geometry::Point;
8use std::collections::HashMap;
9
10// ---------------------------------------------------------------------------
11// FEM Tolerances
12// ---------------------------------------------------------------------------
13
14/// Tolerance for geometric comparisons (e.g., point coincidence, edge lengths).
15pub const GEOMETRY_TOL: f64 = 1e-12;
16
17/// Tolerance for Jacobian determinant checks (degenerate element detection).
18pub const JACOBIAN_TOL: f64 = 1e-20;
19
20/// Tolerance for iterative solver convergence (CG, ICCG, etc.).
21pub const SOLVER_TOL: f64 = 1e-12;
22
23/// Base tolerance for pivot checks in direct solvers (LDL^T, Cholesky).
24/// This is multiplied by a scale factor (matrix norm) for scale-invariant behavior.
25pub const PIVOT_TOL_BASE: f64 = 1e-15;
26
27/// Base tolerance for near-zero checks in warping/stress calculations.
28/// This is multiplied by a scale factor for scale-invariant behavior.
29pub const NEAR_ZERO_TOL_BASE: f64 = 1e-15;
30
31/// @deprecated Use `PIVOT_TOL_BASE` instead. Kept for backwards compatibility.
32#[deprecated(since = "0.2.0", note = "Use PIVOT_TOL_BASE")]
33pub const PIVOT_TOL: f64 = PIVOT_TOL_BASE;
34
35/// @deprecated Use `NEAR_ZERO_TOL_BASE` instead. Kept for backwards compatibility.
36#[deprecated(since = "0.2.0", note = "Use NEAR_ZERO_TOL_BASE")]
37pub const NEAR_ZERO_TOL: f64 = NEAR_ZERO_TOL_BASE;
38
39// ---------------------------------------------------------------------------
40// Gaussian quadrature for Tri6
41// ---------------------------------------------------------------------------
42
43/// Gaussian weights and locations for `n`-point integration of a Tri6 element.
44///
45/// Returns `n` tuples of `(weight, eta, xi, zeta)` where `zeta = 1 - eta - xi`.
46/// Supports n = 1, 3, 4 and 6. The weights are normalised such that they
47/// sum to one; `shape_function()` supplies the physical element area through
48/// its Jacobian.
49pub fn gauss_points(n: usize) -> Vec<(f64, f64, f64, f64)> {
50    match n {
51        1 => vec![(1.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0)],
52        3 => vec![
53            (1.0 / 3.0, 2.0 / 3.0, 1.0 / 6.0, 1.0 / 6.0),
54            (1.0 / 3.0, 1.0 / 6.0, 2.0 / 3.0, 1.0 / 6.0),
55            (1.0 / 3.0, 1.0 / 6.0, 1.0 / 6.0, 2.0 / 3.0),
56        ],
57        4 => vec![
58            (-27.0 / 48.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0),
59            (25.0 / 48.0, 0.6, 0.2, 0.2),
60            (25.0 / 48.0, 0.2, 0.6, 0.2),
61            (25.0 / 48.0, 0.2, 0.2, 0.6),
62        ],
63        6 => {
64            // Six-point rule used by section-properties for stress recovery.
65            // The ordering is coupled to H_INV in `extrapolate_to_nodes`.
66            let root = (38.0 - 44.0 * (2.0_f64 / 5.0).sqrt()).sqrt();
67            let g1 = (8.0 - 10.0_f64.sqrt() + root) / 18.0;
68            let g2 = (8.0 - 10.0_f64.sqrt() - root) / 18.0;
69            let weight_root = (213_125.0 - 53_320.0 * 10.0_f64.sqrt()).sqrt();
70            let w1 = (620.0 + weight_root) / 3720.0;
71            let w2 = (620.0 - weight_root) / 3720.0;
72            vec![
73                (w2, 1.0 - 2.0 * g2, g2, g2),
74                (w2, g2, 1.0 - 2.0 * g2, g2),
75                (w2, g2, g2, 1.0 - 2.0 * g2),
76                (w1, g1, g1, 1.0 - 2.0 * g1),
77                (w1, 1.0 - 2.0 * g1, g1, g1),
78                (w1, g1, 1.0 - 2.0 * g1, g1),
79            ]
80        }
81        _ => panic!("gauss_points: n must be 1, 3, 4, or 6, got {n}"),
82    }
83}
84
85// ---------------------------------------------------------------------------
86// Shape functions for Tri6
87// ---------------------------------------------------------------------------
88
89/// Result of evaluating shape functions at a Gauss point.
90pub struct ShapeFunctionResult {
91    /// Shape function values N(i), i=0..6
92    pub n: [f64; 6],
93    /// Shape function derivatives B(i,j): `2`×`6`
94    /// B\[0\] = dN/dx, B\[1\] = dN/dy
95    pub b: [[f64; 6]; 2],
96    /// Jacobian determinant
97    pub j: f64,
98    /// Global x-coordinate at Gauss point
99    pub x: f64,
100    /// Global y-coordinate at Gauss point
101    pub y: f64,
102}
103
104/// Evaluate Tri6 shape functions, derivatives, Jacobian, and global coords.
105///
106/// `coords` is a 2×6 array: coords\[0\] = x-coords, coords\[1\] = y-coords.
107/// Node ordering: [v0, v1, v2, mid01, mid12, mid20].
108pub fn shape_function(coords: &[[f64; 6]; 2], gp: (f64, f64, f64)) -> ShapeFunctionResult {
109    let (eta, xi, zeta) = gp;
110
111    // Shape function values
112    let n = [
113        eta * (2.0 * eta - 1.0),
114        xi * (2.0 * xi - 1.0),
115        zeta * (2.0 * zeta - 1.0),
116        4.0 * eta * xi,
117        4.0 * xi * zeta,
118        4.0 * eta * zeta,
119    ];
120
121    // Derivatives wrt isoparametric coords [dN/deta; dN/dxi; dN/dzeta]
122    let b_iso = [
123        [4.0 * eta - 1.0, 0.0, 0.0, 4.0 * xi, 0.0, 4.0 * zeta],
124        [0.0, 4.0 * xi - 1.0, 0.0, 4.0 * eta, 4.0 * zeta, 0.0],
125        [0.0, 0.0, 4.0 * zeta - 1.0, 0.0, 4.0 * xi, 4.0 * eta],
126    ];
127
128    // Jacobian matrix (3×3):
129    // J = | 1   sum(dN/deta * x)   sum(dN/deta * y) |
130    //     | 1   sum(dN/dxi  * x)   sum(dN/dxi  * y) |
131    //     | 1   sum(dN/dzeta* x)   sum(dN/dzeta* y) |
132    let mut j_mat = [[1.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 0.0, 0.0]];
133    for k in 0..3 {
134        for i in 0..6 {
135            j_mat[k][1] += b_iso[k][i] * coords[0][i];
136            j_mat[k][2] += b_iso[k][i] * coords[1][i];
137        }
138    }
139
140    // Jacobian determinant = 0.5 * det(J)
141    let det_j = j_mat[0][0] * (j_mat[1][1] * j_mat[2][2] - j_mat[1][2] * j_mat[2][1])
142        - j_mat[0][1] * (j_mat[1][0] * j_mat[2][2] - j_mat[1][2] * j_mat[2][0])
143        + j_mat[0][2] * (j_mat[1][0] * j_mat[2][1] - j_mat[1][1] * j_mat[2][0]);
144    let jacobian = 0.5 * det_j;
145
146    // Check for degenerate element (zero or negative Jacobian)
147    // Use a very small threshold for truly degenerate elements
148    if jacobian.abs() <= JACOBIAN_TOL {
149        return ShapeFunctionResult {
150            n,
151            b: [[0.0; 6]; 2],
152            j: jacobian,
153            x: 0.0,
154            y: 0.0,
155        };
156    }
157
158    // B = tmp_array * J^{-1} * b_iso
159    // tmp_array = [[0,1,0],[0,0,1]]
160    // So B[0] = row 1 of J^{-1} * b_iso, B[1] = row 2 of J^{-1} * b_iso
161    let j_inv = inv3x3(&j_mat);
162    let mut b = [[0.0; 6]; 2];
163    for j in 0..6 {
164        for k in 0..3 {
165            b[0][j] += j_inv[1][k] * b_iso[k][j];
166            b[1][j] += j_inv[2][k] * b_iso[k][j];
167        }
168    }
169
170    // Global coordinates
171    let mut x = 0.0;
172    let mut y = 0.0;
173    for i in 0..6 {
174        x += n[i] * coords[0][i];
175        y += n[i] * coords[1][i];
176    }
177
178    ShapeFunctionResult {
179        n,
180        b,
181        j: jacobian,
182        x,
183        y,
184    }
185}
186
187/// Inverse of a 3×3 matrix.
188fn inv3x3(m: &[[f64; 3]; 3]) -> [[f64; 3]; 3] {
189    let det = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
190        - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
191        + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
192    let inv_det = 1.0 / det;
193    [
194        [
195            (m[1][1] * m[2][2] - m[1][2] * m[2][1]) * inv_det,
196            (m[0][2] * m[2][1] - m[0][1] * m[2][2]) * inv_det,
197            (m[0][1] * m[1][2] - m[0][2] * m[1][1]) * inv_det,
198        ],
199        [
200            (m[1][2] * m[2][0] - m[1][0] * m[2][2]) * inv_det,
201            (m[0][0] * m[2][2] - m[0][2] * m[2][0]) * inv_det,
202            (m[0][2] * m[1][0] - m[0][0] * m[1][2]) * inv_det,
203        ],
204        [
205            (m[1][0] * m[2][1] - m[1][1] * m[2][0]) * inv_det,
206            (m[0][1] * m[2][0] - m[0][0] * m[2][1]) * inv_det,
207            (m[0][0] * m[1][1] - m[0][1] * m[1][0]) * inv_det,
208        ],
209    ]
210}
211
212// ---------------------------------------------------------------------------
213// Extrapolation from Gauss points to nodes
214// ---------------------------------------------------------------------------
215
216/// Extrapolation matrix H^{-1} for 6-point Gauss → 6 nodes.
217const H_INV: [[f64; 6]; 6] = [
218    [
219        1.87365927351160,
220        0.138559587411935,
221        0.138559587411935,
222        -0.638559587411936,
223        0.126340726488397,
224        -0.638559587411935,
225    ],
226    [
227        0.138559587411935,
228        1.87365927351160,
229        0.138559587411935,
230        -0.638559587411935,
231        -0.638559587411935,
232        0.126340726488397,
233    ],
234    [
235        0.138559587411935,
236        0.138559587411935,
237        1.87365927351160,
238        0.126340726488396,
239        -0.638559587411935,
240        -0.638559587411935,
241    ],
242    [
243        0.0749010751157440,
244        0.0749010751157440,
245        0.180053080734478,
246        1.36051633430762,
247        -0.345185782636792,
248        -0.345185782636792,
249    ],
250    [
251        0.180053080734478,
252        0.0749010751157440,
253        0.0749010751157440,
254        -0.345185782636792,
255        1.36051633430762,
256        -0.345185782636792,
257    ],
258    [
259        0.0749010751157440,
260        0.180053080734478,
261        0.0749010751157440,
262        -0.345185782636792,
263        -0.345185782636792,
264        1.36051633430762,
265    ],
266];
267
268/// Extrapolate results at six Gauss points to the six nodes of a Tri6 element.
269pub fn extrapolate_to_nodes(w: &[f64; 6]) -> [f64; 6] {
270    let mut result = [0.0; 6];
271    for i in 0..6 {
272        for j in 0..6 {
273            result[i] += H_INV[i][j] * w[j];
274        }
275    }
276    result
277}
278
279// ---------------------------------------------------------------------------
280// Coordinate transformations
281// ---------------------------------------------------------------------------
282
283/// Convert global (centroidal) coordinates to principal coordinates.
284///
285/// `phi` is the principal axis angle in **radians** — the angle from the
286/// centroidal x-axis to axis 11 (major principal), CCW positive.
287///
288/// ```text
289/// x₁₁ = (x − x_c)·cos(φ) + (y − y_c)·sin(φ)    // along axis 11
290/// y₂₂ = −(x − x_c)·sin(φ) + (y − y_c)·cos(φ)   // along axis 22
291/// ```
292///
293/// Note: this convention matches the standard Mohr's circle and differs
294/// from Python's `sectionproperties` by a sign in `phi`.
295pub fn principal_coordinate(phi: f64, x: f64, y: f64) -> (f64, f64) {
296    let cos_phi = phi.cos();
297    let sin_phi = phi.sin();
298    (x * cos_phi + y * sin_phi, y * cos_phi - x * sin_phi)
299}
300
301/// Convert principal coordinates back to global (centroidal) coordinates.
302///
303/// `phi` is the principal axis angle in **radians**.
304///
305/// ```text
306/// x = x₁₁·cos(φ) − y₂₂·sin(φ)
307/// y = x₁₁·sin(φ) + y₂₂·cos(φ)
308/// ```
309pub fn global_coordinate(phi: f64, x11: f64, y22: f64) -> (f64, f64) {
310    let cos_phi = phi.cos();
311    let sin_phi = phi.sin();
312    (x11 * cos_phi - y22 * sin_phi, x11 * sin_phi + y22 * cos_phi)
313}
314
315// ---------------------------------------------------------------------------
316// Shear parameters
317// ---------------------------------------------------------------------------
318
319/// Compute shear parameters used in shear load vector and coefficient assembly.
320pub fn shear_parameter(nx: f64, ny: f64, ixx: f64, iyy: f64, ixy: f64) -> [f64; 6] {
321    let r = nx * nx - ny * ny;
322    let q = 2.0 * nx * ny;
323    let d1 = ixx * r - ixy * q;
324    let d2 = ixy * r + ixx * q;
325    let h1 = -ixy * r + iyy * q;
326    let h2 = -iyy * r - ixy * q;
327    [r, q, d1, d2, h1, h2]
328}
329
330// ---------------------------------------------------------------------------
331// Tri6 element
332// ---------------------------------------------------------------------------
333
334/// Six-noded quadratic triangular finite element.
335///
336/// Node ordering: [v0, v1, v2, mid01, mid12, mid20] where v0,v1,v2 are
337/// vertices and midXX are mid-edge nodes.
338#[derive(Debug, Clone)]
339pub struct Tri6 {
340    /// Element id
341    pub el_id: usize,
342    /// Coordinates: coords\[0\] = x\[0..6\], coords\[1\] = y\[0..6\]
343    pub coords: [[f64; 6]; 2],
344    /// Global node ids
345    pub node_ids: [usize; 6],
346    /// Elastic modulus
347    pub elastic_modulus: f64,
348    /// Shear modulus
349    pub shear_modulus: f64,
350    /// Density
351    pub density: f64,
352}
353
354impl Tri6 {
355    /// Create a Tri6 element from 6 points and node ids.
356    ///
357    /// Validates that the element has positive orientation (CCW winding).
358    /// Returns `Err(FemError::InvalidElementOrientation)` if the element
359    /// is degenerate or has negative Jacobian (CW winding).
360    pub fn from_points(
361        el_id: usize,
362        points: [Point; 6],
363        node_ids: [usize; 6],
364        em: f64,
365        gm: f64,
366        rho: f64,
367    ) -> Result<Self, crate::mesh::fem::FemError> {
368        let mut coords = [[0.0; 6]; 2];
369        for i in 0..6 {
370            coords[0][i] = points[i].x;
371            coords[1][i] = points[i].y;
372        }
373        // Check orientation at centroid (xi=eta=zeta=1/3)
374        let sf = shape_function(&coords, (1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0));
375        if sf.j.abs() <= JACOBIAN_TOL {
376            return Err(crate::mesh::fem::FemError::DegenerateElement);
377        }
378        if sf.j < 0.0 {
379            return Err(crate::mesh::fem::FemError::InvalidElementOrientation);
380        }
381        Ok(Self {
382            el_id,
383            coords,
384            node_ids,
385            elastic_modulus: em,
386            shear_modulus: gm,
387            density: rho,
388        })
389    }
390
391    /// Calculate geometric properties: (area, qx, qy, ixx, iyy, ixy).
392    pub fn geometric_properties(&self) -> (f64, f64, f64, f64, f64, f64) {
393        let mut area = 0.0;
394        let mut qx = 0.0;
395        let mut qy = 0.0;
396        let mut ixx = 0.0;
397        let mut iyy = 0.0;
398        let mut ixy = 0.0;
399
400        for &(w, eta, xi, zeta) in &gauss_points(4) {
401            let sf = shape_function(&self.coords, (eta, xi, zeta));
402            let weight = w * sf.j;
403            area += weight;
404            qx += weight * sf.y;
405            qy += weight * sf.x;
406            ixx += weight * sf.y * sf.y;
407            iyy += weight * sf.x * sf.x;
408            ixy += weight * sf.y * sf.x;
409        }
410
411        (area, qx, qy, ixx, iyy, ixy)
412    }
413
414    /// Calculate torsion stiffness matrix (6×6), load vector (6), and constraint vector (6).
415    pub fn torsion_properties(&self) -> ([[f64; 6]; 6], [f64; 6], [f64; 6]) {
416        let mut k_el = [[0.0; 6]; 6];
417        let mut f_el = [0.0; 6];
418        let mut c_el = [0.0; 6];
419
420        // K_e, F_e and C_e: use 6-point Gauss rule that exactly integrates
421        // quadratic shape functions (degree 4 precision for triangles).
422        for &(w, eta, xi, zeta) in &gauss_points(6) {
423            let sf = shape_function(&self.coords, (eta, xi, zeta));
424            let weight = w * sf.j * self.elastic_modulus;
425
426            // k_el += weight * B^T * B
427            for i in 0..6 {
428                for j in 0..6 {
429                    k_el[i][j] += weight * (sf.b[0][i] * sf.b[0][j] + sf.b[1][i] * sf.b[1][j]);
430                }
431                // f_el += weight * B^T * [y, -x]
432                f_el[i] += weight * (sf.b[0][i] * sf.y - sf.b[1][i] * sf.x);
433                // c_el += E·N dA: Lagrange constraint ∫ E·ω dA = 0
434                c_el[i] += weight * sf.n[i];
435            }
436        }
437
438        (k_el, f_el, c_el)
439    }
440
441    /// Calculate shear load vectors f_psi and f_phi.
442    pub fn shear_load_vectors(
443        &self,
444        ixx: f64,
445        iyy: f64,
446        ixy: f64,
447        nu: f64,
448    ) -> ([f64; 6], [f64; 6]) {
449        let mut f_psi = [0.0; 6];
450        let mut f_phi = [0.0; 6];
451
452        for &(w, eta, xi, zeta) in &gauss_points(4) {
453            let sf = shape_function(&self.coords, (eta, xi, zeta));
454            let weight = w * sf.j * self.elastic_modulus;
455            let [_, _, d1, d2, h1, h2] = shear_parameter(sf.x, sf.y, ixx, iyy, ixy);
456
457            for i in 0..6 {
458                // f_psi += weight * (nu/2 * B^T * [d1, d2] + 2*(1+nu) * N * (ixx*x - ixy*y))
459                f_psi[i] += weight
460                    * (nu / 2.0 * (sf.b[0][i] * d1 + sf.b[1][i] * d2)
461                        + 2.0 * (1.0 + nu) * sf.n[i] * (ixx * sf.x - ixy * sf.y));
462                // f_phi += weight * (nu/2 * B^T * [h1, h2] + 2*(1+nu) * N * (iyy*y - ixy*x))
463                f_phi[i] += weight
464                    * (nu / 2.0 * (sf.b[0][i] * h1 + sf.b[1][i] * h2)
465                        + 2.0 * (1.0 + nu) * sf.n[i] * (iyy * sf.y - ixy * sf.x));
466            }
467        }
468
469        (f_psi, f_phi)
470    }
471
472    /// Calculate shear centre and warping integrals.
473    ///
474    /// Returns (sc_xint, sc_yint, q_omega, i_omega, i_xomega, i_yomega).
475    pub fn shear_warping_integrals(
476        &self,
477        ixx: f64,
478        iyy: f64,
479        ixy: f64,
480        omega: &[f64; 6],
481    ) -> (f64, f64, f64, f64, f64, f64) {
482        let mut sc_xint = 0.0;
483        let mut sc_yint = 0.0;
484        let mut q_omega = 0.0;
485        let mut i_omega = 0.0;
486        let mut i_xomega = 0.0;
487        let mut i_yomega = 0.0;
488
489        for &(w, eta, xi, zeta) in &gauss_points(4) {
490            let sf = shape_function(&self.coords, (eta, xi, zeta));
491            let weight = w * sf.j * self.elastic_modulus;
492
493            let n_omega: f64 = (0..6).map(|i| sf.n[i] * omega[i]).sum();
494            let r2 = sf.x * sf.x + sf.y * sf.y;
495
496            sc_xint += weight * (iyy * sf.x + ixy * sf.y) * r2;
497            sc_yint += weight * (ixx * sf.y + ixy * sf.x) * r2;
498            q_omega += weight * n_omega;
499            i_omega += weight * n_omega * n_omega;
500            i_xomega += weight * sf.x * n_omega;
501            i_yomega += weight * sf.y * n_omega;
502        }
503
504        (sc_xint, sc_yint, q_omega, i_omega, i_xomega, i_yomega)
505    }
506
507    /// Calculate shear deformation coefficients (kappa_x, kappa_y, kappa_xy).
508    pub fn shear_coefficients(
509        &self,
510        ixx: f64,
511        iyy: f64,
512        ixy: f64,
513        psi_shear: &[f64; 6],
514        phi_shear: &[f64; 6],
515        nu: f64,
516    ) -> (f64, f64, f64) {
517        let mut kappa_x = 0.0;
518        let mut kappa_y = 0.0;
519        let mut kappa_xy = 0.0;
520
521        for &(w, eta, xi, zeta) in &gauss_points(4) {
522            let sf = shape_function(&self.coords, (eta, xi, zeta));
523            let weight = w * sf.j * self.elastic_modulus;
524            let [_, _, d1, d2, h1, h2] = shear_parameter(sf.x, sf.y, ixx, iyy, ixy);
525
526            // B * psi - nu/2 * [d1, d2]
527            let mut b_psi_d = [0.0; 2];
528            let mut b_phi_h = [0.0; 2];
529            for i in 0..6 {
530                b_psi_d[0] += sf.b[0][i] * psi_shear[i];
531                b_psi_d[1] += sf.b[1][i] * psi_shear[i];
532                b_phi_h[0] += sf.b[0][i] * phi_shear[i];
533                b_phi_h[1] += sf.b[1][i] * phi_shear[i];
534            }
535            b_psi_d[0] -= nu / 2.0 * d1;
536            b_psi_d[1] -= nu / 2.0 * d2;
537            b_phi_h[0] -= nu / 2.0 * h1;
538            b_phi_h[1] -= nu / 2.0 * h2;
539
540            kappa_x += weight * (b_psi_d[0] * b_psi_d[0] + b_psi_d[1] * b_psi_d[1]);
541            kappa_y += weight * (b_phi_h[0] * b_phi_h[0] + b_phi_h[1] * b_phi_h[1]);
542            kappa_xy += weight * (b_psi_d[0] * b_phi_h[0] + b_psi_d[1] * b_phi_h[1]);
543        }
544
545        (kappa_x, kappa_y, kappa_xy)
546    }
547
548    /// Calculate monosymmetry integrals (int_x, int_y, int_11, int_22).
549    ///
550    /// `phi` is the principal axis angle in **radians**.
551    pub fn monosymmetry_integrals(&self, phi: f64) -> (f64, f64, f64, f64) {
552        let mut int_x = 0.0;
553        let mut int_y = 0.0;
554        let mut int_11 = 0.0;
555        let mut int_22 = 0.0;
556
557        for &(w, eta, xi, zeta) in &gauss_points(4) {
558            let sf = shape_function(&self.coords, (eta, xi, zeta));
559            let weight = w * sf.j * self.elastic_modulus;
560            let (nx_11, ny_22) = principal_coordinate(phi, sf.x, sf.y);
561
562            int_x += weight * (sf.x * sf.x * sf.y + sf.y * sf.y * sf.y);
563            int_y += weight * (sf.y * sf.y * sf.x + sf.x * sf.x * sf.x);
564            int_11 += weight * (nx_11 * nx_11 * ny_22 + ny_22 * ny_22 * ny_22);
565            int_22 += weight * (ny_22 * ny_22 * nx_11 + nx_11 * nx_11 * nx_11);
566        }
567
568        (int_x, int_y, int_11, int_22)
569    }
570
571    /// Calculate the St. Venant shear-stress factor at Gauss points.
572    ///
573    /// For pure torsion with torque `T`, the stress is `T / J` times the
574    /// returned factor: `(∂ω/∂x - y, ∂ω/∂y + x)`.
575    ///
576    /// Returns (max_shear_stress_magnitude, max_shear_stress_vector)
577    /// where max_shear_stress_vector = (tau_x, tau_y) at the point of max magnitude.
578    pub fn torsion_shear_stress(&self, omega: &[f64; 6]) -> (f64, (f64, f64)) {
579        let mut max_tau = 0.0;
580        let mut max_tau_vec = (0.0, 0.0);
581
582        // `psi` and `phi` are shear-deformation auxiliary fields.  They do
583        // not describe St. Venant torsion; only the warping solution does.
584        let gps = gauss_points(6);
585        for &(_w, eta, xi, zeta) in &gps {
586            let sf = shape_function(&self.coords, (eta, xi, zeta));
587
588            let mut b_omega = [0.0; 2];
589            for k in 0..6 {
590                b_omega[0] += sf.b[0][k] * omega[k];
591                b_omega[1] += sf.b[1][k] * omega[k];
592            }
593
594            // St. Venant shear stress components
595            // τ_x = ∂ω/∂x - y (for unit twist)
596            // τ_y = ∂ω/∂y + x (for unit twist)
597            let tau_x = b_omega[0] - sf.y;
598            let tau_y = b_omega[1] + sf.x;
599
600            let tau_mag = (tau_x * tau_x + tau_y * tau_y).sqrt();
601            if tau_mag > max_tau {
602                max_tau = tau_mag;
603                max_tau_vec = (tau_x, tau_y);
604            }
605        }
606        (max_tau, max_tau_vec)
607    }
608
609    /// Calculate element stresses at the 6 Gauss points, extrapolated to nodes.
610    ///
611    /// Returns 11 nodal stress arrays (each length 6):
612    /// (sig_zz_n, sig_zz_mxx, sig_zz_myy, sig_zz_m11, sig_zz_m22,
613    ///  sig_zx_mzz, sig_zy_mzz, sig_zx_vx, sig_zy_vx, sig_zx_vy, sig_zy_vy)
614    pub fn element_stress(
615        &self,
616        n: f64,
617        mxx: f64,
618        myy: f64,
619        m11: f64,
620        m22: f64,
621        mzz: f64,
622        vx: f64,
623        vy: f64,
624        ea: f64,
625        cx: f64,
626        cy: f64,
627        ixx: f64,
628        iyy: f64,
629        ixy: f64,
630        i11: f64,
631        i22: f64,
632        phi: f64,
633        j: f64,
634        nu: f64,
635        omega: &[f64; 6],
636        psi_shear: &[f64; 6],
637        phi_shear: &[f64; 6],
638        delta_s: f64,
639    ) -> [[f64; 6]; 11] {
640        let em = self.elastic_modulus;
641        let denom = ixx * iyy - ixy * ixy;
642
643        // Axial stress (constant over element)
644        let sig_zz_n = [em * n / ea; 6];
645
646        let mut sig_zz_mxx_gp = [0.0; 6];
647        let mut sig_zz_myy_gp = [0.0; 6];
648        let mut sig_zz_m11_gp = [0.0; 6];
649        let mut sig_zz_m22_gp = [0.0; 6];
650        let mut sig_zx_mzz_gp = [0.0; 6];
651        let mut sig_zy_mzz_gp = [0.0; 6];
652        let mut sig_zx_vx_gp = [0.0; 6];
653        let mut sig_zy_vx_gp = [0.0; 6];
654        let mut sig_zx_vy_gp = [0.0; 6];
655        let mut sig_zy_vy_gp = [0.0; 6];
656
657        // Centroidal coords
658        let coords_c = [
659            [
660                self.coords[0][0] - cx,
661                self.coords[0][1] - cx,
662                self.coords[0][2] - cx,
663                self.coords[0][3] - cx,
664                self.coords[0][4] - cx,
665                self.coords[0][5] - cx,
666            ],
667            [
668                self.coords[1][0] - cy,
669                self.coords[1][1] - cy,
670                self.coords[1][2] - cy,
671                self.coords[1][3] - cy,
672                self.coords[1][4] - cy,
673                self.coords[1][5] - cy,
674            ],
675        ];
676
677        let gps = gauss_points(6);
678        for (i, &(_w, eta, xi, zeta)) in gps.iter().enumerate() {
679            let sf = shape_function(&coords_c, (eta, xi, zeta));
680            let (nx_11, ny_22) = principal_coordinate(phi, sf.x, sf.y);
681            let [_, _, d1, d2, h1, h2] = shear_parameter(sf.x, sf.y, ixx, iyy, ixy);
682
683            // Bending stresses
684            if denom.abs() > NEAR_ZERO_TOL_BASE {
685                sig_zz_mxx_gp[i] = em * (-ixy * mxx / denom * sf.x + iyy * mxx / denom * sf.y);
686                sig_zz_myy_gp[i] = em * (-ixx * myy / denom * sf.x + ixy * myy / denom * sf.y);
687            }
688            if i11.abs() > NEAR_ZERO_TOL_BASE {
689                sig_zz_m11_gp[i] = em * m11 / i11 * ny_22;
690            }
691            if i22.abs() > NEAR_ZERO_TOL_BASE {
692                sig_zz_m22_gp[i] = em * -m22 / i22 * nx_11;
693            }
694
695            // Torsional shear stress
696            if mzz.abs() > GEOMETRY_TOL && j.abs() > GEOMETRY_TOL {
697                let mut b_omega = [0.0; 2];
698                for k in 0..6 {
699                    b_omega[0] += sf.b[0][k] * omega[k];
700                    b_omega[1] += sf.b[1][k] * omega[k];
701                }
702                sig_zx_mzz_gp[i] = em * mzz / j * (b_omega[0] - sf.y);
703                sig_zy_mzz_gp[i] = em * mzz / j * (b_omega[1] + sf.x);
704            }
705
706            // Shear stress from Vx
707            if vx.abs() > GEOMETRY_TOL && delta_s.abs() > GEOMETRY_TOL {
708                let mut b_psi = [0.0; 2];
709                for k in 0..6 {
710                    b_psi[0] += sf.b[0][k] * psi_shear[k];
711                    b_psi[1] += sf.b[1][k] * psi_shear[k];
712                }
713                sig_zx_vx_gp[i] = em * vx / delta_s * (b_psi[0] - nu / 2.0 * d1);
714                sig_zy_vx_gp[i] = em * vx / delta_s * (b_psi[1] - nu / 2.0 * d2);
715            }
716
717            // Shear stress from Vy
718            if vy.abs() > GEOMETRY_TOL && delta_s.abs() > GEOMETRY_TOL {
719                let mut b_phi = [0.0; 2];
720                for k in 0..6 {
721                    b_phi[0] += sf.b[0][k] * phi_shear[k];
722                    b_phi[1] += sf.b[1][k] * phi_shear[k];
723                }
724                sig_zx_vy_gp[i] = em * vy / delta_s * (b_phi[0] - nu / 2.0 * h1);
725                sig_zy_vy_gp[i] = em * vy / delta_s * (b_phi[1] - nu / 2.0 * h2);
726            }
727        }
728
729        // Extrapolate from Gauss points to nodes
730        [
731            sig_zz_n,
732            extrapolate_to_nodes(&sig_zz_mxx_gp),
733            extrapolate_to_nodes(&sig_zz_myy_gp),
734            extrapolate_to_nodes(&sig_zz_m11_gp),
735            extrapolate_to_nodes(&sig_zz_m22_gp),
736            extrapolate_to_nodes(&sig_zx_mzz_gp),
737            extrapolate_to_nodes(&sig_zy_mzz_gp),
738            extrapolate_to_nodes(&sig_zx_vx_gp),
739            extrapolate_to_nodes(&sig_zy_vx_gp),
740            extrapolate_to_nodes(&sig_zx_vy_gp),
741            extrapolate_to_nodes(&sig_zy_vy_gp),
742        ]
743    }
744}
745
746// ---------------------------------------------------------------------------
747// Tri6 mesh generation from Tri3 mesh
748// ---------------------------------------------------------------------------
749
750/// A Tri6 mesh: nodes and 6-noded elements.
751#[derive(Debug, Clone)]
752pub struct Tri6Mesh {
753    /// All nodes (original + mid-edge)
754    pub nodes: Vec<Point>,
755    /// Elements: each is 6 node indices [v0, v1, v2, mid01, mid12, mid20]
756    pub elements: Vec<[usize; 6]>,
757}
758
759/// Convert a Tri3 mesh to a Tri6 mesh by adding mid-edge nodes.
760///
761/// Shared edges between elements share the same mid-node.
762/// Ensures all Tri6 elements have CCW orientation (positive Jacobian).
763pub fn tri3_to_tri6(tri3_nodes: &[Point], tri3_elements: &[[usize; 3]]) -> Tri6Mesh {
764    let mut nodes: Vec<Point> = tri3_nodes.to_vec();
765    // Map from (min_node, max_node) -> mid_node_index
766    let mut edge_midpoints: HashMap<(usize, usize), usize> = HashMap::new();
767
768    let mut tri6_elements: Vec<[usize; 6]> = Vec::with_capacity(tri3_elements.len());
769
770    for tri3_elem in tri3_elements {
771        let n0 = tri3_elem[0];
772        let n1 = tri3_elem[1];
773        let n2 = tri3_elem[2];
774        let mid01 = get_or_create_midpoint(&mut nodes, &mut edge_midpoints, n0, n1);
775        let mid12 = get_or_create_midpoint(&mut nodes, &mut edge_midpoints, n1, n2);
776        let mid20 = get_or_create_midpoint(&mut nodes, &mut edge_midpoints, n2, n0);
777        let mut elem = [n0, n1, n2, mid01, mid12, mid20];
778
779        // Check orientation and fix if needed (CCW required)
780        let mut coords = [[0.0; 6]; 2];
781        for k in 0..6 {
782            coords[0][k] = nodes[elem[k]].x;
783            coords[1][k] = nodes[elem[k]].y;
784        }
785        let sf = shape_function(&coords, (1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0));
786        if sf.j < 0.0 {
787            // Reverse the vertex order to fix orientation (CCW required)
788            // Original: [n0, n1, n2, mid01, mid12, mid20]
789            // After vertex reverse: [n2, n1, n0, mid01, mid12, mid20]
790            // Need to reorder midpoints to match: [n2, n1, n0, mid12, mid01, mid20]
791            elem[0..3].reverse(); // Now [n2, n1, n0, mid01, mid12, mid20]
792            elem[3] = mid12;
793            elem[4] = mid01;
794            elem[5] = mid20;
795        }
796        tri6_elements.push(elem);
797    }
798
799    Tri6Mesh {
800        nodes,
801        elements: tri6_elements,
802    }
803}
804
805/// Validates that a Tri6Mesh has no degenerate elements.
806///
807/// Returns `Err(FemError::DegenerateElement)` if any element has
808/// zero or negative Jacobian determinant at any Gauss point.
809pub fn validate_tri6_mesh(mesh: &Tri6Mesh) -> Result<(), crate::mesh::fem::FemError> {
810    let gps = gauss_points(6);
811    for &elem in mesh.elements.iter() {
812        let mut coords = [[0.0; 6]; 2];
813        for k in 0..6 {
814            coords[0][k] = mesh.nodes[elem[k]].x;
815            coords[1][k] = mesh.nodes[elem[k]].y;
816        }
817        for &(_, eta, xi, zeta) in &gps {
818            let sf = shape_function(&coords, (eta, xi, zeta));
819            if sf.j <= JACOBIAN_TOL {
820                return Err(crate::mesh::fem::FemError::DegenerateElement);
821            }
822        }
823    }
824    Ok(())
825}
826
827fn get_or_create_midpoint(
828    nodes: &mut Vec<Point>,
829    edge_map: &mut HashMap<(usize, usize), usize>,
830    a: usize,
831    b: usize,
832) -> usize {
833    let key = if a < b { (a, b) } else { (b, a) };
834    if let Some(&mid) = edge_map.get(&key) {
835        return mid;
836    }
837    let pa = nodes[a];
838    let pb = nodes[b];
839    let mid = Point::new(0.5 * (pa.x + pb.x), 0.5 * (pa.y + pb.y));
840    let mid_idx = nodes.len();
841    nodes.push(mid);
842    edge_map.insert(key, mid_idx);
843    mid_idx
844}
845
846/// Build a list of Tri6 elements from a Tri6Mesh for a given material.
847/// Returns `Err(FemError::DegenerateElement)` if any element has
848/// zero or negative Jacobian determinant at any Gauss point.
849/// Coordinates are kept in section-centroid frame for stress calculation compatibility.
850pub fn build_tri6_elements(
851    mesh: &Tri6Mesh,
852    em: f64,
853    gm: f64,
854    rho: f64,
855) -> Result<Vec<Tri6>, crate::mesh::fem::FemError> {
856    let gps = gauss_points(6);
857    let mut elements = Vec::with_capacity(mesh.elements.len());
858    for (i, &elem) in mesh.elements.iter().enumerate() {
859        let points: [Point; 6] = [
860            mesh.nodes[elem[0]],
861            mesh.nodes[elem[1]],
862            mesh.nodes[elem[2]],
863            mesh.nodes[elem[3]],
864            mesh.nodes[elem[4]],
865            mesh.nodes[elem[5]],
866        ];
867        let coords = [
868            [
869                points[0].x,
870                points[1].x,
871                points[2].x,
872                points[3].x,
873                points[4].x,
874                points[5].x,
875            ],
876            [
877                points[0].y,
878                points[1].y,
879                points[2].y,
880                points[3].y,
881                points[4].y,
882                points[5].y,
883            ],
884        ];
885        // Check for degenerate element at all Gauss points
886        let mut degenerate = false;
887        for &(_, eta, xi, zeta) in &gps {
888            let sf = shape_function(&coords, (eta, xi, zeta));
889            if sf.j <= JACOBIAN_TOL {
890                degenerate = true;
891                break;
892            }
893        }
894        if degenerate {
895            return Err(crate::mesh::fem::FemError::DegenerateElement);
896        }
897        elements.push(Tri6::from_points(i, points, elem, em, gm, rho)?);
898    }
899    Ok(elements)
900}
901
902// ---------------------------------------------------------------------------
903// Sparse matrix and conjugate gradient solver
904// ---------------------------------------------------------------------------
905
906/// Sparse matrix in CSR (compressed sparse row) format.
907///
908/// Triplets added via `add()` are stored uncompressed. Call `compress()`
909/// once to convert to canonical CSR (summing duplicates, sorting by row/col).
910/// After compression, `matvec`/`matvec_into` use the CSR structure.
911#[derive(Debug, Clone)]
912pub struct SparseMatrix {
913    pub n: usize,
914    /// COO triplets (before compression).
915    rows: Vec<usize>,
916    cols: Vec<usize>,
917    vals: Vec<f64>,
918    /// CSR structure (after compress()).
919    row_ptr: Vec<usize>,
920    csr_cols: Vec<usize>,
921    csr_vals: Vec<f64>,
922    /// Cached diagonal entries (sum of duplicates).
923    diag: Vec<f64>,
924    /// Whether the matrix is in CSR format.
925    compressed: bool,
926}
927
928impl SparseMatrix {
929    pub fn new(n: usize) -> Self {
930        Self {
931            n,
932            rows: Vec::new(),
933            cols: Vec::new(),
934            vals: Vec::new(),
935            row_ptr: vec![0; n + 1],
936            csr_cols: Vec::new(),
937            csr_vals: Vec::new(),
938            diag: vec![0.0; n],
939            compressed: false,
940        }
941    }
942
943    /// Add a triplet (duplicates allowed, summed on compress).
944    pub fn add(&mut self, row: usize, col: usize, val: f64) {
945        debug_assert!(row < self.n && col < self.n);
946        self.rows.push(row);
947        self.cols.push(col);
948        self.vals.push(val);
949        self.compressed = false;
950    }
951
952    /// Convert COO triplets to canonical CSR (sum duplicates, sort).
953    /// Idempotent: safe to call multiple times.
954    pub fn compress(&mut self) {
955        if self.compressed {
956            return;
957        }
958        let n = self.n;
959        let nnz = self.rows.len();
960
961        if nnz == 0 {
962            self.row_ptr = vec![0; n + 1];
963            self.csr_cols.clear();
964            self.csr_vals.clear();
965            self.diag = vec![0.0; n];
966            self.compressed = true;
967            return;
968        }
969
970        // Collect and sort triplets by (row, col)
971        let mut triplets: Vec<(usize, usize, f64)> = Vec::with_capacity(nnz);
972        for k in 0..nnz {
973            triplets.push((self.rows[k], self.cols[k], self.vals[k]));
974        }
975        triplets.sort_unstable_by(|a, b| a.0.cmp(&b.0).then(a.1.cmp(&b.1)));
976
977        // Merge duplicates in a single pass
978        let mut csr_cols = Vec::with_capacity(triplets.len());
979        let mut csr_vals = Vec::with_capacity(triplets.len());
980        let mut row_ptr = vec![0usize; n + 1];
981        let mut diag = vec![0.0; n];
982
983        let mut i = 0;
984        while i < triplets.len() {
985            let r = triplets[i].0;
986            let c = triplets[i].1;
987            let mut v = triplets[i].2;
988            let mut j = i + 1;
989            while j < triplets.len() && triplets[j].0 == r && triplets[j].1 == c {
990                v += triplets[j].2;
991                j += 1;
992            }
993            if v.abs() > 0.0 {
994                csr_cols.push(c);
995                csr_vals.push(v);
996                row_ptr[r + 1] += 1;
997                if r == c {
998                    diag[r] += v;
999                }
1000            }
1001            i = j;
1002        }
1003
1004        // Inclusive scan for row_ptr
1005        for r in 0..n {
1006            row_ptr[r + 1] += row_ptr[r];
1007        }
1008
1009        self.row_ptr = row_ptr;
1010        self.csr_cols = csr_cols;
1011        self.csr_vals = csr_vals;
1012        self.diag = diag;
1013        self.compressed = true;
1014    }
1015
1016    /// Matrix-vector product y = A*x (requires compress() first).
1017    pub fn matvec(&self, x: &[f64]) -> Vec<f64> {
1018        let mut y = vec![0.0; self.n];
1019        self.matvec_into(x, &mut y);
1020        y
1021    }
1022
1023    /// Matrix-vector product into a caller-provided buffer (requires compress() first).
1024    pub fn matvec_into(&self, x: &[f64], y: &mut [f64]) {
1025        if !self.compressed {
1026            panic!("SparseMatrix::compress() must be called before matvec");
1027        }
1028        for v in y.iter_mut() {
1029            *v = 0.0;
1030        }
1031        for r in 0..self.n {
1032            let start = self.row_ptr[r];
1033            let end = self.row_ptr[r + 1];
1034            let mut sum = 0.0;
1035            for k in start..end {
1036                sum += self.csr_vals[k] * x[self.csr_cols[k]];
1037            }
1038            y[r] = sum;
1039        }
1040    }
1041
1042    /// Diagonal entry of row `i` (requires compress() first).
1043    pub fn matvec_diag(&self, i: usize) -> f64 {
1044        if !self.compressed {
1045            panic!("SparseMatrix::compress() must be called before matvec_diag");
1046        }
1047        self.diag[i]
1048    }
1049
1050    /// Get CSR data for external solvers (PARDISO, etc.).
1051    pub fn csr_data(&self) -> (&[usize], &[usize], &[f64]) {
1052        if !self.compressed {
1053            panic!("SparseMatrix::compress() must be called before csr_data");
1054        }
1055        (&self.row_ptr, &self.csr_cols, &self.csr_vals)
1056    }
1057
1058    /// Get CSR row pointers (for diagnostics).
1059    pub fn row_ptr(&self) -> &[usize] {
1060        if !self.compressed {
1061            panic!("SparseMatrix::compress() must be called before row_ptr");
1062        }
1063        &self.row_ptr
1064    }
1065
1066    /// Get CSR column indices (for diagnostics).
1067    pub fn csr_cols(&self) -> &[usize] {
1068        if !self.compressed {
1069            panic!("SparseMatrix::compress() must be called before csr_cols");
1070        }
1071        &self.csr_cols
1072    }
1073
1074    /// Get CSR values (for diagnostics).
1075    pub fn csr_vals(&self) -> &[f64] {
1076        if !self.compressed {
1077            panic!("SparseMatrix::compress() must be called before csr_vals");
1078        }
1079        &self.csr_vals
1080    }
1081
1082    /// Frobenius norm of the matrix.
1083    pub fn frobenius_norm(&self) -> f64 {
1084        if !self.compressed {
1085            panic!("SparseMatrix::compress() must be called before frobenius_norm");
1086        }
1087        self.csr_vals.iter().map(|v| v * v).sum::<f64>().sqrt()
1088    }
1089
1090    /// Maximum symmetry error max_{i<j} |A_ij - A_ji|.
1091    pub fn symmetry_max_error(&self) -> f64 {
1092        if !self.compressed {
1093            panic!("SparseMatrix::compress() must be called before symmetry_max_error");
1094        }
1095        let mut max_err = 0.0;
1096        for i in 0..self.n {
1097            let start = self.row_ptr[i];
1098            let end = self.row_ptr[i + 1];
1099            for k in start..end {
1100                let j = self.csr_cols[k];
1101                if j > i {
1102                    let val_ij = self.csr_vals[k];
1103                    let val_ji = self.get(j, i);
1104                    let err = (val_ij - val_ji).abs();
1105                    if err > max_err {
1106                        max_err = err;
1107                    }
1108                }
1109            }
1110        }
1111        max_err
1112    }
1113
1114    /// Check if matrix is symmetric within tolerance.
1115    pub fn is_symmetric(&self, tol: f64) -> bool {
1116        if !self.compressed {
1117            panic!("SparseMatrix::compress() must be called before is_symmetric");
1118        }
1119        for i in 0..self.n {
1120            let start = self.row_ptr[i];
1121            let end = self.row_ptr[i + 1];
1122            for k in start..end {
1123                let j = self.csr_cols[k];
1124                if j > i {
1125                    let val_ij = self.csr_vals[k];
1126                    let val_ji = self.get(j, i);
1127                    // Scale-aware comparison: a matrix assembled in floating
1128                    // point is symmetric only up to round-off, and that
1129                    // round-off grows with the entry magnitude (a stiffness
1130                    // matrix with EA/L ~ 1e8 carries ~1e-8 of asymmetry while
1131                    // being symmetric to ~1e-16 *relative*). An absolute
1132                    // tolerance would misclassify such matrices as
1133                    // non-symmetric. `tol` is therefore relative to the entry
1134                    // magnitude, with a floor of 1.0 so unit-scale problems
1135                    // keep the previous absolute behaviour.
1136                    let scale = val_ij.abs().max(val_ji.abs()).max(1.0);
1137                    if (val_ij - val_ji).abs() > tol * scale {
1138                        return false;
1139                    }
1140                }
1141            }
1142        }
1143        true
1144    }
1145
1146    /// Get value at (row, col) - requires compress().
1147    pub fn get(&self, row: usize, col: usize) -> f64 {
1148        if !self.compressed {
1149            panic!("SparseMatrix::compress() must be called before get");
1150        }
1151        let start = self.row_ptr[row];
1152        let end = self.row_ptr[row + 1];
1153        for k in start..end {
1154            if self.csr_cols[k] == col {
1155                return self.csr_vals[k];
1156            }
1157        }
1158        0.0
1159    }
1160
1161    /// Get the raw COO triplets (for diagnostics/export).
1162    pub fn triplets(&self) -> (&[usize], &[usize], &[f64]) {
1163        if !self.compressed {
1164            panic!("SparseMatrix::compress() must be called before accessing triplets");
1165        }
1166        (&self.rows, &self.cols, &self.vals)
1167    }
1168}
1169
1170/// Conjugate gradient solver with diagonal preconditioning.
1171///
1172/// Requires `a` to be compressed (call `a.compress()` first).
1173/// Returns a `CgResult` containing the solution and convergence information.
1174pub fn cg_solve(a: &SparseMatrix, b: &[f64], max_iter: usize, tol: f64) -> CgResult {
1175    let n = b.len();
1176
1177    // Diagonal preconditioner: M = diag(A), M_inv = 1/diag(A).
1178    // Uses pre-computed diagonal from CSR structure.
1179    let mut m_inv = vec![1.0; n];
1180    for i in 0..n {
1181        let d = a.diag[i];
1182        if d.abs() > NEAR_ZERO_TOL_BASE {
1183            m_inv[i] = 1.0 / d;
1184        }
1185    }
1186
1187    let mut x = vec![0.0; n];
1188    let mut r = b.to_vec();
1189
1190    // Relative convergence: scale tolerance by ||b|| so that problems with
1191    // small absolute magnitudes (e.g. unit-modulus scaled FEM loads) still
1192    // iterate to a meaningful solution.
1193    let b_norm = b.iter().map(|v| v * v).sum::<f64>().sqrt();
1194    if b_norm == 0.0 || !b_norm.is_finite() {
1195        return CgResult {
1196            x,
1197            iterations: 0,
1198            residual: 0.0,
1199            status: CgStatus::InvalidInput,
1200        };
1201    }
1202
1203    let mut z: Vec<f64> = r.iter().zip(m_inv.iter()).map(|(a, b)| a * b).collect();
1204    let mut p = z.clone();
1205    let mut rz_old: f64 = r.iter().zip(z.iter()).map(|(a, b)| a * b).sum();
1206    let mut ap_buf = vec![0.0; n];
1207
1208    let mut iterations = 0;
1209    let mut converged = false;
1210    let mut residual = 0.0;
1211
1212    for iter in 0..max_iter {
1213        a.matvec_into(&p, &mut ap_buf);
1214        let ap: &[f64] = &ap_buf;
1215        let p_ap: f64 = p.iter().zip(ap.iter()).map(|(a, b)| a * b).sum();
1216
1217        // Check for breakdown: p^T A p <= 0 indicates A is not SPD
1218        if p_ap <= 0.0 || !p_ap.is_finite() {
1219            return CgResult {
1220                x,
1221                iterations,
1222                residual: r.iter().map(|v| v * v).sum::<f64>().sqrt(),
1223                status: CgStatus::NotPositiveDefinite,
1224            };
1225        }
1226
1227        let alpha = rz_old / p_ap;
1228        if !alpha.is_finite() {
1229            return CgResult {
1230                x,
1231                iterations,
1232                residual: r.iter().map(|v| v * v).sum::<f64>().sqrt(),
1233                status: CgStatus::Breakdown,
1234            };
1235        }
1236
1237        for i in 0..n {
1238            x[i] += alpha * p[i];
1239            r[i] -= alpha * ap[i];
1240        }
1241
1242        let rs_new: f64 = r.iter().map(|v| v * v).sum();
1243        residual = rs_new.sqrt();
1244        if residual < tol * b_norm {
1245            converged = true;
1246            iterations = iter + 1;
1247            break;
1248        }
1249
1250        if rz_old == 0.0 || !rz_old.is_finite() {
1251            return CgResult {
1252                x,
1253                iterations,
1254                residual,
1255                status: CgStatus::Breakdown,
1256            };
1257        }
1258        for i in 0..n {
1259            z[i] = r[i] * m_inv[i];
1260        }
1261        let rz_new: f64 = r.iter().zip(z.iter()).map(|(a, b)| a * b).sum();
1262        if !rz_new.is_finite() {
1263            return CgResult {
1264                x,
1265                iterations,
1266                residual,
1267                status: CgStatus::Breakdown,
1268            };
1269        }
1270        let beta = rz_new / rz_old;
1271        if !beta.is_finite() {
1272            return CgResult {
1273                x,
1274                iterations,
1275                residual,
1276                status: CgStatus::Breakdown,
1277            };
1278        }
1279        for i in 0..n {
1280            p[i] = z[i] + beta * p[i];
1281        }
1282        rz_old = rz_new;
1283        iterations = iter + 1;
1284    }
1285
1286    CgResult {
1287        x,
1288        iterations,
1289        residual,
1290        status: if converged {
1291            CgStatus::Converged
1292        } else {
1293            CgStatus::MaxIterations
1294        },
1295    }
1296}
1297
1298/// Conjugate Gradient solver termination status.
1299#[derive(Debug, Clone, Copy, PartialEq, Eq)]
1300pub enum CgStatus {
1301    /// Converged to requested tolerance.
1302    Converged,
1303    /// Reached maximum iterations without converging.
1304    MaxIterations,
1305    /// Matrix not positive definite (p^T A p <= 0 detected).
1306    NotPositiveDefinite,
1307    /// Numerical breakdown (non-finite alpha, beta, or residual).
1308    Breakdown,
1309    /// Invalid input (empty RHS, non-finite b_norm).
1310    InvalidInput,
1311}
1312
1313/// Result of the Conjugate Gradient solver.
1314#[derive(Debug, Clone)]
1315pub struct CgResult {
1316    /// Approximate solution vector.
1317    pub x: Vec<f64>,
1318    /// Number of iterations performed.
1319    pub iterations: usize,
1320    /// Final residual norm ||b - A*x||.
1321    pub residual: f64,
1322    /// Solver termination status.
1323    pub status: CgStatus,
1324}
1325
1326impl CgResult {
1327    /// Returns true if the solver converged successfully.
1328    pub fn converged(&self) -> bool {
1329        matches!(self.status, CgStatus::Converged)
1330    }
1331}
1332
1333/// Solve the Lagrangian system using sparse CG + Schur complement.
1334///
1335/// \[K  c\] \[u\]   \[f\]
1336/// \[c^T 0\] \[λ\] = \[0\]
1337///
1338/// u = K^{-1}*(f - c*λ),  λ = (c^T*K^{-1}*c)^{-1} * c^T*K^{-1}*f
1339pub fn solve_lagrange_sparse(
1340    k: &SparseMatrix,
1341    c: &[f64],
1342    f: &[f64],
1343) -> Result<Vec<f64>, crate::mesh::fem::FemError> {
1344    solve_lagrange_sparse_tol(k, c, f, 1e-6)
1345}
1346
1347/// Like [`solve_lagrange_sparse`] with an explicit relative CG tolerance.
1348pub fn solve_lagrange_sparse_tol(
1349    k: &SparseMatrix,
1350    c: &[f64],
1351    f: &[f64],
1352    tol: f64,
1353) -> Result<Vec<f64>, crate::mesh::fem::FemError> {
1354    // Tight tolerances (needed for shear-centre difference quantities)
1355    // require a proportionally larger iteration budget.
1356    let n = k.n;
1357    let max_iter = if tol < 1e-8 {
1358        (n * 20).clamp(20000, 600000)
1359    } else {
1360        (n * 4).clamp(1000, 60000)
1361    };
1362    let _n = k.n;
1363
1364    // Try solving without regularization first (matching Skyline behavior).
1365    // Warping Laplacian has a constant null space; CG will detect non-SPD
1366    // and we fall back to regularized K.
1367    let mut k_reg = k.clone();
1368    k_reg.compress();
1369    let n = k.n;
1370
1371    let w1 = cg_solve(&k_reg, f, max_iter, tol);
1372    let w2 = cg_solve(&k_reg, c, max_iter, tol);
1373
1374    // If CG didn't converge (breakdown or non-SPD), fall back to regularization.
1375    let (w1, w2) = if w1.converged() && w2.converged() {
1376        (w1.x, w2.x)
1377    } else {
1378        // Regularize K: K_reg = K + ε*I
1379        let mut k_reg = k.clone();
1380        k_reg.compress();
1381        let mut diag_sum = 0.0;
1382        for i in 0..n {
1383            diag_sum += k_reg.matvec_diag(i);
1384        }
1385        let eps = diag_sum.max(1e-300) / n as f64 * 1e-9;
1386        for i in 0..n {
1387            k_reg.add(i, i, eps);
1388        }
1389        k_reg.compress();
1390
1391        let w1 = cg_solve(&k_reg, f, max_iter, tol).x;
1392        let w2 = cg_solve(&k_reg, c, max_iter, tol).x;
1393        (w1, w2)
1394    };
1395
1396    let ct_w2: f64 = c.iter().zip(w2.iter()).map(|(a, b)| a * b).sum();
1397    let ct_w1: f64 = c.iter().zip(w1.iter()).map(|(a, b)| a * b).sum();
1398
1399    if ct_w2.abs() < NEAR_ZERO_TOL_BASE {
1400        return Ok(w1);
1401    }
1402    let lambda = ct_w1 / ct_w2;
1403
1404    let mut u = vec![0.0; n];
1405    for i in 0..n {
1406        u[i] = w1[i] - lambda * w2[i];
1407    }
1408    Ok(u)
1409}
1410
1411// ---------------------------------------------------------------------------
1412// Linear solver for Lagrangian system
1413// ---------------------------------------------------------------------------
1414
1415/// Solve the Lagrangian system [K  c; c^T 0] [u; lambda] = [f; 0].
1416///
1417/// `k` is n×n stiffness matrix, `c` is n constraint vector, `f` is n load vector.
1418/// Returns the solution vector u (length n), or `Err` if singular.
1419pub fn solve_lagrange(
1420    k: &[Vec<f64>],
1421    c: &[f64],
1422    f: &[f64],
1423) -> Result<Vec<f64>, crate::mesh::fem::FemError> {
1424    let n = k.len();
1425    // Augmented system (n+1)×(n+1)
1426    let mut a = vec![vec![0.0; n + 1]; n + 1];
1427    for i in 0..n {
1428        for j in 0..n {
1429            a[i][j] = k[i][j];
1430        }
1431        a[i][n] = c[i];
1432        a[n][i] = c[i];
1433    }
1434    let mut rhs = vec![0.0; n + 1];
1435    for i in 0..n {
1436        rhs[i] = f[i];
1437    }
1438
1439    // Solve using Gaussian elimination with partial pivoting
1440    solve_dense(&mut a, &mut rhs)?;
1441    Ok(rhs[..n].to_vec())
1442}
1443
1444/// Solve a dense linear system A*x = b in place (Gaussian elimination with partial pivoting).
1445/// Returns `Err(FemError::SingularMatrix)` if the matrix is singular.
1446pub fn solve_dense(a: &mut [Vec<f64>], b: &mut [f64]) -> Result<(), crate::mesh::fem::FemError> {
1447    let n = a.len();
1448    // Compute matrix scale for scale-invariant tolerance
1449    let mut scale = 0.0f64;
1450    for i in 0..n {
1451        for j in 0..n {
1452            scale = scale.max(a[i][j].abs());
1453        }
1454    }
1455    let pivot_tol = PIVOT_TOL_BASE * scale.max(1.0);
1456    for k in 0..n {
1457        // Partial pivoting
1458        let mut max_row = k;
1459        let mut max_val = a[k][k].abs();
1460        for i in (k + 1)..n {
1461            if a[i][k].abs() > max_val {
1462                max_val = a[i][k].abs();
1463                max_row = i;
1464            }
1465        }
1466        if max_row != k {
1467            a.swap(k, max_row);
1468            b.swap(k, max_row);
1469        }
1470        let pivot = a[k][k];
1471        if pivot.abs() < pivot_tol {
1472            return Err(crate::mesh::fem::FemError::SingularMatrix);
1473        }
1474        for i in (k + 1)..n {
1475            let factor = a[i][k] / pivot;
1476            for j in k..n {
1477                a[i][j] -= factor * a[k][j];
1478            }
1479            b[i] -= factor * b[k];
1480        }
1481    }
1482    // Back substitution
1483    for k in (0..n).rev() {
1484        let pivot = a[k][k];
1485        if pivot.abs() < pivot_tol {
1486            return Err(crate::mesh::fem::FemError::SingularMatrix);
1487        }
1488        let mut sum = 0.0;
1489        for j in (k + 1)..n {
1490            sum += a[k][j] * b[j];
1491        }
1492        b[k] = (b[k] - sum) / pivot;
1493    }
1494    Ok(())
1495}
1496
1497// ---------------------------------------------------------------------------
1498// Tests
1499// ---------------------------------------------------------------------------
1500
1501/// Sparse matrix in CSC (compressed sparse column) format.
1502///
1503/// Mirrors scipy `csc_matrix` as used by Python sectionproperties'
1504/// warping solver (`assemble_torsion` returns `k_lg` in CSC form).
1505#[derive(Debug, Clone)]
1506pub struct CscMatrix {
1507    pub n_rows: usize,
1508    pub n_cols: usize,
1509    /// Column offsets, length n_cols + 1.
1510    pub col_ptr: Vec<usize>,
1511    /// Row indices sorted within each column.
1512    pub rows: Vec<usize>,
1513    /// Nonzero values, column-major.
1514    pub vals: Vec<f64>,
1515}
1516
1517impl CscMatrix {
1518    /// Build a CSC matrix from CSR data, summing duplicates
1519    /// (mirrors `csr_matrix(...).tocsc()`).
1520    pub fn from_csr(
1521        n_rows: usize,
1522        n_cols: usize,
1523        row_ptr: &[usize],
1524        csr_cols: &[usize],
1525        csr_vals: &[f64],
1526    ) -> Self {
1527        debug_assert_eq!(row_ptr.len(), n_rows + 1);
1528        debug_assert_eq!(csr_cols.len(), csr_vals.len());
1529
1530        // Count nonzeros per column
1531        let mut counts = vec![0usize; n_cols];
1532        for r in 0..n_rows {
1533            let start = row_ptr[r];
1534            let end = row_ptr[r + 1];
1535            for k in start..end {
1536                counts[csr_cols[k]] += 1;
1537            }
1538        }
1539
1540        // Build col_ptr
1541        let mut col_ptr = vec![0usize; n_cols + 1];
1542        for c in 0..n_cols {
1543            col_ptr[c + 1] = col_ptr[c] + counts[c];
1544        }
1545
1546        // Fill rows and vals
1547        let mut rows = vec![0usize; csr_cols.len()];
1548        let mut vals = vec![0.0; csr_vals.len()];
1549        let mut col_counts = vec![0usize; n_cols];
1550
1551        for r in 0..n_rows {
1552            let start = row_ptr[r];
1553            let end = row_ptr[r + 1];
1554            for k in start..end {
1555                let c = csr_cols[k];
1556                let idx = col_ptr[c] + col_counts[c];
1557                rows[idx] = r;
1558                vals[idx] = csr_vals[k];
1559                col_counts[c] += 1;
1560            }
1561        }
1562
1563        Self {
1564            n_rows,
1565            n_cols,
1566            col_ptr,
1567            rows,
1568            vals,
1569        }
1570    }
1571
1572    /// Build a CSC matrix from COO triplets, summing duplicates
1573    /// (mirrors `coo_matrix(...).tocsc()`). Kept for backward compatibility.
1574    pub fn from_coo(
1575        n_rows: usize,
1576        n_cols: usize,
1577        row: &[usize],
1578        col: &[usize],
1579        data: &[f64],
1580    ) -> Self {
1581        debug_assert_eq!(row.len(), col.len());
1582        debug_assert_eq!(row.len(), data.len());
1583
1584        // Sort triplets by (column, row).
1585        let mut order: Vec<usize> = (0..data.len()).collect();
1586        order.sort_unstable_by_key(|&k| (col[k], row[k]));
1587
1588        let mut rows: Vec<usize> = Vec::with_capacity(data.len());
1589        let mut vals: Vec<f64> = Vec::with_capacity(data.len());
1590        let mut counts = vec![0usize; n_cols];
1591
1592        let mut pending: Option<(usize, usize, f64)> = None;
1593        for &k in &order {
1594            let key = (row[k], col[k]);
1595            match pending {
1596                None => pending = Some((key.0, key.1, data[k])),
1597                Some((pr, pc, pv)) if pr == key.0 && pc == key.1 => {
1598                    pending = Some((pr, pc, pv + data[k]));
1599                }
1600                Some((pr, pc, pv)) => {
1601                    rows.push(pr);
1602                    vals.push(pv);
1603                    counts[pc] += 1;
1604                    pending = Some((key.0, key.1, data[k]));
1605                }
1606            }
1607        }
1608        if let Some((pr, pc, pv)) = pending {
1609            rows.push(pr);
1610            vals.push(pv);
1611            counts[pc] += 1;
1612        }
1613
1614        let mut col_ptr = vec![0usize; n_cols + 1];
1615        for cc in 0..n_cols {
1616            col_ptr[cc + 1] = col_ptr[cc] + counts[cc];
1617        }
1618
1619        Self {
1620            n_rows,
1621            n_cols,
1622            col_ptr,
1623            rows,
1624            vals,
1625        }
1626    }
1627
1628    /// y = A x
1629    pub fn matvec(&self, x: &[f64]) -> Vec<f64> {
1630        let mut y = vec![0.0; self.n_rows];
1631        for c in 0..self.n_cols {
1632            let xc = x[c];
1633            for k in self.col_ptr[c]..self.col_ptr[c + 1] {
1634                y[self.rows[k]] += self.vals[k] * xc;
1635            }
1636        }
1637        y
1638    }
1639}
1640
1641/// Direct Lagrangian solver over an (N+1)x(N+1) augmented CSC system.
1642///
1643/// Mirrors Python `solve_direct_lagrange`: the augmented matrix is
1644/// [[K, c], [c^T, 0]] assembled by `assemble_torsion_lagrange`, solved with
1645/// RHS [f, 0]; the multiplier magnitude must satisfy
1646/// |u`N`| / max|u| <= 1e-7.
1647pub struct DirectLagrangeSolver {
1648    /// Size of the leading K block.
1649    pub n: usize,
1650    kernel: LagrangeKernelInstance,
1651    c: Vec<f64>,
1652}
1653
1654/// Numeric backend for the augmented direct solve.
1655#[derive(Debug, Clone, Copy, PartialEq, Eq)]
1656pub enum LagrangeKernel {
1657    /// Skyline LDL^T of the regularised leading block (always available).
1658    Skyline,
1659    /// Intel MKL PARDISO (requires the `pardiso` feature and MKL runtime).
1660    #[cfg(feature = "pardiso")]
1661    Pardiso,
1662}
1663
1664enum LagrangeKernelInstance {
1665    Skyline(SkylineLdlt),
1666    /// PARDISO needs &mut for its internal solve phases.
1667    #[cfg(feature = "pardiso")]
1668    Pardiso(std::sync::Mutex<crate::fea::solvers::pardiso::PardisoSolver>),
1669}
1670
1671/// Options controlling the direct Lagrangian solver behavior.
1672#[derive(Debug, Clone, Copy, PartialEq, Eq)]
1673pub struct SolverOptions {
1674    /// If true, automatically regularize a singular leading block by adding
1675    /// a small multiple of the identity (eps ~ 1e-9 * avg_diag). Default: false.
1676    ///
1677    /// When false (default), a singular leading block returns
1678    /// `FemError::SingularMatrix` instead of silently adding stiffness.
1679    /// This prevents masking modeling errors (missing boundary conditions,
1680    /// mechanisms, etc.) behind artificial stiffness.
1681    pub auto_regularize_singular: bool,
1682}
1683
1684impl Default for SolverOptions {
1685    fn default() -> Self {
1686        Self {
1687            auto_regularize_singular: false,
1688        }
1689    }
1690}
1691
1692impl DirectLagrangeSolver {
1693    /// Assemble the augmented Lagrangian matrix exactly as Python
1694    /// `Section.assemble_torsion` does: [[K, c], [c^T, 0]] in CSC format.
1695    pub fn assemble_torsion_lagrange(k: &SparseMatrix, c: &[f64]) -> CscMatrix {
1696        let n = k.n;
1697        let mut row: Vec<usize> = Vec::with_capacity(k.rows.len() + 2 * n);
1698        let mut col: Vec<usize> = Vec::with_capacity(row.capacity());
1699        let mut data: Vec<f64> = Vec::with_capacity(row.capacity());
1700        row.extend_from_slice(&k.rows);
1701        col.extend_from_slice(&k.cols);
1702        data.extend_from_slice(&k.vals);
1703        // column vector
1704        row.extend(0..n);
1705        col.extend(std::iter::repeat(n).take(n));
1706        data.extend_from_slice(c);
1707        // row vector
1708        row.extend(std::iter::repeat(n).take(n));
1709        col.extend(0..n);
1710        data.extend_from_slice(c);
1711        CscMatrix::from_coo(n + 1, n + 1, &row, &col, &data)
1712    }
1713
1714    /// Factor using the default backend: the skyline LDL^T, which is
1715    /// self-contained and robust.
1716    ///
1717    /// PARDISO is powerful but demands a fully provisioned MKL runtime;
1718    /// select it explicitly via [`DirectLagrangeSolver::with_kernel`] when
1719    /// the environment is known to be complete.
1720    pub fn new(k: &SparseMatrix, c: &[f64]) -> Result<Self, crate::mesh::fem::FemError> {
1721        Self::with_kernel(LagrangeKernel::Skyline, k, c, SolverOptions::default())
1722    }
1723
1724    /// Factor with an explicit kernel and options.
1725    pub fn with_kernel(
1726        kernel: LagrangeKernel,
1727        k: &SparseMatrix,
1728        c: &[f64],
1729        opts: SolverOptions,
1730    ) -> Result<Self, crate::mesh::fem::FemError> {
1731        let instance = match kernel {
1732            LagrangeKernel::Skyline => {
1733                let mut m = k.clone();
1734                m.compress();
1735                let m = match SkylineLdlt::factor(&m) {
1736                    Ok(ldlt) => ldlt,
1737                    Err(crate::mesh::fem::FemError::SingularMatrix)
1738                        if opts.auto_regularize_singular =>
1739                    {
1740                        let mut m_reg = k.clone();
1741                        m_reg.compress();
1742                        let mut diag_avg = 0.0;
1743                        for i in 0..m_reg.n {
1744                            diag_avg += m_reg.matvec_diag(i);
1745                        }
1746                        let eps = diag_avg.max(1e-300) / m_reg.n as f64 * 1e-9;
1747                        for i in 0..m_reg.n {
1748                            m_reg.add(i, i, eps);
1749                        }
1750                        m_reg.compress();
1751                        SkylineLdlt::factor(&m_reg)?
1752                    }
1753                    Err(e) => return Err(e),
1754                };
1755                LagrangeKernelInstance::Skyline(m)
1756            }
1757            #[cfg(feature = "pardiso")]
1758            LagrangeKernel::Pardiso => LagrangeKernelInstance::Pardiso(std::sync::Mutex::new(
1759                crate::fea::solvers::pardiso::PardisoSolver::new(k, c)
1760                    .map_err(|_| crate::mesh::fem::FemError::SingularMatrix)?,
1761            )),
1762        };
1763        Ok(Self {
1764            n: k.n,
1765            kernel: instance,
1766            c: c.to_vec(),
1767        })
1768    }
1769
1770    /// Solve [K c; c^T 0] [u; lam] = [f; 0] and return u.
1771    pub fn solve(&self, f: &[f64]) -> Result<Vec<f64>, crate::mesh::fem::FemError> {
1772        self.solve_full(f).map(|(u, _lam)| u)
1773    }
1774
1775    /// Solve returning `(u, lambda)`.
1776    pub fn solve_full(&self, f: &[f64]) -> Result<(Vec<f64>, f64), crate::mesh::fem::FemError> {
1777        match &self.kernel {
1778            LagrangeKernelInstance::Skyline(ldlt) => {
1779                let w1 = ldlt.solve(f)?;
1780                let w2 = ldlt.solve(&self.c)?;
1781                let ct_w2: f64 = self.c.iter().zip(w2.iter()).map(|(&a, &b)| a * b).sum();
1782                let ct_w1: f64 = self.c.iter().zip(w1.iter()).map(|(&a, &b)| a * b).sum();
1783                let lambda = if ct_w2.abs() > NEAR_ZERO_TOL_BASE {
1784                    ct_w1 / ct_w2
1785                } else {
1786                    0.0
1787                };
1788                let u = w1
1789                    .iter()
1790                    .zip(w2.iter())
1791                    .map(|(&a, &b)| a - lambda * b)
1792                    .collect();
1793                Ok((u, lambda))
1794            }
1795            #[cfg(feature = "pardiso")]
1796            LagrangeKernelInstance::Pardiso(p) => {
1797                // The augmented solve yields lambda as the last unknown.
1798                p.lock()
1799                    .unwrap()
1800                    .solve_with_multiplier(f)
1801                    .map_err(|e| crate::mesh::fem::FemError::ConvergenceFailed)
1802            }
1803        }
1804    }
1805
1806    /// Multiplier error metric |lam| / max|u| (Python's u[-1]/max|u|).
1807    pub fn multiplier_error(&self, f: &[f64], u: &[f64]) -> f64 {
1808        match self.solve_full(f) {
1809            Ok((_u_full, lambda)) => {
1810                let max_u = u.iter().fold(0.0f64, |a, &v| a.max(v.abs()));
1811                if max_u > 0.0 {
1812                    lambda.abs() / max_u
1813                } else {
1814                    f64::INFINITY
1815                }
1816            }
1817            Err(_) => f64::INFINITY,
1818        }
1819    }
1820}
1821/// Reverse Cuthill-McKee ordering for bandwidth reduction.
1822///
1823/// Returns `perm` where `perm[new_index] = old_index`.
1824fn rcm_order(n: usize, adj: &[Vec<usize>]) -> Vec<usize> {
1825    let mut visited = vec![false; n];
1826    let mut perm: Vec<usize> = Vec::with_capacity(n);
1827
1828    if n == 0 {
1829        return perm;
1830    }
1831
1832    let mut bfs_farthest = |from: usize| -> usize {
1833        for v in visited.iter_mut() {
1834            *v = false;
1835        }
1836        let mut queue = std::collections::VecDeque::new();
1837        queue.push_back(from);
1838        visited[from] = true;
1839        let mut last = from;
1840        while let Some(u) = queue.pop_front() {
1841            for &w in &adj[u] {
1842                if !visited[w] {
1843                    visited[w] = true;
1844                    queue.push_back(w);
1845                    last = w;
1846                }
1847            }
1848        }
1849        last
1850    };
1851
1852    let mut start = 0;
1853    while start < n && adj[start].is_empty() {
1854        start += 1;
1855    }
1856    if start >= n {
1857        return (0..n).collect();
1858    }
1859
1860    let peripheral = {
1861        let far = bfs_farthest(start);
1862        bfs_farthest(far)
1863    };
1864
1865    for v in visited.iter_mut() {
1866        *v = false;
1867    }
1868    let mut queue = std::collections::VecDeque::new();
1869    queue.push_back(peripheral);
1870    visited[peripheral] = true;
1871    while let Some(u) = queue.pop_front() {
1872        perm.push(u);
1873        let mut nbrs: Vec<usize> = adj[u].iter().filter(|&&w| !visited[w]).copied().collect();
1874        nbrs.sort_by_key(|&w| adj[w].len());
1875        for w in nbrs {
1876            visited[w] = true;
1877            queue.push_back(w);
1878        }
1879    }
1880    for i in 0..n {
1881        if !visited[i] {
1882            perm.push(i);
1883        }
1884    }
1885
1886    perm.reverse();
1887    perm
1888}
1889
1890/// Skyline (profile) LDL^T direct solver for sparse SPD matrices.
1891///
1892/// Factor once with [`SkylineLdlt::factor`], solve any number of right-hand
1893/// sides with [`SkylineLdlt::solve`]. RCM ordering keeps the profile small,
1894/// which makes 2D FE meshes (moderate bandwidth) very fast.
1895pub struct SkylineLdlt {
1896    n: usize,
1897    diag: Vec<f64>,
1898    /// First (permuted) column index stored in each row.
1899    first: Vec<usize>,
1900    /// Row-major skyline values for columns [first[i], i).
1901    lower: Vec<f64>,
1902    /// Offset of row i's segment within `lower`.
1903    row_start: Vec<usize>,
1904    /// RCM permutation: perm[new_index] = original_index.
1905    perm: Vec<usize>,
1906    /// Scale factor for scale-invariant tolerance (max diagonal entry).
1907    scale: f64,
1908}
1909
1910impl SkylineLdlt {
1911    /// Get the scale factor used for scale-invariant tolerances (max diagonal entry).
1912    pub fn scale(&self) -> f64 {
1913        self.scale
1914    }
1915
1916    pub fn factor(matrix: &SparseMatrix) -> Result<Self, crate::mesh::fem::FemError> {
1917        use std::collections::HashMap;
1918
1919        let n = matrix.n;
1920
1921        // Use CSR data if compressed, otherwise build from COO
1922        let map: HashMap<(usize, usize), f64> = if matrix.compressed {
1923            let mut m = HashMap::new();
1924            for r in 0..n {
1925                let start = matrix.row_ptr[r];
1926                let end = matrix.row_ptr[r + 1];
1927                for k in start..end {
1928                    let c = matrix.csr_cols[k];
1929                    *m.entry((r, c)).or_insert(0.0) += matrix.csr_vals[k];
1930                }
1931            }
1932            m
1933        } else {
1934            let mut m = HashMap::new();
1935            for k in 0..matrix.rows.len() {
1936                *m.entry((matrix.rows[k], matrix.cols[k])).or_insert(0.0) += matrix.vals[k];
1937            }
1938            m
1939        };
1940
1941        // Adjacency for RCM.
1942        let mut adj: Vec<Vec<usize>> = vec![Vec::new(); n];
1943        for &(r, c) in map.keys() {
1944            if r != c {
1945                adj[r].push(c);
1946            }
1947        }
1948        for a in adj.iter_mut() {
1949            a.sort_unstable();
1950            a.dedup();
1951        }
1952        let perm = rcm_order(n, &adj);
1953
1954        let mut old_to_new = vec![0usize; n];
1955        for (new_idx, &old_idx) in perm.iter().enumerate() {
1956            old_to_new[old_idx] = new_idx;
1957        }
1958
1959        // Skyline profile of the permuted lower triangle.
1960        let mut first = vec![n; n];
1961        for &(r, c) in map.keys() {
1962            let i = old_to_new[r];
1963            let j = old_to_new[c];
1964            let (hi, lo) = if i > j { (i, j) } else { (j, i) };
1965            if hi != lo && lo < first[hi] {
1966                first[hi] = lo;
1967            }
1968        }
1969        for i in 0..n {
1970            if first[i] > i {
1971                first[i] = i;
1972            }
1973        }
1974
1975        let mut row_start = vec![0usize; n + 1];
1976        for i in 0..n {
1977            row_start[i + 1] = row_start[i] + (i - first[i]);
1978        }
1979        let mut lower = vec![0.0; row_start[n]];
1980        let mut diag = vec![0.0; n];
1981
1982        // Scatter original values into the skyline (lower triangle).
1983        for (&(r, c), &v) in map.iter() {
1984            let i = old_to_new[r];
1985            let j = old_to_new[c];
1986            if i > j {
1987                lower[row_start[i] + (j - first[i])] = v;
1988            } else if j > i {
1989                lower[row_start[j] + (i - first[j])] = v;
1990            } else {
1991                diag[i] = v;
1992            }
1993        }
1994
1995        // In-place Crout LDL^T factorisation.
1996        // First pass: compute diagonal scale for scale-invariant pivot tolerance
1997        let mut diag_for_scale = vec![0.0f64; n];
1998        for i in 0..n {
1999            let _d = 0.0;
2000            for _k in first[i]..i {
2001                let _l_ik = 0.0; // L not yet computed, but we only need diag for scale
2002            }
2003            // We need a preliminary diagonal. Use matrix diagonal as approximation.
2004            // The actual diagonal will be computed during factorization.
2005            diag_for_scale[i] = 1.0; // placeholder, will use actual scale later
2006        }
2007        // Use matrix Frobenius norm as scale proxy for pivot tolerance
2008        let mut matrix_scale = 0.0f64;
2009        for (&(_r, _c), &v) in map.iter() {
2010            matrix_scale = matrix_scale.max(v.abs());
2011        }
2012        let pivot_tol = PIVOT_TOL_BASE * matrix_scale.max(1.0);
2013
2014        // In-place Crout LDL^T factorisation.
2015        for i in 0..n {
2016            let rs_i = row_start[i];
2017            for k in first[i]..i {
2018                let rs_k = row_start[k];
2019                let j0 = first[i].max(first[k]);
2020                let mut s = lower[rs_i + (k - first[i])];
2021                for j in j0..k {
2022                    s -= lower[rs_i + (j - first[i])] * lower[rs_k + (j - first[k])] * diag[j];
2023                }
2024                // Scale-invariant pivot check
2025                if diag[k].abs() < pivot_tol {
2026                    return Err(crate::mesh::fem::FemError::SingularMatrix);
2027                }
2028                lower[rs_i + (k - first[i])] = s / diag[k];
2029            }
2030            let mut d = diag[i];
2031            for k in first[i]..i {
2032                let l_ik = lower[rs_i + (k - first[i])];
2033                d -= l_ik * l_ik * diag[k];
2034            }
2035            if !(d > 0.0) || !d.is_finite() {
2036                return Err(crate::mesh::fem::FemError::SingularMatrix);
2037            }
2038            diag[i] = d;
2039        }
2040
2041        // Compute scale factor for scale-invariant tolerances (max diagonal)
2042        let scale = diag.iter().fold(0.0f64, |a, &v| a.max(v.abs()));
2043
2044        Ok(Self {
2045            n,
2046            diag,
2047            first,
2048            lower,
2049            row_start,
2050            perm,
2051            scale,
2052        })
2053    }
2054
2055    /// Solve A x = b.
2056    pub fn solve(&self, b: &[f64]) -> Result<Vec<f64>, crate::mesh::fem::FemError> {
2057        let n = self.n;
2058        if b.len() != n {
2059            return Err(crate::mesh::fem::FemError::InvalidMesh);
2060        }
2061
2062        // Permute the right-hand side into the RCM ordering.
2063        let mut x: Vec<f64> = self.perm.iter().map(|&old| b[old]).collect();
2064        for i in 0..n {
2065            let rs = self.row_start[i];
2066            let mut s = x[i];
2067            for k in self.first[i]..i {
2068                s -= self.lower[rs + (k - self.first[i])] * x[k];
2069            }
2070            x[i] = s;
2071        }
2072        // Diagonal scaling with scale-invariant tolerance.
2073        let pivot_tol = PIVOT_TOL_BASE * self.scale.max(1.0);
2074        for i in 0..n {
2075            if self.diag[i].abs() < pivot_tol {
2076                return Err(crate::mesh::fem::FemError::SingularMatrix);
2077            }
2078            x[i] /= self.diag[i];
2079        }
2080        // Backward substitution: L^T x = y via scattered updates.
2081        for i in (0..n).rev() {
2082            let rs = self.row_start[i];
2083            let xi = x[i];
2084            if xi != 0.0 {
2085                for k in self.first[i]..i {
2086                    x[k] -= self.lower[rs + (k - self.first[i])] * xi;
2087                }
2088            }
2089        }
2090        // Inverse permutation back to the original ordering.
2091        let mut out = vec![0.0; n];
2092        for (new_idx, &old_idx) in self.perm.iter().enumerate() {
2093            out[old_idx] = x[new_idx];
2094        }
2095        Ok(out)
2096    }
2097
2098    /// Solve with a Lagrange multiplier constraint vector, mirroring
2099    /// [`solve_lagrange_sparse`]: u = w1 - lambda * w2 with
2100    /// lambda = (c.w1)/(c.w2).
2101    pub fn solve_lagrange(
2102        &self,
2103        c: &[f64],
2104        f: &[f64],
2105    ) -> Result<Vec<f64>, crate::mesh::fem::FemError> {
2106        let w1 = self.solve(f)?;
2107        let w2 = self.solve(c)?;
2108        let ct_w2: f64 = c.iter().zip(w2.iter()).map(|(&a, &b)| a * b).sum();
2109        let ct_w1: f64 = c.iter().zip(w1.iter()).map(|(&a, &b)| a * b).sum();
2110        // Scale-invariant tolerance for Lagrange multiplier computation
2111        let near_zero_tol = NEAR_ZERO_TOL_BASE * self.scale.max(1.0);
2112        let lambda = if ct_w2.abs() > near_zero_tol {
2113            ct_w1 / ct_w2
2114        } else {
2115            0.0
2116        };
2117
2118        let mut u = vec![0.0; self.n];
2119        for i in 0..self.n {
2120            u[i] = w1[i] - lambda * w2[i];
2121        }
2122        Ok(u)
2123    }
2124}
2125pub mod mechanism;
2126pub mod solver;
2127pub mod solvers;
2128#[cfg(test)]
2129mod direct_lagrange_tests {
2130    use super::*;
2131
2132    #[test]
2133    fn csc_from_coo_sums_duplicates() {
2134        let m = CscMatrix::from_coo(3, 3, &[0, 1, 0], &[0, 1, 0], &[1.0, 2.0, 3.5]);
2135        // (0,0) should be 4.5 after duplicate summing
2136        let col0 = &m.vals[m.col_ptr[0]..m.col_ptr[1]];
2137        assert!((col0.iter().sum::<f64>() - 4.5).abs() < 1e-12);
2138        assert_eq!(m.col_ptr[2] - m.col_ptr[1], 1);
2139    }
2140
2141    #[test]
2142    fn augmented_direct_matches_schur() {
2143        // Same tridiagonal system as the skyline lagrange test.
2144        let n = 20;
2145        let mut k = SparseMatrix::new(n);
2146        for i in 0..n {
2147            k.add(i, i, 2.0);
2148            if i + 1 < n {
2149                k.add(i, i + 1, -1.0);
2150                k.add(i + 1, i, -1.0);
2151            }
2152        }
2153        let c: Vec<f64> = vec![1.0; n];
2154        let f: Vec<f64> = (0..n).map(|i| (i as f64) * 0.1 + 0.05).collect();
2155
2156        let u_ref = {
2157            let mut k2 = k.clone();
2158            k2.compress();
2159            SkylineLdlt::factor(&k2)
2160                .unwrap()
2161                .solve_lagrange(&c, &f)
2162                .unwrap()
2163        };
2164
2165        // Python-style: assemble augmented CSC, factor leading block.
2166        let k_lg = DirectLagrangeSolver::assemble_torsion_lagrange(&k, &c);
2167        assert_eq!(k_lg.n_rows, n + 1);
2168        assert_eq!(k_lg.n_cols, n + 1);
2169        let solver = DirectLagrangeSolver::new(&k, &c).unwrap();
2170        let u_dir = solver.solve(&f).unwrap();
2171
2172        for i in 0..n {
2173            assert!(
2174                (u_ref[i] - u_dir[i]).abs() < 5e-6,
2175                "i={} ref={} dir={}",
2176                i,
2177                u_ref[i],
2178                u_dir[i]
2179            );
2180        }
2181
2182        // NOTE: Python also checks |multiplier|/max|u| <= 1e-7; that holds
2183        // for genuine warping problems (c sums to ~0) but not for this
2184        // synthetic constraint, so it is not asserted here.
2185    }
2186}
2187#[cfg(test)]
2188mod skyline_tests {
2189    use super::*;
2190
2191    #[test]
2192    fn skyline_debug_4x4() {
2193        let mut a = SparseMatrix::new(4);
2194        let entries = [
2195            (0, 0, 4.0),
2196            (1, 1, 5.0),
2197            (2, 2, 6.0),
2198            (3, 3, 7.0),
2199            (0, 1, 1.0),
2200            (1, 0, 1.0),
2201            (1, 2, 2.0),
2202            (2, 1, 2.0),
2203            (2, 3, 1.0),
2204            (3, 2, 1.0),
2205        ];
2206        for &(r, c, v) in &entries {
2207            a.add(r, c, v);
2208        }
2209        let s = SkylineLdlt::factor(&a).unwrap();
2210        eprintln!(
2211            "perm-like diag={:?} first={:?} lower={:?}",
2212            s.diag, s.first, s.lower
2213        );
2214        let x = s.solve(&[1.0, 2.0, 3.0, 4.0]);
2215        eprintln!("x={:?} expected~[0.1934,0.2264,0.3374,0.5232]", x);
2216    }
2217
2218    #[test]
2219    fn skyline_matches_direct_dense_solve() {
2220        // SPD matrix: A = [[4,1,0,0],[1,5,2,0],[0,2,6,1],[0,0,1,7]]
2221        let mut a = SparseMatrix::new(4);
2222        let entries = [
2223            (0, 0, 4.0),
2224            (1, 1, 5.0),
2225            (2, 2, 6.0),
2226            (3, 3, 7.0),
2227            (0, 1, 1.0),
2228            (1, 0, 1.0),
2229            (1, 2, 2.0),
2230            (2, 1, 2.0),
2231            (2, 3, 1.0),
2232            (3, 2, 1.0),
2233        ];
2234        for &(r, c, v) in &entries {
2235            a.add(r, c, v);
2236        }
2237        let solver = SkylineLdlt::factor(&a).unwrap();
2238
2239        // Solve two right-hand sides and verify A x = b.
2240        for b in [vec![1.0, 2.0, 3.0, 4.0], vec![4.0, 3.0, 2.0, 1.0]] {
2241            let x = solver.solve(&b).unwrap();
2242            // residual check
2243            for row in 0..4 {
2244                let mut sum = 0.0;
2245                for &(r, c, v) in &entries {
2246                    if r == row {
2247                        sum += v * x[c];
2248                    }
2249                }
2250                assert!(
2251                    (sum - b[row]).abs() < 1e-10,
2252                    "row {} got {} (x={:?})",
2253                    row,
2254                    sum - b[row],
2255                    x
2256                );
2257            }
2258        }
2259
2260        // Compare against CG
2261        let mut a2 = a.clone();
2262        a2.compress();
2263        let b = vec![1.0, 2.0, 3.0, 4.0];
2264        let x_cg = cg_solve(&a2, &b, 2000, SOLVER_TOL).x;
2265        let x_dir = SkylineLdlt::factor(&a).unwrap().solve(&b).unwrap();
2266        for i in 0..4 {
2267            assert!((x_cg[i] - x_dir[i]).abs() < 1e-8);
2268        }
2269    }
2270
2271    #[test]
2272    fn skyline_lagrange_matches_sparse() {
2273        // Small FE-like Laplacian path with constraint vector.
2274        let n = 20;
2275        let mut k = SparseMatrix::new(n);
2276        for i in 0..n {
2277            k.add(i, i, 2.0);
2278            if i + 1 < n {
2279                k.add(i, i + 1, -1.0);
2280                k.add(i + 1, i, -1.0);
2281            }
2282        }
2283        let c: Vec<f64> = (0..n).map(|_i| 1.0).collect();
2284        let f: Vec<f64> = (0..n).map(|i| (i as f64) * 0.1 + 0.05).collect();
2285
2286        // High-precision CG reference
2287        let k_reg = {
2288            let mut m = k.clone();
2289            m.compress();
2290            for i in 0..n {
2291                m.add(i, i, 1e-10);
2292            }
2293            m.compress();
2294            m
2295        };
2296        let w1 = cg_solve(&k_reg, &f, 200000, 1e-13).x;
2297        let w2 = cg_solve(&k_reg, &c, 200000, 1e-13).x;
2298        let ct_w2: f64 = c.iter().zip(w2.iter()).map(|(&a, &b)| a * b).sum();
2299        let ct_w1: f64 = c.iter().zip(w1.iter()).map(|(&a, &b)| a * b).sum();
2300        // u = w1 - lambda*w2 with constraint c^T u = 0 gives
2301        // lambda = (c^T w1) / (c^T w2) = ct_w1 / ct_w2.
2302        let lambda = ct_w1 / ct_w2;
2303        let u_ref: Vec<f64> = w1
2304            .iter()
2305            .zip(w2.iter())
2306            .map(|(&a, &b)| a - lambda * b)
2307            .collect();
2308
2309        let u_dir = SkylineLdlt::factor(&k)
2310            .unwrap()
2311            .solve_lagrange(&c, &f)
2312            .unwrap();
2313        for i in 0..n {
2314            assert!(
2315                (u_ref[i] - u_dir[i]).abs() < 5e-6,
2316                "i={} ref={:.12} dir={:.12}",
2317                i,
2318                u_ref[i],
2319                u_dir[i]
2320            );
2321        }
2322    }
2323}
2324#[cfg(test)]
2325mod tests {
2326    use super::*;
2327
2328    fn rect_tri6() -> Tri6 {
2329        // Unit triangle with mid-nodes (CCW orientation)
2330        let points = [
2331            Point::new(0.0, 0.0),
2332            Point::new(1.0, 0.0),
2333            Point::new(0.0, 1.0),
2334            Point::new(0.5, 0.0),
2335            Point::new(0.5, 0.5),
2336            Point::new(0.0, 0.5),
2337        ];
2338        Tri6::from_points(0, points, [0, 1, 2, 3, 4, 5], 1.0, 1.0, 1.0).unwrap()
2339    }
2340
2341    #[test]
2342    fn gauss_points_sum_to_one() {
2343        for &n in &[1, 3, 4, 6] {
2344            let gps = gauss_points(n);
2345            assert_eq!(gps.len(), n, "gauss({n}) returned {} points", gps.len());
2346            let sum: f64 = gps.iter().map(|&(w, _, _, _)| w).sum();
2347            assert!((sum - 1.0).abs() < 1e-12, "gauss({n}): weights sum = {sum}");
2348        }
2349    }
2350
2351    #[test]
2352    fn torsion_constraint_is_elastic_modulus_weighted() {
2353        let points = [
2354            Point::new(0.0, 0.0),
2355            Point::new(1.0, 0.0),
2356            Point::new(0.0, 1.0),
2357            Point::new(0.5, 0.0),
2358            Point::new(0.5, 0.5),
2359            Point::new(0.0, 0.5),
2360        ];
2361        let tri = Tri6::from_points(0, points, [0, 1, 2, 3, 4, 5], 7.0, 1.0, 1.0).unwrap();
2362        let (_, _, constraint) = tri.torsion_properties();
2363
2364        // The 6-point rule exactly integrates corner functions to zero
2365        // and mid-side functions to A/3, multiplied by the elastic modulus.
2366        for i in 0..3 {
2367            assert!(
2368                constraint[i].abs() < 1e-12,
2369                "corner constraint[{}] = {}",
2370                i,
2371                constraint[i]
2372            );
2373        }
2374        for value in &constraint[3..] {
2375            assert!(
2376                (*value - 7.0 / 6.0).abs() < 1e-12,
2377                "mid-side constraint = {value}"
2378            );
2379        }
2380    }
2381
2382    #[test]
2383    fn tri6_area_unit_triangle() {
2384        let tri = rect_tri6();
2385        let (area, _, _, _, _, _) = tri.geometric_properties();
2386        assert!((area - 0.5).abs() < 1e-10, "area = {area}");
2387    }
2388
2389    #[test]
2390    fn torsion_shear_stress_uses_warping_solution() {
2391        let tri = rect_tri6();
2392        let zero_warping = [0.0; 6];
2393        // Nodal values of the linear field omega = x.
2394        let linear_warping = [0.0, 1.0, 0.0, 0.5, 0.5, 0.0];
2395
2396        let (zero_factor, _) = tri.torsion_shear_stress(&zero_warping);
2397        let (linear_factor, _) = tri.torsion_shear_stress(&linear_warping);
2398
2399        assert!(
2400            (linear_factor - zero_factor).abs() > 1e-6,
2401            "the torsion stress factor must vary with the warping solution"
2402        );
2403    }
2404
2405    #[test]
2406    fn tri6_centroid_unit_triangle() {
2407        let tri = rect_tri6();
2408        let (area, qx, qy, _, _, _) = tri.geometric_properties();
2409        let cx = qy / area;
2410        let cy = qx / area;
2411        assert!((cx - 1.0 / 3.0).abs() < 1e-10, "cx = {cx}");
2412        assert!((cy - 1.0 / 3.0).abs() < 1e-10, "cy = {cy}");
2413    }
2414
2415    #[test]
2416    fn tri6_moments_unit_triangle() {
2417        let tri = rect_tri6();
2418        let (_, _, _, ixx, iyy, ixy) = tri.geometric_properties();
2419        // For unit triangle about origin:
2420        // Ixx = ∫y² dA = 1/12, Iyy = ∫x² dA = 1/12, Ixy = ∫xy dA = 1/24
2421        assert!((ixx - 1.0 / 12.0).abs() < 1e-10, "ixx = {ixx}");
2422        assert!((iyy - 1.0 / 12.0).abs() < 1e-10, "iyy = {iyy}");
2423        assert!((ixy - 1.0 / 24.0).abs() < 1e-10, "ixy = {ixy}");
2424    }
2425
2426    #[test]
2427    fn principal_coordinate_zero_phi() {
2428        let (x11, y22) = principal_coordinate(0.0, 3.0, 4.0);
2429        assert!((x11 - 3.0).abs() < 1e-12);
2430        assert!((y22 - 4.0).abs() < 1e-12);
2431    }
2432
2433    #[test]
2434    fn principal_coordinate_90_degrees() {
2435        let (x11, y22) = principal_coordinate(std::f64::consts::FRAC_PI_2, 3.0, 0.0);
2436        assert!(x11.abs() < 1e-12);
2437        assert!((y22 + 3.0).abs() < 1e-12);
2438    }
2439
2440    #[test]
2441    fn tri3_to_tri6_creates_midpoints() {
2442        let nodes = vec![
2443            Point::new(0.0, 0.0),
2444            Point::new(1.0, 0.0),
2445            Point::new(0.0, 1.0),
2446        ];
2447        let elements = vec![[0, 1, 2]];
2448        let mesh = tri3_to_tri6(&nodes, &elements);
2449        assert_eq!(mesh.nodes.len(), 6); // 3 original + 3 mid
2450        assert_eq!(mesh.elements.len(), 1);
2451        // Check mid-node of edge 0-1
2452        let mid01 = mesh.nodes[mesh.elements[0][3]];
2453        assert!((mid01.x - 0.5).abs() < 1e-12);
2454        assert!(mid01.y.abs() < 1e-12);
2455    }
2456
2457    #[test]
2458    fn tri3_to_tri6_shares_midpoints() {
2459        let nodes = vec![
2460            Point::new(0.0, 0.0),
2461            Point::new(1.0, 0.0),
2462            Point::new(0.0, 1.0),
2463            Point::new(1.0, 1.0),
2464        ];
2465        let elements = vec![[0, 1, 2], [1, 3, 2]];
2466        let mesh = tri3_to_tri6(&nodes, &elements);
2467        // 4 original + 5 unique edges (01, 12, 20, 13, 32) = 9 nodes
2468        assert_eq!(mesh.nodes.len(), 9);
2469        // Edge 1-2 is shared, so mid12 of elem0 == mid20 of elem1
2470        assert_eq!(mesh.elements[0][4], mesh.elements[1][5]);
2471    }
2472
2473    #[test]
2474    fn solve_dense_simple() {
2475        let mut a = vec![vec![2.0, 1.0], vec![1.0, 3.0]];
2476        let mut b = vec![5.0, 10.0];
2477        solve_dense(&mut a, &mut b).unwrap();
2478        // Solution: 2x + y = 5, x + 3y = 10 => x = 10-3y, 2(10-3y)+y = 5 => 20-5y = 5 => y = 3, x = 1
2479        assert!((b[0] - 1.0).abs() < 1e-10, "x = {}", b[0]);
2480        assert!((b[1] - 3.0).abs() < 1e-10, "y = {}", b[1]);
2481    }
2482
2483    #[test]
2484    fn solve_lagrange_simple() {
2485        // Simple test: K = I, c = [1, 1], f = [1, 0]
2486        // Solution should satisfy u + lambda * c = f and c^T u = 0
2487        let k = vec![vec![1.0, 0.0], vec![0.0, 1.0]];
2488        let c = vec![1.0, 1.0];
2489        let f = vec![1.0, 0.0];
2490        let u = solve_lagrange(&k, &c, &f).unwrap();
2491        // u0 + lambda = 1, u1 + lambda = 0, u0 + u1 = 0
2492        // => u0 = -u1, -u1 + lambda = 1, u1 + lambda = 0
2493        // => lambda = -u1, -u1 - u1 = 1 => u1 = -0.5, u0 = 0.5, lambda = 0.5
2494        assert!((u[0] - 0.5).abs() < 1e-10, "u0 = {}", u[0]);
2495        assert!((u[1] + 0.5).abs() < 1e-10, "u1 = {}", u[1]);
2496        // Direct constraint check: c^T u = 0 (independent of lambda formula)
2497        let ct_u: f64 = c.iter().zip(u.iter()).map(|(a, b)| a * b).sum();
2498        assert!(ct_u.abs() < 1e-10, "constraint residual c^T u = {}", ct_u);
2499    }
2500
2501    #[test]
2502    fn skyline_lagrange_constraint_residual() {
2503        // Verify c^T u ≈ 0 for SkylineLdlt::solve_lagrange (catches lambda formula bugs)
2504        let n = 10;
2505        let mut k = SparseMatrix::new(n);
2506        for i in 0..n {
2507            k.add(i, i, 2.0);
2508            if i + 1 < n {
2509                k.add(i, i + 1, -1.0);
2510                k.add(i + 1, i, -1.0);
2511            }
2512        }
2513        let c: Vec<f64> = vec![1.0; n];
2514        let f: Vec<f64> = (0..n).map(|i| (i as f64) * 0.1 + 0.05).collect();
2515
2516        let u = {
2517            let mut k2 = k.clone();
2518            k2.compress();
2519            SkylineLdlt::factor(&k2)
2520                .unwrap()
2521                .solve_lagrange(&c, &f)
2522                .unwrap()
2523        };
2524        let ct_u: f64 = c.iter().zip(u.iter()).map(|(a, b)| a * b).sum();
2525        assert!(
2526            ct_u.abs() < 1e-10,
2527            "SkylineLdlt constraint residual c^T u = {}",
2528            ct_u
2529        );
2530    }
2531
2532    #[test]
2533    fn cg_solve_breakdown_detection() {
2534        // Non-SPD matrix with negative diagonal: A = [[-1, 0], [0, 1]]
2535        // This will have p^T A p <= 0 on the first iteration
2536        let mut a = SparseMatrix::new(2);
2537        a.add(0, 0, -1.0);
2538        a.add(1, 1, 1.0);
2539        let b = vec![1.0, 1.0];
2540        a.compress();
2541        let result = cg_solve(&a, &b, 100, SOLVER_TOL);
2542        // Should detect non-SPD (p^T A p <= 0) and not converge
2543        assert!(
2544            !result.converged(),
2545            "CG should not converge on non-SPD matrix"
2546        );
2547        assert_eq!(result.status, CgStatus::NotPositiveDefinite);
2548        // iterations may be 0 if breakdown happens on first check
2549    }
2550
2551    #[test]
2552    fn cg_solve_finite_checks() {
2553        // SPD matrix: A = [[2, 1], [1, 2]]
2554        let mut a = SparseMatrix::new(2);
2555        a.add(0, 0, 2.0);
2556        a.add(0, 1, 1.0);
2557        a.add(1, 0, 1.0);
2558        a.add(1, 1, 2.0);
2559        let b = vec![1.0, 2.0];
2560        a.compress();
2561        let result = cg_solve(&a, &b, 100, SOLVER_TOL);
2562        assert!(
2563            result.converged(),
2564            "CG should converge on SPD matrix: {}",
2565            result.residual
2566        );
2567        assert_eq!(result.status, CgStatus::Converged);
2568        // Check all output values are finite
2569        assert!(
2570            result.x.iter().all(|v| v.is_finite()),
2571            "Solution has non-finite values"
2572        );
2573        assert!(result.residual.is_finite(), "Residual is non-finite");
2574    }
2575
2576    #[test]
2577    fn extrapolate_to_nodes_constant() {
2578        // Constant values should extrapolate to the same constant
2579        let w = [1.0; 6];
2580        let nodes = extrapolate_to_nodes(&w);
2581        for &v in &nodes {
2582            assert!((v - 1.0).abs() < 1e-10, "v = {v}");
2583        }
2584    }
2585
2586    #[test]
2587    fn tri6_degenerate_element_detection() {
2588        // Create a degenerate triangle (collinear points)
2589        let nodes = vec![
2590            Point::new(0.0, 0.0),
2591            Point::new(1.0, 0.0),
2592            Point::new(2.0, 0.0), // collinear!
2593            Point::new(0.5, 0.0), // mid01
2594            Point::new(1.5, 0.0), // mid12
2595            Point::new(1.0, 0.0), // mid20
2596        ];
2597        let elements = vec![[0, 1, 2, 3, 4, 5]];
2598        let mesh = Tri6Mesh { nodes, elements };
2599
2600        // validate_tri6_mesh should return DegenerateElement error
2601        let result = validate_tri6_mesh(&mesh);
2602        assert!(result.is_err());
2603        assert_eq!(
2604            result.unwrap_err(),
2605            crate::mesh::fem::FemError::DegenerateElement
2606        );
2607
2608        // build_tri6_elements should return DegenerateElement error
2609        let result = build_tri6_elements(&mesh, 200e9, 80e9, 7850.0);
2610        assert!(result.is_err());
2611        assert_eq!(
2612            result.unwrap_err(),
2613            crate::mesh::fem::FemError::DegenerateElement
2614        );
2615    }
2616
2617    #[test]
2618    fn tri6_orientation_check() {
2619        // Valid CCW triangle should work
2620        let points_ccw = [
2621            Point::new(0.0, 0.0),
2622            Point::new(1.0, 0.0),
2623            Point::new(0.0, 1.0),
2624            Point::new(0.5, 0.0),
2625            Point::new(0.5, 0.5),
2626            Point::new(0.0, 0.5),
2627        ];
2628        let result = Tri6::from_points(0, points_ccw, [0, 1, 2, 3, 4, 5], 1.0, 1.0, 1.0);
2629        assert!(result.is_ok(), "CCW triangle should be valid");
2630
2631        // CW triangle should fail
2632        let points_cw = [
2633            Point::new(0.0, 0.0),
2634            Point::new(0.0, 1.0),
2635            Point::new(1.0, 0.0),
2636            Point::new(0.0, 0.5),
2637            Point::new(0.5, 0.5),
2638            Point::new(0.5, 0.0),
2639        ];
2640        let result = Tri6::from_points(0, points_cw, [0, 1, 2, 3, 4, 5], 1.0, 1.0, 1.0);
2641        assert!(result.is_err(), "CW triangle should fail");
2642        assert_eq!(
2643            result.unwrap_err(),
2644            crate::mesh::fem::FemError::InvalidElementOrientation
2645        );
2646    }
2647}