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