1use crate::geometry::Point;
8use std::collections::HashMap;
9
10#[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
48pub const GEOMETRY_TOL: f64 = 1e-12;
54
55pub const JACOBIAN_TOL: f64 = 1e-20;
57
58pub const SOLVER_TOL: f64 = 1e-12;
60
61pub const PIVOT_TOL_BASE: f64 = 1e-15;
64
65pub const NEAR_ZERO_TOL_BASE: f64 = 1e-15;
68
69#[deprecated(since = "0.2.0", note = "Use PIVOT_TOL_BASE")]
71pub const PIVOT_TOL: f64 = PIVOT_TOL_BASE;
72
73#[deprecated(since = "0.2.0", note = "Use NEAR_ZERO_TOL_BASE")]
75pub const NEAR_ZERO_TOL: f64 = NEAR_ZERO_TOL_BASE;
76
77pub 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 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
123pub struct ShapeFunctionResult {
129 pub n: [f64; 6],
131 pub b: [[f64; 6]; 2],
134 pub j: f64,
136 pub x: f64,
138 pub y: f64,
140}
141
142pub fn shape_function(coords: &[[f64; 6]; 2], gp: (f64, f64, f64)) -> ShapeFunctionResult {
147 let (eta, xi, zeta) = gp;
148
149 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 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 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 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 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 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 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
225fn 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
250const 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
306pub 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
317pub 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
339pub 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
353pub 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#[derive(Debug, Clone)]
377pub struct Tri6 {
378 pub el_id: usize,
380 pub coords: [[f64; 6]; 2],
382 pub node_ids: [usize; 6],
384 pub elastic_modulus: f64,
386 pub shear_modulus: f64,
388 pub density: f64,
390}
391
392impl Tri6 {
393 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 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 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 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 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 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[i] += weight * (sf.b[0][i] * sf.y - sf.b[1][i] * sf.x);
471 c_el[i] += weight * sf.n[i];
473 }
474 }
475
476 (k_el, f_el, c_el)
477 }
478
479 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[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[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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 [
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#[derive(Debug, Clone)]
790pub struct Tri6Mesh {
791 pub nodes: Vec<Point>,
793 pub elements: Vec<[usize; 6]>,
795}
796
797pub fn tri3_to_tri6(tri3_nodes: &[Point], tri3_elements: &[[usize; 3]]) -> Tri6Mesh {
802 let mut nodes: Vec<Point> = tri3_nodes.to_vec();
803 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 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 elem[0..3].reverse(); 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
843pub 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
884pub 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 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#[derive(Debug, Clone)]
950pub struct SparseMatrix {
951 pub n: usize,
952 rows: Vec<usize>,
954 cols: Vec<usize>,
955 vals: Vec<f64>,
956 row_ptr: Vec<usize>,
958 csr_cols: Vec<usize>,
959 csr_vals: Vec<f64>,
960 diag: Vec<f64>,
962 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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
1208pub fn cg_solve(a: &SparseMatrix, b: &[f64], max_iter: usize, tol: f64) -> CgResult {
1213 let n = b.len();
1214
1215 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 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 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#[derive(Debug, Clone, Copy, PartialEq, Eq)]
1338pub enum CgStatus {
1339 Converged,
1341 MaxIterations,
1343 NotPositiveDefinite,
1345 Breakdown,
1347 InvalidInput,
1349}
1350
1351#[derive(Debug, Clone)]
1353pub struct CgResult {
1354 pub x: Vec<f64>,
1356 pub iterations: usize,
1358 pub residual: f64,
1360 pub status: CgStatus,
1362}
1363
1364impl CgResult {
1365 pub fn converged(&self) -> bool {
1367 matches!(self.status, CgStatus::Converged)
1368 }
1369}
1370
1371pub 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
1385pub 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 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 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 let (w1, w2) = if w1.converged() && w2.converged() {
1414 (w1.x, w2.x)
1415 } else {
1416 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
1449pub 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 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_dense(&mut a, &mut rhs)?;
1479 Ok(rhs[..n].to_vec())
1480}
1481
1482pub fn solve_dense(a: &mut [Vec<f64>], b: &mut [f64]) -> Result<(), crate::fea::FemError> {
1485 let n = a.len();
1486 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 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 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#[derive(Debug, Clone)]
1544pub struct CscMatrix {
1545 pub n_rows: usize,
1546 pub n_cols: usize,
1547 pub col_ptr: Vec<usize>,
1549 pub rows: Vec<usize>,
1551 pub vals: Vec<f64>,
1553}
1554
1555impl CscMatrix {
1556 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 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 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 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 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 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 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
1679pub struct DirectLagrangeSolver {
1686 pub n: usize,
1688 kernel: LagrangeKernelInstance,
1689 c: Vec<f64>,
1690}
1691
1692#[derive(Debug, Clone, Copy, PartialEq, Eq)]
1694pub enum LagrangeKernel {
1695 Skyline,
1697}
1698
1699enum LagrangeKernelInstance {
1700 Skyline(SkylineLdlt),
1701}
1702
1703#[derive(Debug, Clone, Copy, PartialEq, Eq)]
1705pub struct SolverOptions {
1706 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 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 row.extend(0..n);
1737 col.extend(std::iter::repeat(n).take(n));
1738 data.extend_from_slice(c);
1739 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 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 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 pub fn solve(&self, f: &[f64]) -> Result<Vec<f64>, crate::fea::FemError> {
1793 self.solve_full(f).map(|(u, _lam)| u)
1794 }
1795
1796 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 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}
1834fn 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
1903pub struct SkylineLdlt {
1909 n: usize,
1910 diag: Vec<f64>,
1911 first: Vec<usize>,
1913 lower: Vec<f64>,
1915 row_start: Vec<usize>,
1917 perm: Vec<usize>,
1919 scale: f64,
1921}
1922
1923impl SkylineLdlt {
1924 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 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 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 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 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 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; }
2016 diag_for_scale[i] = 1.0; }
2020 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 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 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 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 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 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 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 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 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 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 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 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 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 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 }
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 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 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 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 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 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 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 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 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 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 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 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); assert_eq!(mesh.elements.len(), 1);
2459 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 assert_eq!(mesh.nodes.len(), 9);
2477 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 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 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 assert!((u[0] - 0.5).abs() < 1e-10, "u0 = {}", u[0]);
2503 assert!((u[1] + 0.5).abs() < 1e-10, "u1 = {}", u[1]);
2504 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 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 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 assert!(
2552 !result.converged(),
2553 "CG should not converge on non-SPD matrix"
2554 );
2555 assert_eq!(result.status, CgStatus::NotPositiveDefinite);
2556 }
2558
2559 #[test]
2560 fn cg_solve_finite_checks() {
2561 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 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 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 let nodes = vec![
2598 Point::new(0.0, 0.0),
2599 Point::new(1.0, 0.0),
2600 Point::new(2.0, 0.0), Point::new(0.5, 0.0), Point::new(1.5, 0.0), Point::new(1.0, 0.0), ];
2605 let elements = vec![[0, 1, 2, 3, 4, 5]];
2606 let mesh = Tri6Mesh { nodes, elements };
2607
2608 let result = validate_tri6_mesh(&mesh);
2610 assert!(result.is_err());
2611 assert_eq!(result.unwrap_err(), crate::fea::FemError::DegenerateElement);
2612
2613 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 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 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}