1use crate::geometry::Point;
8use std::collections::HashMap;
9
10pub const GEOMETRY_TOL: f64 = 1e-12;
16
17pub const JACOBIAN_TOL: f64 = 1e-20;
19
20pub const SOLVER_TOL: f64 = 1e-12;
22
23pub const PIVOT_TOL_BASE: f64 = 1e-15;
26
27pub const NEAR_ZERO_TOL_BASE: f64 = 1e-15;
30
31#[deprecated(since = "0.2.0", note = "Use PIVOT_TOL_BASE")]
33pub const PIVOT_TOL: f64 = PIVOT_TOL_BASE;
34
35#[deprecated(since = "0.2.0", note = "Use NEAR_ZERO_TOL_BASE")]
37pub const NEAR_ZERO_TOL: f64 = NEAR_ZERO_TOL_BASE;
38
39pub fn gauss_points(n: usize) -> Vec<(f64, f64, f64, f64)> {
50 match n {
51 1 => vec![(1.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0)],
52 3 => vec![
53 (1.0 / 3.0, 2.0 / 3.0, 1.0 / 6.0, 1.0 / 6.0),
54 (1.0 / 3.0, 1.0 / 6.0, 2.0 / 3.0, 1.0 / 6.0),
55 (1.0 / 3.0, 1.0 / 6.0, 1.0 / 6.0, 2.0 / 3.0),
56 ],
57 4 => vec![
58 (-27.0 / 48.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0),
59 (25.0 / 48.0, 0.6, 0.2, 0.2),
60 (25.0 / 48.0, 0.2, 0.6, 0.2),
61 (25.0 / 48.0, 0.2, 0.2, 0.6),
62 ],
63 6 => {
64 let root = (38.0 - 44.0 * (2.0_f64 / 5.0).sqrt()).sqrt();
67 let g1 = (8.0 - 10.0_f64.sqrt() + root) / 18.0;
68 let g2 = (8.0 - 10.0_f64.sqrt() - root) / 18.0;
69 let weight_root = (213_125.0 - 53_320.0 * 10.0_f64.sqrt()).sqrt();
70 let w1 = (620.0 + weight_root) / 3720.0;
71 let w2 = (620.0 - weight_root) / 3720.0;
72 vec![
73 (w2, 1.0 - 2.0 * g2, g2, g2),
74 (w2, g2, 1.0 - 2.0 * g2, g2),
75 (w2, g2, g2, 1.0 - 2.0 * g2),
76 (w1, g1, g1, 1.0 - 2.0 * g1),
77 (w1, 1.0 - 2.0 * g1, g1, g1),
78 (w1, g1, 1.0 - 2.0 * g1, g1),
79 ]
80 }
81 _ => panic!("gauss_points: n must be 1, 3, 4, or 6, got {n}"),
82 }
83}
84
85pub struct ShapeFunctionResult {
91 pub n: [f64; 6],
93 pub b: [[f64; 6]; 2],
96 pub j: f64,
98 pub x: f64,
100 pub y: f64,
102}
103
104pub fn shape_function(coords: &[[f64; 6]; 2], gp: (f64, f64, f64)) -> ShapeFunctionResult {
109 let (eta, xi, zeta) = gp;
110
111 let n = [
113 eta * (2.0 * eta - 1.0),
114 xi * (2.0 * xi - 1.0),
115 zeta * (2.0 * zeta - 1.0),
116 4.0 * eta * xi,
117 4.0 * xi * zeta,
118 4.0 * eta * zeta,
119 ];
120
121 let b_iso = [
123 [4.0 * eta - 1.0, 0.0, 0.0, 4.0 * xi, 0.0, 4.0 * zeta],
124 [0.0, 4.0 * xi - 1.0, 0.0, 4.0 * eta, 4.0 * zeta, 0.0],
125 [0.0, 0.0, 4.0 * zeta - 1.0, 0.0, 4.0 * xi, 4.0 * eta],
126 ];
127
128 let mut j_mat = [[1.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 0.0, 0.0]];
133 for k in 0..3 {
134 for i in 0..6 {
135 j_mat[k][1] += b_iso[k][i] * coords[0][i];
136 j_mat[k][2] += b_iso[k][i] * coords[1][i];
137 }
138 }
139
140 let det_j = j_mat[0][0] * (j_mat[1][1] * j_mat[2][2] - j_mat[1][2] * j_mat[2][1])
142 - j_mat[0][1] * (j_mat[1][0] * j_mat[2][2] - j_mat[1][2] * j_mat[2][0])
143 + j_mat[0][2] * (j_mat[1][0] * j_mat[2][1] - j_mat[1][1] * j_mat[2][0]);
144 let jacobian = 0.5 * det_j;
145
146 if jacobian.abs() <= JACOBIAN_TOL {
149 return ShapeFunctionResult {
150 n,
151 b: [[0.0; 6]; 2],
152 j: jacobian,
153 x: 0.0,
154 y: 0.0,
155 };
156 }
157
158 let j_inv = inv3x3(&j_mat);
162 let mut b = [[0.0; 6]; 2];
163 for j in 0..6 {
164 for k in 0..3 {
165 b[0][j] += j_inv[1][k] * b_iso[k][j];
166 b[1][j] += j_inv[2][k] * b_iso[k][j];
167 }
168 }
169
170 let mut x = 0.0;
172 let mut y = 0.0;
173 for i in 0..6 {
174 x += n[i] * coords[0][i];
175 y += n[i] * coords[1][i];
176 }
177
178 ShapeFunctionResult {
179 n,
180 b,
181 j: jacobian,
182 x,
183 y,
184 }
185}
186
187fn inv3x3(m: &[[f64; 3]; 3]) -> [[f64; 3]; 3] {
189 let det = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
190 - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
191 + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
192 let inv_det = 1.0 / det;
193 [
194 [
195 (m[1][1] * m[2][2] - m[1][2] * m[2][1]) * inv_det,
196 (m[0][2] * m[2][1] - m[0][1] * m[2][2]) * inv_det,
197 (m[0][1] * m[1][2] - m[0][2] * m[1][1]) * inv_det,
198 ],
199 [
200 (m[1][2] * m[2][0] - m[1][0] * m[2][2]) * inv_det,
201 (m[0][0] * m[2][2] - m[0][2] * m[2][0]) * inv_det,
202 (m[0][2] * m[1][0] - m[0][0] * m[1][2]) * inv_det,
203 ],
204 [
205 (m[1][0] * m[2][1] - m[1][1] * m[2][0]) * inv_det,
206 (m[0][1] * m[2][0] - m[0][0] * m[2][1]) * inv_det,
207 (m[0][0] * m[1][1] - m[0][1] * m[1][0]) * inv_det,
208 ],
209 ]
210}
211
212const H_INV: [[f64; 6]; 6] = [
218 [
219 1.87365927351160,
220 0.138559587411935,
221 0.138559587411935,
222 -0.638559587411936,
223 0.126340726488397,
224 -0.638559587411935,
225 ],
226 [
227 0.138559587411935,
228 1.87365927351160,
229 0.138559587411935,
230 -0.638559587411935,
231 -0.638559587411935,
232 0.126340726488397,
233 ],
234 [
235 0.138559587411935,
236 0.138559587411935,
237 1.87365927351160,
238 0.126340726488396,
239 -0.638559587411935,
240 -0.638559587411935,
241 ],
242 [
243 0.0749010751157440,
244 0.0749010751157440,
245 0.180053080734478,
246 1.36051633430762,
247 -0.345185782636792,
248 -0.345185782636792,
249 ],
250 [
251 0.180053080734478,
252 0.0749010751157440,
253 0.0749010751157440,
254 -0.345185782636792,
255 1.36051633430762,
256 -0.345185782636792,
257 ],
258 [
259 0.0749010751157440,
260 0.180053080734478,
261 0.0749010751157440,
262 -0.345185782636792,
263 -0.345185782636792,
264 1.36051633430762,
265 ],
266];
267
268pub fn extrapolate_to_nodes(w: &[f64; 6]) -> [f64; 6] {
270 let mut result = [0.0; 6];
271 for i in 0..6 {
272 for j in 0..6 {
273 result[i] += H_INV[i][j] * w[j];
274 }
275 }
276 result
277}
278
279pub fn principal_coordinate(phi: f64, x: f64, y: f64) -> (f64, f64) {
296 let cos_phi = phi.cos();
297 let sin_phi = phi.sin();
298 (x * cos_phi + y * sin_phi, y * cos_phi - x * sin_phi)
299}
300
301pub fn global_coordinate(phi: f64, x11: f64, y22: f64) -> (f64, f64) {
310 let cos_phi = phi.cos();
311 let sin_phi = phi.sin();
312 (x11 * cos_phi - y22 * sin_phi, x11 * sin_phi + y22 * cos_phi)
313}
314
315pub fn shear_parameter(nx: f64, ny: f64, ixx: f64, iyy: f64, ixy: f64) -> [f64; 6] {
321 let r = nx * nx - ny * ny;
322 let q = 2.0 * nx * ny;
323 let d1 = ixx * r - ixy * q;
324 let d2 = ixy * r + ixx * q;
325 let h1 = -ixy * r + iyy * q;
326 let h2 = -iyy * r - ixy * q;
327 [r, q, d1, d2, h1, h2]
328}
329
330#[derive(Debug, Clone)]
339pub struct Tri6 {
340 pub el_id: usize,
342 pub coords: [[f64; 6]; 2],
344 pub node_ids: [usize; 6],
346 pub elastic_modulus: f64,
348 pub shear_modulus: f64,
350 pub density: f64,
352}
353
354impl Tri6 {
355 pub fn from_points(
361 el_id: usize,
362 points: [Point; 6],
363 node_ids: [usize; 6],
364 em: f64,
365 gm: f64,
366 rho: f64,
367 ) -> Result<Self, crate::mesh::fem::FemError> {
368 let mut coords = [[0.0; 6]; 2];
369 for i in 0..6 {
370 coords[0][i] = points[i].x;
371 coords[1][i] = points[i].y;
372 }
373 let sf = shape_function(&coords, (1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0));
375 if sf.j.abs() <= JACOBIAN_TOL {
376 return Err(crate::mesh::fem::FemError::DegenerateElement);
377 }
378 if sf.j < 0.0 {
379 return Err(crate::mesh::fem::FemError::InvalidElementOrientation);
380 }
381 Ok(Self {
382 el_id,
383 coords,
384 node_ids,
385 elastic_modulus: em,
386 shear_modulus: gm,
387 density: rho,
388 })
389 }
390
391 pub fn geometric_properties(&self) -> (f64, f64, f64, f64, f64, f64) {
393 let mut area = 0.0;
394 let mut qx = 0.0;
395 let mut qy = 0.0;
396 let mut ixx = 0.0;
397 let mut iyy = 0.0;
398 let mut ixy = 0.0;
399
400 for &(w, eta, xi, zeta) in &gauss_points(4) {
401 let sf = shape_function(&self.coords, (eta, xi, zeta));
402 let weight = w * sf.j;
403 area += weight;
404 qx += weight * sf.y;
405 qy += weight * sf.x;
406 ixx += weight * sf.y * sf.y;
407 iyy += weight * sf.x * sf.x;
408 ixy += weight * sf.y * sf.x;
409 }
410
411 (area, qx, qy, ixx, iyy, ixy)
412 }
413
414 pub fn torsion_properties(&self) -> ([[f64; 6]; 6], [f64; 6], [f64; 6]) {
416 let mut k_el = [[0.0; 6]; 6];
417 let mut f_el = [0.0; 6];
418 let mut c_el = [0.0; 6];
419
420 for &(w, eta, xi, zeta) in &gauss_points(6) {
423 let sf = shape_function(&self.coords, (eta, xi, zeta));
424 let weight = w * sf.j * self.elastic_modulus;
425
426 for i in 0..6 {
428 for j in 0..6 {
429 k_el[i][j] += weight * (sf.b[0][i] * sf.b[0][j] + sf.b[1][i] * sf.b[1][j]);
430 }
431 f_el[i] += weight * (sf.b[0][i] * sf.y - sf.b[1][i] * sf.x);
433 c_el[i] += weight * sf.n[i];
435 }
436 }
437
438 (k_el, f_el, c_el)
439 }
440
441 pub fn shear_load_vectors(
443 &self,
444 ixx: f64,
445 iyy: f64,
446 ixy: f64,
447 nu: f64,
448 ) -> ([f64; 6], [f64; 6]) {
449 let mut f_psi = [0.0; 6];
450 let mut f_phi = [0.0; 6];
451
452 for &(w, eta, xi, zeta) in &gauss_points(4) {
453 let sf = shape_function(&self.coords, (eta, xi, zeta));
454 let weight = w * sf.j * self.elastic_modulus;
455 let [_, _, d1, d2, h1, h2] = shear_parameter(sf.x, sf.y, ixx, iyy, ixy);
456
457 for i in 0..6 {
458 f_psi[i] += weight
460 * (nu / 2.0 * (sf.b[0][i] * d1 + sf.b[1][i] * d2)
461 + 2.0 * (1.0 + nu) * sf.n[i] * (ixx * sf.x - ixy * sf.y));
462 f_phi[i] += weight
464 * (nu / 2.0 * (sf.b[0][i] * h1 + sf.b[1][i] * h2)
465 + 2.0 * (1.0 + nu) * sf.n[i] * (iyy * sf.y - ixy * sf.x));
466 }
467 }
468
469 (f_psi, f_phi)
470 }
471
472 pub fn shear_warping_integrals(
476 &self,
477 ixx: f64,
478 iyy: f64,
479 ixy: f64,
480 omega: &[f64; 6],
481 ) -> (f64, f64, f64, f64, f64, f64) {
482 let mut sc_xint = 0.0;
483 let mut sc_yint = 0.0;
484 let mut q_omega = 0.0;
485 let mut i_omega = 0.0;
486 let mut i_xomega = 0.0;
487 let mut i_yomega = 0.0;
488
489 for &(w, eta, xi, zeta) in &gauss_points(4) {
490 let sf = shape_function(&self.coords, (eta, xi, zeta));
491 let weight = w * sf.j * self.elastic_modulus;
492
493 let n_omega: f64 = (0..6).map(|i| sf.n[i] * omega[i]).sum();
494 let r2 = sf.x * sf.x + sf.y * sf.y;
495
496 sc_xint += weight * (iyy * sf.x + ixy * sf.y) * r2;
497 sc_yint += weight * (ixx * sf.y + ixy * sf.x) * r2;
498 q_omega += weight * n_omega;
499 i_omega += weight * n_omega * n_omega;
500 i_xomega += weight * sf.x * n_omega;
501 i_yomega += weight * sf.y * n_omega;
502 }
503
504 (sc_xint, sc_yint, q_omega, i_omega, i_xomega, i_yomega)
505 }
506
507 pub fn shear_coefficients(
509 &self,
510 ixx: f64,
511 iyy: f64,
512 ixy: f64,
513 psi_shear: &[f64; 6],
514 phi_shear: &[f64; 6],
515 nu: f64,
516 ) -> (f64, f64, f64) {
517 let mut kappa_x = 0.0;
518 let mut kappa_y = 0.0;
519 let mut kappa_xy = 0.0;
520
521 for &(w, eta, xi, zeta) in &gauss_points(4) {
522 let sf = shape_function(&self.coords, (eta, xi, zeta));
523 let weight = w * sf.j * self.elastic_modulus;
524 let [_, _, d1, d2, h1, h2] = shear_parameter(sf.x, sf.y, ixx, iyy, ixy);
525
526 let mut b_psi_d = [0.0; 2];
528 let mut b_phi_h = [0.0; 2];
529 for i in 0..6 {
530 b_psi_d[0] += sf.b[0][i] * psi_shear[i];
531 b_psi_d[1] += sf.b[1][i] * psi_shear[i];
532 b_phi_h[0] += sf.b[0][i] * phi_shear[i];
533 b_phi_h[1] += sf.b[1][i] * phi_shear[i];
534 }
535 b_psi_d[0] -= nu / 2.0 * d1;
536 b_psi_d[1] -= nu / 2.0 * d2;
537 b_phi_h[0] -= nu / 2.0 * h1;
538 b_phi_h[1] -= nu / 2.0 * h2;
539
540 kappa_x += weight * (b_psi_d[0] * b_psi_d[0] + b_psi_d[1] * b_psi_d[1]);
541 kappa_y += weight * (b_phi_h[0] * b_phi_h[0] + b_phi_h[1] * b_phi_h[1]);
542 kappa_xy += weight * (b_psi_d[0] * b_phi_h[0] + b_psi_d[1] * b_phi_h[1]);
543 }
544
545 (kappa_x, kappa_y, kappa_xy)
546 }
547
548 pub fn monosymmetry_integrals(&self, phi: f64) -> (f64, f64, f64, f64) {
552 let mut int_x = 0.0;
553 let mut int_y = 0.0;
554 let mut int_11 = 0.0;
555 let mut int_22 = 0.0;
556
557 for &(w, eta, xi, zeta) in &gauss_points(4) {
558 let sf = shape_function(&self.coords, (eta, xi, zeta));
559 let weight = w * sf.j * self.elastic_modulus;
560 let (nx_11, ny_22) = principal_coordinate(phi, sf.x, sf.y);
561
562 int_x += weight * (sf.x * sf.x * sf.y + sf.y * sf.y * sf.y);
563 int_y += weight * (sf.y * sf.y * sf.x + sf.x * sf.x * sf.x);
564 int_11 += weight * (nx_11 * nx_11 * ny_22 + ny_22 * ny_22 * ny_22);
565 int_22 += weight * (ny_22 * ny_22 * nx_11 + nx_11 * nx_11 * nx_11);
566 }
567
568 (int_x, int_y, int_11, int_22)
569 }
570
571 pub fn torsion_shear_stress(&self, omega: &[f64; 6]) -> (f64, (f64, f64)) {
579 let mut max_tau = 0.0;
580 let mut max_tau_vec = (0.0, 0.0);
581
582 let gps = gauss_points(6);
585 for &(_w, eta, xi, zeta) in &gps {
586 let sf = shape_function(&self.coords, (eta, xi, zeta));
587
588 let mut b_omega = [0.0; 2];
589 for k in 0..6 {
590 b_omega[0] += sf.b[0][k] * omega[k];
591 b_omega[1] += sf.b[1][k] * omega[k];
592 }
593
594 let tau_x = b_omega[0] - sf.y;
598 let tau_y = b_omega[1] + sf.x;
599
600 let tau_mag = (tau_x * tau_x + tau_y * tau_y).sqrt();
601 if tau_mag > max_tau {
602 max_tau = tau_mag;
603 max_tau_vec = (tau_x, tau_y);
604 }
605 }
606 (max_tau, max_tau_vec)
607 }
608
609 pub fn element_stress(
615 &self,
616 n: f64,
617 mxx: f64,
618 myy: f64,
619 m11: f64,
620 m22: f64,
621 mzz: f64,
622 vx: f64,
623 vy: f64,
624 ea: f64,
625 cx: f64,
626 cy: f64,
627 ixx: f64,
628 iyy: f64,
629 ixy: f64,
630 i11: f64,
631 i22: f64,
632 phi: f64,
633 j: f64,
634 nu: f64,
635 omega: &[f64; 6],
636 psi_shear: &[f64; 6],
637 phi_shear: &[f64; 6],
638 delta_s: f64,
639 ) -> [[f64; 6]; 11] {
640 let em = self.elastic_modulus;
641 let denom = ixx * iyy - ixy * ixy;
642
643 let sig_zz_n = [em * n / ea; 6];
645
646 let mut sig_zz_mxx_gp = [0.0; 6];
647 let mut sig_zz_myy_gp = [0.0; 6];
648 let mut sig_zz_m11_gp = [0.0; 6];
649 let mut sig_zz_m22_gp = [0.0; 6];
650 let mut sig_zx_mzz_gp = [0.0; 6];
651 let mut sig_zy_mzz_gp = [0.0; 6];
652 let mut sig_zx_vx_gp = [0.0; 6];
653 let mut sig_zy_vx_gp = [0.0; 6];
654 let mut sig_zx_vy_gp = [0.0; 6];
655 let mut sig_zy_vy_gp = [0.0; 6];
656
657 let coords_c = [
659 [
660 self.coords[0][0] - cx,
661 self.coords[0][1] - cx,
662 self.coords[0][2] - cx,
663 self.coords[0][3] - cx,
664 self.coords[0][4] - cx,
665 self.coords[0][5] - cx,
666 ],
667 [
668 self.coords[1][0] - cy,
669 self.coords[1][1] - cy,
670 self.coords[1][2] - cy,
671 self.coords[1][3] - cy,
672 self.coords[1][4] - cy,
673 self.coords[1][5] - cy,
674 ],
675 ];
676
677 let gps = gauss_points(6);
678 for (i, &(_w, eta, xi, zeta)) in gps.iter().enumerate() {
679 let sf = shape_function(&coords_c, (eta, xi, zeta));
680 let (nx_11, ny_22) = principal_coordinate(phi, sf.x, sf.y);
681 let [_, _, d1, d2, h1, h2] = shear_parameter(sf.x, sf.y, ixx, iyy, ixy);
682
683 if denom.abs() > NEAR_ZERO_TOL_BASE {
685 sig_zz_mxx_gp[i] = em * (-ixy * mxx / denom * sf.x + iyy * mxx / denom * sf.y);
686 sig_zz_myy_gp[i] = em * (-ixx * myy / denom * sf.x + ixy * myy / denom * sf.y);
687 }
688 if i11.abs() > NEAR_ZERO_TOL_BASE {
689 sig_zz_m11_gp[i] = em * m11 / i11 * ny_22;
690 }
691 if i22.abs() > NEAR_ZERO_TOL_BASE {
692 sig_zz_m22_gp[i] = em * -m22 / i22 * nx_11;
693 }
694
695 if mzz.abs() > GEOMETRY_TOL && j.abs() > GEOMETRY_TOL {
697 let mut b_omega = [0.0; 2];
698 for k in 0..6 {
699 b_omega[0] += sf.b[0][k] * omega[k];
700 b_omega[1] += sf.b[1][k] * omega[k];
701 }
702 sig_zx_mzz_gp[i] = em * mzz / j * (b_omega[0] - sf.y);
703 sig_zy_mzz_gp[i] = em * mzz / j * (b_omega[1] + sf.x);
704 }
705
706 if vx.abs() > GEOMETRY_TOL && delta_s.abs() > GEOMETRY_TOL {
708 let mut b_psi = [0.0; 2];
709 for k in 0..6 {
710 b_psi[0] += sf.b[0][k] * psi_shear[k];
711 b_psi[1] += sf.b[1][k] * psi_shear[k];
712 }
713 sig_zx_vx_gp[i] = em * vx / delta_s * (b_psi[0] - nu / 2.0 * d1);
714 sig_zy_vx_gp[i] = em * vx / delta_s * (b_psi[1] - nu / 2.0 * d2);
715 }
716
717 if vy.abs() > GEOMETRY_TOL && delta_s.abs() > GEOMETRY_TOL {
719 let mut b_phi = [0.0; 2];
720 for k in 0..6 {
721 b_phi[0] += sf.b[0][k] * phi_shear[k];
722 b_phi[1] += sf.b[1][k] * phi_shear[k];
723 }
724 sig_zx_vy_gp[i] = em * vy / delta_s * (b_phi[0] - nu / 2.0 * h1);
725 sig_zy_vy_gp[i] = em * vy / delta_s * (b_phi[1] - nu / 2.0 * h2);
726 }
727 }
728
729 [
731 sig_zz_n,
732 extrapolate_to_nodes(&sig_zz_mxx_gp),
733 extrapolate_to_nodes(&sig_zz_myy_gp),
734 extrapolate_to_nodes(&sig_zz_m11_gp),
735 extrapolate_to_nodes(&sig_zz_m22_gp),
736 extrapolate_to_nodes(&sig_zx_mzz_gp),
737 extrapolate_to_nodes(&sig_zy_mzz_gp),
738 extrapolate_to_nodes(&sig_zx_vx_gp),
739 extrapolate_to_nodes(&sig_zy_vx_gp),
740 extrapolate_to_nodes(&sig_zx_vy_gp),
741 extrapolate_to_nodes(&sig_zy_vy_gp),
742 ]
743 }
744}
745
746#[derive(Debug, Clone)]
752pub struct Tri6Mesh {
753 pub nodes: Vec<Point>,
755 pub elements: Vec<[usize; 6]>,
757}
758
759pub fn tri3_to_tri6(tri3_nodes: &[Point], tri3_elements: &[[usize; 3]]) -> Tri6Mesh {
764 let mut nodes: Vec<Point> = tri3_nodes.to_vec();
765 let mut edge_midpoints: HashMap<(usize, usize), usize> = HashMap::new();
767
768 let mut tri6_elements: Vec<[usize; 6]> = Vec::with_capacity(tri3_elements.len());
769
770 for tri3_elem in tri3_elements {
771 let n0 = tri3_elem[0];
772 let n1 = tri3_elem[1];
773 let n2 = tri3_elem[2];
774 let mid01 = get_or_create_midpoint(&mut nodes, &mut edge_midpoints, n0, n1);
775 let mid12 = get_or_create_midpoint(&mut nodes, &mut edge_midpoints, n1, n2);
776 let mid20 = get_or_create_midpoint(&mut nodes, &mut edge_midpoints, n2, n0);
777 let mut elem = [n0, n1, n2, mid01, mid12, mid20];
778
779 let mut coords = [[0.0; 6]; 2];
781 for k in 0..6 {
782 coords[0][k] = nodes[elem[k]].x;
783 coords[1][k] = nodes[elem[k]].y;
784 }
785 let sf = shape_function(&coords, (1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0));
786 if sf.j < 0.0 {
787 elem[0..3].reverse(); elem[3] = mid12;
793 elem[4] = mid01;
794 elem[5] = mid20;
795 }
796 tri6_elements.push(elem);
797 }
798
799 Tri6Mesh {
800 nodes,
801 elements: tri6_elements,
802 }
803}
804
805pub fn validate_tri6_mesh(mesh: &Tri6Mesh) -> Result<(), crate::mesh::fem::FemError> {
810 let gps = gauss_points(6);
811 for &elem in mesh.elements.iter() {
812 let mut coords = [[0.0; 6]; 2];
813 for k in 0..6 {
814 coords[0][k] = mesh.nodes[elem[k]].x;
815 coords[1][k] = mesh.nodes[elem[k]].y;
816 }
817 for &(_, eta, xi, zeta) in &gps {
818 let sf = shape_function(&coords, (eta, xi, zeta));
819 if sf.j <= JACOBIAN_TOL {
820 return Err(crate::mesh::fem::FemError::DegenerateElement);
821 }
822 }
823 }
824 Ok(())
825}
826
827fn get_or_create_midpoint(
828 nodes: &mut Vec<Point>,
829 edge_map: &mut HashMap<(usize, usize), usize>,
830 a: usize,
831 b: usize,
832) -> usize {
833 let key = if a < b { (a, b) } else { (b, a) };
834 if let Some(&mid) = edge_map.get(&key) {
835 return mid;
836 }
837 let pa = nodes[a];
838 let pb = nodes[b];
839 let mid = Point::new(0.5 * (pa.x + pb.x), 0.5 * (pa.y + pb.y));
840 let mid_idx = nodes.len();
841 nodes.push(mid);
842 edge_map.insert(key, mid_idx);
843 mid_idx
844}
845
846pub fn build_tri6_elements(
851 mesh: &Tri6Mesh,
852 em: f64,
853 gm: f64,
854 rho: f64,
855) -> Result<Vec<Tri6>, crate::mesh::fem::FemError> {
856 let gps = gauss_points(6);
857 let mut elements = Vec::with_capacity(mesh.elements.len());
858 for (i, &elem) in mesh.elements.iter().enumerate() {
859 let points: [Point; 6] = [
860 mesh.nodes[elem[0]],
861 mesh.nodes[elem[1]],
862 mesh.nodes[elem[2]],
863 mesh.nodes[elem[3]],
864 mesh.nodes[elem[4]],
865 mesh.nodes[elem[5]],
866 ];
867 let coords = [
868 [
869 points[0].x,
870 points[1].x,
871 points[2].x,
872 points[3].x,
873 points[4].x,
874 points[5].x,
875 ],
876 [
877 points[0].y,
878 points[1].y,
879 points[2].y,
880 points[3].y,
881 points[4].y,
882 points[5].y,
883 ],
884 ];
885 let mut degenerate = false;
887 for &(_, eta, xi, zeta) in &gps {
888 let sf = shape_function(&coords, (eta, xi, zeta));
889 if sf.j <= JACOBIAN_TOL {
890 degenerate = true;
891 break;
892 }
893 }
894 if degenerate {
895 return Err(crate::mesh::fem::FemError::DegenerateElement);
896 }
897 elements.push(Tri6::from_points(i, points, elem, em, gm, rho)?);
898 }
899 Ok(elements)
900}
901
902#[derive(Debug, Clone)]
912pub struct SparseMatrix {
913 pub n: usize,
914 rows: Vec<usize>,
916 cols: Vec<usize>,
917 vals: Vec<f64>,
918 row_ptr: Vec<usize>,
920 csr_cols: Vec<usize>,
921 csr_vals: Vec<f64>,
922 diag: Vec<f64>,
924 compressed: bool,
926}
927
928impl SparseMatrix {
929 pub fn new(n: usize) -> Self {
930 Self {
931 n,
932 rows: Vec::new(),
933 cols: Vec::new(),
934 vals: Vec::new(),
935 row_ptr: vec![0; n + 1],
936 csr_cols: Vec::new(),
937 csr_vals: Vec::new(),
938 diag: vec![0.0; n],
939 compressed: false,
940 }
941 }
942
943 pub fn add(&mut self, row: usize, col: usize, val: f64) {
945 debug_assert!(row < self.n && col < self.n);
946 self.rows.push(row);
947 self.cols.push(col);
948 self.vals.push(val);
949 self.compressed = false;
950 }
951
952 pub fn compress(&mut self) {
955 if self.compressed {
956 return;
957 }
958 let n = self.n;
959 let nnz = self.rows.len();
960
961 if nnz == 0 {
962 self.row_ptr = vec![0; n + 1];
963 self.csr_cols.clear();
964 self.csr_vals.clear();
965 self.diag = vec![0.0; n];
966 self.compressed = true;
967 return;
968 }
969
970 let mut triplets: Vec<(usize, usize, f64)> = Vec::with_capacity(nnz);
972 for k in 0..nnz {
973 triplets.push((self.rows[k], self.cols[k], self.vals[k]));
974 }
975 triplets.sort_unstable_by(|a, b| a.0.cmp(&b.0).then(a.1.cmp(&b.1)));
976
977 let mut csr_cols = Vec::with_capacity(triplets.len());
979 let mut csr_vals = Vec::with_capacity(triplets.len());
980 let mut row_ptr = vec![0usize; n + 1];
981 let mut diag = vec![0.0; n];
982
983 let mut i = 0;
984 while i < triplets.len() {
985 let r = triplets[i].0;
986 let c = triplets[i].1;
987 let mut v = triplets[i].2;
988 let mut j = i + 1;
989 while j < triplets.len() && triplets[j].0 == r && triplets[j].1 == c {
990 v += triplets[j].2;
991 j += 1;
992 }
993 if v.abs() > 0.0 {
994 csr_cols.push(c);
995 csr_vals.push(v);
996 row_ptr[r + 1] += 1;
997 if r == c {
998 diag[r] += v;
999 }
1000 }
1001 i = j;
1002 }
1003
1004 for r in 0..n {
1006 row_ptr[r + 1] += row_ptr[r];
1007 }
1008
1009 self.row_ptr = row_ptr;
1010 self.csr_cols = csr_cols;
1011 self.csr_vals = csr_vals;
1012 self.diag = diag;
1013 self.compressed = true;
1014 }
1015
1016 pub fn matvec(&self, x: &[f64]) -> Vec<f64> {
1018 let mut y = vec![0.0; self.n];
1019 self.matvec_into(x, &mut y);
1020 y
1021 }
1022
1023 pub fn matvec_into(&self, x: &[f64], y: &mut [f64]) {
1025 if !self.compressed {
1026 panic!("SparseMatrix::compress() must be called before matvec");
1027 }
1028 for v in y.iter_mut() {
1029 *v = 0.0;
1030 }
1031 for r in 0..self.n {
1032 let start = self.row_ptr[r];
1033 let end = self.row_ptr[r + 1];
1034 let mut sum = 0.0;
1035 for k in start..end {
1036 sum += self.csr_vals[k] * x[self.csr_cols[k]];
1037 }
1038 y[r] = sum;
1039 }
1040 }
1041
1042 pub fn matvec_diag(&self, i: usize) -> f64 {
1044 if !self.compressed {
1045 panic!("SparseMatrix::compress() must be called before matvec_diag");
1046 }
1047 self.diag[i]
1048 }
1049
1050 pub fn csr_data(&self) -> (&[usize], &[usize], &[f64]) {
1052 if !self.compressed {
1053 panic!("SparseMatrix::compress() must be called before csr_data");
1054 }
1055 (&self.row_ptr, &self.csr_cols, &self.csr_vals)
1056 }
1057
1058 pub fn row_ptr(&self) -> &[usize] {
1060 if !self.compressed {
1061 panic!("SparseMatrix::compress() must be called before row_ptr");
1062 }
1063 &self.row_ptr
1064 }
1065
1066 pub fn csr_cols(&self) -> &[usize] {
1068 if !self.compressed {
1069 panic!("SparseMatrix::compress() must be called before csr_cols");
1070 }
1071 &self.csr_cols
1072 }
1073
1074 pub fn csr_vals(&self) -> &[f64] {
1076 if !self.compressed {
1077 panic!("SparseMatrix::compress() must be called before csr_vals");
1078 }
1079 &self.csr_vals
1080 }
1081
1082 pub fn frobenius_norm(&self) -> f64 {
1084 if !self.compressed {
1085 panic!("SparseMatrix::compress() must be called before frobenius_norm");
1086 }
1087 self.csr_vals.iter().map(|v| v * v).sum::<f64>().sqrt()
1088 }
1089
1090 pub fn symmetry_max_error(&self) -> f64 {
1092 if !self.compressed {
1093 panic!("SparseMatrix::compress() must be called before symmetry_max_error");
1094 }
1095 let mut max_err = 0.0;
1096 for i in 0..self.n {
1097 let start = self.row_ptr[i];
1098 let end = self.row_ptr[i + 1];
1099 for k in start..end {
1100 let j = self.csr_cols[k];
1101 if j > i {
1102 let val_ij = self.csr_vals[k];
1103 let val_ji = self.get(j, i);
1104 let err = (val_ij - val_ji).abs();
1105 if err > max_err {
1106 max_err = err;
1107 }
1108 }
1109 }
1110 }
1111 max_err
1112 }
1113
1114 pub fn is_symmetric(&self, tol: f64) -> bool {
1116 if !self.compressed {
1117 panic!("SparseMatrix::compress() must be called before is_symmetric");
1118 }
1119 for i in 0..self.n {
1120 let start = self.row_ptr[i];
1121 let end = self.row_ptr[i + 1];
1122 for k in start..end {
1123 let j = self.csr_cols[k];
1124 if j > i {
1125 let val_ij = self.csr_vals[k];
1126 let val_ji = self.get(j, i);
1127 let scale = val_ij.abs().max(val_ji.abs()).max(1.0);
1137 if (val_ij - val_ji).abs() > tol * scale {
1138 return false;
1139 }
1140 }
1141 }
1142 }
1143 true
1144 }
1145
1146 pub fn get(&self, row: usize, col: usize) -> f64 {
1148 if !self.compressed {
1149 panic!("SparseMatrix::compress() must be called before get");
1150 }
1151 let start = self.row_ptr[row];
1152 let end = self.row_ptr[row + 1];
1153 for k in start..end {
1154 if self.csr_cols[k] == col {
1155 return self.csr_vals[k];
1156 }
1157 }
1158 0.0
1159 }
1160
1161 pub fn triplets(&self) -> (&[usize], &[usize], &[f64]) {
1163 if !self.compressed {
1164 panic!("SparseMatrix::compress() must be called before accessing triplets");
1165 }
1166 (&self.rows, &self.cols, &self.vals)
1167 }
1168}
1169
1170pub fn cg_solve(a: &SparseMatrix, b: &[f64], max_iter: usize, tol: f64) -> CgResult {
1175 let n = b.len();
1176
1177 let mut m_inv = vec![1.0; n];
1180 for i in 0..n {
1181 let d = a.diag[i];
1182 if d.abs() > NEAR_ZERO_TOL_BASE {
1183 m_inv[i] = 1.0 / d;
1184 }
1185 }
1186
1187 let mut x = vec![0.0; n];
1188 let mut r = b.to_vec();
1189
1190 let b_norm = b.iter().map(|v| v * v).sum::<f64>().sqrt();
1194 if b_norm == 0.0 || !b_norm.is_finite() {
1195 return CgResult {
1196 x,
1197 iterations: 0,
1198 residual: 0.0,
1199 status: CgStatus::InvalidInput,
1200 };
1201 }
1202
1203 let mut z: Vec<f64> = r.iter().zip(m_inv.iter()).map(|(a, b)| a * b).collect();
1204 let mut p = z.clone();
1205 let mut rz_old: f64 = r.iter().zip(z.iter()).map(|(a, b)| a * b).sum();
1206 let mut ap_buf = vec![0.0; n];
1207
1208 let mut iterations = 0;
1209 let mut converged = false;
1210 let mut residual = 0.0;
1211
1212 for iter in 0..max_iter {
1213 a.matvec_into(&p, &mut ap_buf);
1214 let ap: &[f64] = &ap_buf;
1215 let p_ap: f64 = p.iter().zip(ap.iter()).map(|(a, b)| a * b).sum();
1216
1217 if p_ap <= 0.0 || !p_ap.is_finite() {
1219 return CgResult {
1220 x,
1221 iterations,
1222 residual: r.iter().map(|v| v * v).sum::<f64>().sqrt(),
1223 status: CgStatus::NotPositiveDefinite,
1224 };
1225 }
1226
1227 let alpha = rz_old / p_ap;
1228 if !alpha.is_finite() {
1229 return CgResult {
1230 x,
1231 iterations,
1232 residual: r.iter().map(|v| v * v).sum::<f64>().sqrt(),
1233 status: CgStatus::Breakdown,
1234 };
1235 }
1236
1237 for i in 0..n {
1238 x[i] += alpha * p[i];
1239 r[i] -= alpha * ap[i];
1240 }
1241
1242 let rs_new: f64 = r.iter().map(|v| v * v).sum();
1243 residual = rs_new.sqrt();
1244 if residual < tol * b_norm {
1245 converged = true;
1246 iterations = iter + 1;
1247 break;
1248 }
1249
1250 if rz_old == 0.0 || !rz_old.is_finite() {
1251 return CgResult {
1252 x,
1253 iterations,
1254 residual,
1255 status: CgStatus::Breakdown,
1256 };
1257 }
1258 for i in 0..n {
1259 z[i] = r[i] * m_inv[i];
1260 }
1261 let rz_new: f64 = r.iter().zip(z.iter()).map(|(a, b)| a * b).sum();
1262 if !rz_new.is_finite() {
1263 return CgResult {
1264 x,
1265 iterations,
1266 residual,
1267 status: CgStatus::Breakdown,
1268 };
1269 }
1270 let beta = rz_new / rz_old;
1271 if !beta.is_finite() {
1272 return CgResult {
1273 x,
1274 iterations,
1275 residual,
1276 status: CgStatus::Breakdown,
1277 };
1278 }
1279 for i in 0..n {
1280 p[i] = z[i] + beta * p[i];
1281 }
1282 rz_old = rz_new;
1283 iterations = iter + 1;
1284 }
1285
1286 CgResult {
1287 x,
1288 iterations,
1289 residual,
1290 status: if converged {
1291 CgStatus::Converged
1292 } else {
1293 CgStatus::MaxIterations
1294 },
1295 }
1296}
1297
1298#[derive(Debug, Clone, Copy, PartialEq, Eq)]
1300pub enum CgStatus {
1301 Converged,
1303 MaxIterations,
1305 NotPositiveDefinite,
1307 Breakdown,
1309 InvalidInput,
1311}
1312
1313#[derive(Debug, Clone)]
1315pub struct CgResult {
1316 pub x: Vec<f64>,
1318 pub iterations: usize,
1320 pub residual: f64,
1322 pub status: CgStatus,
1324}
1325
1326impl CgResult {
1327 pub fn converged(&self) -> bool {
1329 matches!(self.status, CgStatus::Converged)
1330 }
1331}
1332
1333pub fn solve_lagrange_sparse(
1340 k: &SparseMatrix,
1341 c: &[f64],
1342 f: &[f64],
1343) -> Result<Vec<f64>, crate::mesh::fem::FemError> {
1344 solve_lagrange_sparse_tol(k, c, f, 1e-6)
1345}
1346
1347pub fn solve_lagrange_sparse_tol(
1349 k: &SparseMatrix,
1350 c: &[f64],
1351 f: &[f64],
1352 tol: f64,
1353) -> Result<Vec<f64>, crate::mesh::fem::FemError> {
1354 let n = k.n;
1357 let max_iter = if tol < 1e-8 {
1358 (n * 20).clamp(20000, 600000)
1359 } else {
1360 (n * 4).clamp(1000, 60000)
1361 };
1362 let _n = k.n;
1363
1364 let mut k_reg = k.clone();
1368 k_reg.compress();
1369 let n = k.n;
1370
1371 let w1 = cg_solve(&k_reg, f, max_iter, tol);
1372 let w2 = cg_solve(&k_reg, c, max_iter, tol);
1373
1374 let (w1, w2) = if w1.converged() && w2.converged() {
1376 (w1.x, w2.x)
1377 } else {
1378 let mut k_reg = k.clone();
1380 k_reg.compress();
1381 let mut diag_sum = 0.0;
1382 for i in 0..n {
1383 diag_sum += k_reg.matvec_diag(i);
1384 }
1385 let eps = diag_sum.max(1e-300) / n as f64 * 1e-9;
1386 for i in 0..n {
1387 k_reg.add(i, i, eps);
1388 }
1389 k_reg.compress();
1390
1391 let w1 = cg_solve(&k_reg, f, max_iter, tol).x;
1392 let w2 = cg_solve(&k_reg, c, max_iter, tol).x;
1393 (w1, w2)
1394 };
1395
1396 let ct_w2: f64 = c.iter().zip(w2.iter()).map(|(a, b)| a * b).sum();
1397 let ct_w1: f64 = c.iter().zip(w1.iter()).map(|(a, b)| a * b).sum();
1398
1399 if ct_w2.abs() < NEAR_ZERO_TOL_BASE {
1400 return Ok(w1);
1401 }
1402 let lambda = ct_w1 / ct_w2;
1403
1404 let mut u = vec![0.0; n];
1405 for i in 0..n {
1406 u[i] = w1[i] - lambda * w2[i];
1407 }
1408 Ok(u)
1409}
1410
1411pub fn solve_lagrange(
1420 k: &[Vec<f64>],
1421 c: &[f64],
1422 f: &[f64],
1423) -> Result<Vec<f64>, crate::mesh::fem::FemError> {
1424 let n = k.len();
1425 let mut a = vec![vec![0.0; n + 1]; n + 1];
1427 for i in 0..n {
1428 for j in 0..n {
1429 a[i][j] = k[i][j];
1430 }
1431 a[i][n] = c[i];
1432 a[n][i] = c[i];
1433 }
1434 let mut rhs = vec![0.0; n + 1];
1435 for i in 0..n {
1436 rhs[i] = f[i];
1437 }
1438
1439 solve_dense(&mut a, &mut rhs)?;
1441 Ok(rhs[..n].to_vec())
1442}
1443
1444pub fn solve_dense(a: &mut [Vec<f64>], b: &mut [f64]) -> Result<(), crate::mesh::fem::FemError> {
1447 let n = a.len();
1448 let mut scale = 0.0f64;
1450 for i in 0..n {
1451 for j in 0..n {
1452 scale = scale.max(a[i][j].abs());
1453 }
1454 }
1455 let pivot_tol = PIVOT_TOL_BASE * scale.max(1.0);
1456 for k in 0..n {
1457 let mut max_row = k;
1459 let mut max_val = a[k][k].abs();
1460 for i in (k + 1)..n {
1461 if a[i][k].abs() > max_val {
1462 max_val = a[i][k].abs();
1463 max_row = i;
1464 }
1465 }
1466 if max_row != k {
1467 a.swap(k, max_row);
1468 b.swap(k, max_row);
1469 }
1470 let pivot = a[k][k];
1471 if pivot.abs() < pivot_tol {
1472 return Err(crate::mesh::fem::FemError::SingularMatrix);
1473 }
1474 for i in (k + 1)..n {
1475 let factor = a[i][k] / pivot;
1476 for j in k..n {
1477 a[i][j] -= factor * a[k][j];
1478 }
1479 b[i] -= factor * b[k];
1480 }
1481 }
1482 for k in (0..n).rev() {
1484 let pivot = a[k][k];
1485 if pivot.abs() < pivot_tol {
1486 return Err(crate::mesh::fem::FemError::SingularMatrix);
1487 }
1488 let mut sum = 0.0;
1489 for j in (k + 1)..n {
1490 sum += a[k][j] * b[j];
1491 }
1492 b[k] = (b[k] - sum) / pivot;
1493 }
1494 Ok(())
1495}
1496
1497#[derive(Debug, Clone)]
1506pub struct CscMatrix {
1507 pub n_rows: usize,
1508 pub n_cols: usize,
1509 pub col_ptr: Vec<usize>,
1511 pub rows: Vec<usize>,
1513 pub vals: Vec<f64>,
1515}
1516
1517impl CscMatrix {
1518 pub fn from_csr(
1521 n_rows: usize,
1522 n_cols: usize,
1523 row_ptr: &[usize],
1524 csr_cols: &[usize],
1525 csr_vals: &[f64],
1526 ) -> Self {
1527 debug_assert_eq!(row_ptr.len(), n_rows + 1);
1528 debug_assert_eq!(csr_cols.len(), csr_vals.len());
1529
1530 let mut counts = vec![0usize; n_cols];
1532 for r in 0..n_rows {
1533 let start = row_ptr[r];
1534 let end = row_ptr[r + 1];
1535 for k in start..end {
1536 counts[csr_cols[k]] += 1;
1537 }
1538 }
1539
1540 let mut col_ptr = vec![0usize; n_cols + 1];
1542 for c in 0..n_cols {
1543 col_ptr[c + 1] = col_ptr[c] + counts[c];
1544 }
1545
1546 let mut rows = vec![0usize; csr_cols.len()];
1548 let mut vals = vec![0.0; csr_vals.len()];
1549 let mut col_counts = vec![0usize; n_cols];
1550
1551 for r in 0..n_rows {
1552 let start = row_ptr[r];
1553 let end = row_ptr[r + 1];
1554 for k in start..end {
1555 let c = csr_cols[k];
1556 let idx = col_ptr[c] + col_counts[c];
1557 rows[idx] = r;
1558 vals[idx] = csr_vals[k];
1559 col_counts[c] += 1;
1560 }
1561 }
1562
1563 Self {
1564 n_rows,
1565 n_cols,
1566 col_ptr,
1567 rows,
1568 vals,
1569 }
1570 }
1571
1572 pub fn from_coo(
1575 n_rows: usize,
1576 n_cols: usize,
1577 row: &[usize],
1578 col: &[usize],
1579 data: &[f64],
1580 ) -> Self {
1581 debug_assert_eq!(row.len(), col.len());
1582 debug_assert_eq!(row.len(), data.len());
1583
1584 let mut order: Vec<usize> = (0..data.len()).collect();
1586 order.sort_unstable_by_key(|&k| (col[k], row[k]));
1587
1588 let mut rows: Vec<usize> = Vec::with_capacity(data.len());
1589 let mut vals: Vec<f64> = Vec::with_capacity(data.len());
1590 let mut counts = vec![0usize; n_cols];
1591
1592 let mut pending: Option<(usize, usize, f64)> = None;
1593 for &k in &order {
1594 let key = (row[k], col[k]);
1595 match pending {
1596 None => pending = Some((key.0, key.1, data[k])),
1597 Some((pr, pc, pv)) if pr == key.0 && pc == key.1 => {
1598 pending = Some((pr, pc, pv + data[k]));
1599 }
1600 Some((pr, pc, pv)) => {
1601 rows.push(pr);
1602 vals.push(pv);
1603 counts[pc] += 1;
1604 pending = Some((key.0, key.1, data[k]));
1605 }
1606 }
1607 }
1608 if let Some((pr, pc, pv)) = pending {
1609 rows.push(pr);
1610 vals.push(pv);
1611 counts[pc] += 1;
1612 }
1613
1614 let mut col_ptr = vec![0usize; n_cols + 1];
1615 for cc in 0..n_cols {
1616 col_ptr[cc + 1] = col_ptr[cc] + counts[cc];
1617 }
1618
1619 Self {
1620 n_rows,
1621 n_cols,
1622 col_ptr,
1623 rows,
1624 vals,
1625 }
1626 }
1627
1628 pub fn matvec(&self, x: &[f64]) -> Vec<f64> {
1630 let mut y = vec![0.0; self.n_rows];
1631 for c in 0..self.n_cols {
1632 let xc = x[c];
1633 for k in self.col_ptr[c]..self.col_ptr[c + 1] {
1634 y[self.rows[k]] += self.vals[k] * xc;
1635 }
1636 }
1637 y
1638 }
1639}
1640
1641pub struct DirectLagrangeSolver {
1648 pub n: usize,
1650 kernel: LagrangeKernelInstance,
1651 c: Vec<f64>,
1652}
1653
1654#[derive(Debug, Clone, Copy, PartialEq, Eq)]
1656pub enum LagrangeKernel {
1657 Skyline,
1659 #[cfg(feature = "pardiso")]
1661 Pardiso,
1662}
1663
1664enum LagrangeKernelInstance {
1665 Skyline(SkylineLdlt),
1666 #[cfg(feature = "pardiso")]
1668 Pardiso(std::sync::Mutex<crate::fea::solvers::pardiso::PardisoSolver>),
1669}
1670
1671#[derive(Debug, Clone, Copy, PartialEq, Eq)]
1673pub struct SolverOptions {
1674 pub auto_regularize_singular: bool,
1682}
1683
1684impl Default for SolverOptions {
1685 fn default() -> Self {
1686 Self {
1687 auto_regularize_singular: false,
1688 }
1689 }
1690}
1691
1692impl DirectLagrangeSolver {
1693 pub fn assemble_torsion_lagrange(k: &SparseMatrix, c: &[f64]) -> CscMatrix {
1696 let n = k.n;
1697 let mut row: Vec<usize> = Vec::with_capacity(k.rows.len() + 2 * n);
1698 let mut col: Vec<usize> = Vec::with_capacity(row.capacity());
1699 let mut data: Vec<f64> = Vec::with_capacity(row.capacity());
1700 row.extend_from_slice(&k.rows);
1701 col.extend_from_slice(&k.cols);
1702 data.extend_from_slice(&k.vals);
1703 row.extend(0..n);
1705 col.extend(std::iter::repeat(n).take(n));
1706 data.extend_from_slice(c);
1707 row.extend(std::iter::repeat(n).take(n));
1709 col.extend(0..n);
1710 data.extend_from_slice(c);
1711 CscMatrix::from_coo(n + 1, n + 1, &row, &col, &data)
1712 }
1713
1714 pub fn new(k: &SparseMatrix, c: &[f64]) -> Result<Self, crate::mesh::fem::FemError> {
1721 Self::with_kernel(LagrangeKernel::Skyline, k, c, SolverOptions::default())
1722 }
1723
1724 pub fn with_kernel(
1726 kernel: LagrangeKernel,
1727 k: &SparseMatrix,
1728 c: &[f64],
1729 opts: SolverOptions,
1730 ) -> Result<Self, crate::mesh::fem::FemError> {
1731 let instance = match kernel {
1732 LagrangeKernel::Skyline => {
1733 let mut m = k.clone();
1734 m.compress();
1735 let m = match SkylineLdlt::factor(&m) {
1736 Ok(ldlt) => ldlt,
1737 Err(crate::mesh::fem::FemError::SingularMatrix)
1738 if opts.auto_regularize_singular =>
1739 {
1740 let mut m_reg = k.clone();
1741 m_reg.compress();
1742 let mut diag_avg = 0.0;
1743 for i in 0..m_reg.n {
1744 diag_avg += m_reg.matvec_diag(i);
1745 }
1746 let eps = diag_avg.max(1e-300) / m_reg.n as f64 * 1e-9;
1747 for i in 0..m_reg.n {
1748 m_reg.add(i, i, eps);
1749 }
1750 m_reg.compress();
1751 SkylineLdlt::factor(&m_reg)?
1752 }
1753 Err(e) => return Err(e),
1754 };
1755 LagrangeKernelInstance::Skyline(m)
1756 }
1757 #[cfg(feature = "pardiso")]
1758 LagrangeKernel::Pardiso => LagrangeKernelInstance::Pardiso(std::sync::Mutex::new(
1759 crate::fea::solvers::pardiso::PardisoSolver::new(k, c)
1760 .map_err(|_| crate::mesh::fem::FemError::SingularMatrix)?,
1761 )),
1762 };
1763 Ok(Self {
1764 n: k.n,
1765 kernel: instance,
1766 c: c.to_vec(),
1767 })
1768 }
1769
1770 pub fn solve(&self, f: &[f64]) -> Result<Vec<f64>, crate::mesh::fem::FemError> {
1772 self.solve_full(f).map(|(u, _lam)| u)
1773 }
1774
1775 pub fn solve_full(&self, f: &[f64]) -> Result<(Vec<f64>, f64), crate::mesh::fem::FemError> {
1777 match &self.kernel {
1778 LagrangeKernelInstance::Skyline(ldlt) => {
1779 let w1 = ldlt.solve(f)?;
1780 let w2 = ldlt.solve(&self.c)?;
1781 let ct_w2: f64 = self.c.iter().zip(w2.iter()).map(|(&a, &b)| a * b).sum();
1782 let ct_w1: f64 = self.c.iter().zip(w1.iter()).map(|(&a, &b)| a * b).sum();
1783 let lambda = if ct_w2.abs() > NEAR_ZERO_TOL_BASE {
1784 ct_w1 / ct_w2
1785 } else {
1786 0.0
1787 };
1788 let u = w1
1789 .iter()
1790 .zip(w2.iter())
1791 .map(|(&a, &b)| a - lambda * b)
1792 .collect();
1793 Ok((u, lambda))
1794 }
1795 #[cfg(feature = "pardiso")]
1796 LagrangeKernelInstance::Pardiso(p) => {
1797 p.lock()
1799 .unwrap()
1800 .solve_with_multiplier(f)
1801 .map_err(|e| crate::mesh::fem::FemError::ConvergenceFailed)
1802 }
1803 }
1804 }
1805
1806 pub fn multiplier_error(&self, f: &[f64], u: &[f64]) -> f64 {
1808 match self.solve_full(f) {
1809 Ok((_u_full, lambda)) => {
1810 let max_u = u.iter().fold(0.0f64, |a, &v| a.max(v.abs()));
1811 if max_u > 0.0 {
1812 lambda.abs() / max_u
1813 } else {
1814 f64::INFINITY
1815 }
1816 }
1817 Err(_) => f64::INFINITY,
1818 }
1819 }
1820}
1821fn rcm_order(n: usize, adj: &[Vec<usize>]) -> Vec<usize> {
1825 let mut visited = vec![false; n];
1826 let mut perm: Vec<usize> = Vec::with_capacity(n);
1827
1828 if n == 0 {
1829 return perm;
1830 }
1831
1832 let mut bfs_farthest = |from: usize| -> usize {
1833 for v in visited.iter_mut() {
1834 *v = false;
1835 }
1836 let mut queue = std::collections::VecDeque::new();
1837 queue.push_back(from);
1838 visited[from] = true;
1839 let mut last = from;
1840 while let Some(u) = queue.pop_front() {
1841 for &w in &adj[u] {
1842 if !visited[w] {
1843 visited[w] = true;
1844 queue.push_back(w);
1845 last = w;
1846 }
1847 }
1848 }
1849 last
1850 };
1851
1852 let mut start = 0;
1853 while start < n && adj[start].is_empty() {
1854 start += 1;
1855 }
1856 if start >= n {
1857 return (0..n).collect();
1858 }
1859
1860 let peripheral = {
1861 let far = bfs_farthest(start);
1862 bfs_farthest(far)
1863 };
1864
1865 for v in visited.iter_mut() {
1866 *v = false;
1867 }
1868 let mut queue = std::collections::VecDeque::new();
1869 queue.push_back(peripheral);
1870 visited[peripheral] = true;
1871 while let Some(u) = queue.pop_front() {
1872 perm.push(u);
1873 let mut nbrs: Vec<usize> = adj[u].iter().filter(|&&w| !visited[w]).copied().collect();
1874 nbrs.sort_by_key(|&w| adj[w].len());
1875 for w in nbrs {
1876 visited[w] = true;
1877 queue.push_back(w);
1878 }
1879 }
1880 for i in 0..n {
1881 if !visited[i] {
1882 perm.push(i);
1883 }
1884 }
1885
1886 perm.reverse();
1887 perm
1888}
1889
1890pub struct SkylineLdlt {
1896 n: usize,
1897 diag: Vec<f64>,
1898 first: Vec<usize>,
1900 lower: Vec<f64>,
1902 row_start: Vec<usize>,
1904 perm: Vec<usize>,
1906 scale: f64,
1908}
1909
1910impl SkylineLdlt {
1911 pub fn scale(&self) -> f64 {
1913 self.scale
1914 }
1915
1916 pub fn factor(matrix: &SparseMatrix) -> Result<Self, crate::mesh::fem::FemError> {
1917 use std::collections::HashMap;
1918
1919 let n = matrix.n;
1920
1921 let map: HashMap<(usize, usize), f64> = if matrix.compressed {
1923 let mut m = HashMap::new();
1924 for r in 0..n {
1925 let start = matrix.row_ptr[r];
1926 let end = matrix.row_ptr[r + 1];
1927 for k in start..end {
1928 let c = matrix.csr_cols[k];
1929 *m.entry((r, c)).or_insert(0.0) += matrix.csr_vals[k];
1930 }
1931 }
1932 m
1933 } else {
1934 let mut m = HashMap::new();
1935 for k in 0..matrix.rows.len() {
1936 *m.entry((matrix.rows[k], matrix.cols[k])).or_insert(0.0) += matrix.vals[k];
1937 }
1938 m
1939 };
1940
1941 let mut adj: Vec<Vec<usize>> = vec![Vec::new(); n];
1943 for &(r, c) in map.keys() {
1944 if r != c {
1945 adj[r].push(c);
1946 }
1947 }
1948 for a in adj.iter_mut() {
1949 a.sort_unstable();
1950 a.dedup();
1951 }
1952 let perm = rcm_order(n, &adj);
1953
1954 let mut old_to_new = vec![0usize; n];
1955 for (new_idx, &old_idx) in perm.iter().enumerate() {
1956 old_to_new[old_idx] = new_idx;
1957 }
1958
1959 let mut first = vec![n; n];
1961 for &(r, c) in map.keys() {
1962 let i = old_to_new[r];
1963 let j = old_to_new[c];
1964 let (hi, lo) = if i > j { (i, j) } else { (j, i) };
1965 if hi != lo && lo < first[hi] {
1966 first[hi] = lo;
1967 }
1968 }
1969 for i in 0..n {
1970 if first[i] > i {
1971 first[i] = i;
1972 }
1973 }
1974
1975 let mut row_start = vec![0usize; n + 1];
1976 for i in 0..n {
1977 row_start[i + 1] = row_start[i] + (i - first[i]);
1978 }
1979 let mut lower = vec![0.0; row_start[n]];
1980 let mut diag = vec![0.0; n];
1981
1982 for (&(r, c), &v) in map.iter() {
1984 let i = old_to_new[r];
1985 let j = old_to_new[c];
1986 if i > j {
1987 lower[row_start[i] + (j - first[i])] = v;
1988 } else if j > i {
1989 lower[row_start[j] + (i - first[j])] = v;
1990 } else {
1991 diag[i] = v;
1992 }
1993 }
1994
1995 let mut diag_for_scale = vec![0.0f64; n];
1998 for i in 0..n {
1999 let _d = 0.0;
2000 for _k in first[i]..i {
2001 let _l_ik = 0.0; }
2003 diag_for_scale[i] = 1.0; }
2007 let mut matrix_scale = 0.0f64;
2009 for (&(_r, _c), &v) in map.iter() {
2010 matrix_scale = matrix_scale.max(v.abs());
2011 }
2012 let pivot_tol = PIVOT_TOL_BASE * matrix_scale.max(1.0);
2013
2014 for i in 0..n {
2016 let rs_i = row_start[i];
2017 for k in first[i]..i {
2018 let rs_k = row_start[k];
2019 let j0 = first[i].max(first[k]);
2020 let mut s = lower[rs_i + (k - first[i])];
2021 for j in j0..k {
2022 s -= lower[rs_i + (j - first[i])] * lower[rs_k + (j - first[k])] * diag[j];
2023 }
2024 if diag[k].abs() < pivot_tol {
2026 return Err(crate::mesh::fem::FemError::SingularMatrix);
2027 }
2028 lower[rs_i + (k - first[i])] = s / diag[k];
2029 }
2030 let mut d = diag[i];
2031 for k in first[i]..i {
2032 let l_ik = lower[rs_i + (k - first[i])];
2033 d -= l_ik * l_ik * diag[k];
2034 }
2035 if !(d > 0.0) || !d.is_finite() {
2036 return Err(crate::mesh::fem::FemError::SingularMatrix);
2037 }
2038 diag[i] = d;
2039 }
2040
2041 let scale = diag.iter().fold(0.0f64, |a, &v| a.max(v.abs()));
2043
2044 Ok(Self {
2045 n,
2046 diag,
2047 first,
2048 lower,
2049 row_start,
2050 perm,
2051 scale,
2052 })
2053 }
2054
2055 pub fn solve(&self, b: &[f64]) -> Result<Vec<f64>, crate::mesh::fem::FemError> {
2057 let n = self.n;
2058 if b.len() != n {
2059 return Err(crate::mesh::fem::FemError::InvalidMesh);
2060 }
2061
2062 let mut x: Vec<f64> = self.perm.iter().map(|&old| b[old]).collect();
2064 for i in 0..n {
2065 let rs = self.row_start[i];
2066 let mut s = x[i];
2067 for k in self.first[i]..i {
2068 s -= self.lower[rs + (k - self.first[i])] * x[k];
2069 }
2070 x[i] = s;
2071 }
2072 let pivot_tol = PIVOT_TOL_BASE * self.scale.max(1.0);
2074 for i in 0..n {
2075 if self.diag[i].abs() < pivot_tol {
2076 return Err(crate::mesh::fem::FemError::SingularMatrix);
2077 }
2078 x[i] /= self.diag[i];
2079 }
2080 for i in (0..n).rev() {
2082 let rs = self.row_start[i];
2083 let xi = x[i];
2084 if xi != 0.0 {
2085 for k in self.first[i]..i {
2086 x[k] -= self.lower[rs + (k - self.first[i])] * xi;
2087 }
2088 }
2089 }
2090 let mut out = vec![0.0; n];
2092 for (new_idx, &old_idx) in self.perm.iter().enumerate() {
2093 out[old_idx] = x[new_idx];
2094 }
2095 Ok(out)
2096 }
2097
2098 pub fn solve_lagrange(
2102 &self,
2103 c: &[f64],
2104 f: &[f64],
2105 ) -> Result<Vec<f64>, crate::mesh::fem::FemError> {
2106 let w1 = self.solve(f)?;
2107 let w2 = self.solve(c)?;
2108 let ct_w2: f64 = c.iter().zip(w2.iter()).map(|(&a, &b)| a * b).sum();
2109 let ct_w1: f64 = c.iter().zip(w1.iter()).map(|(&a, &b)| a * b).sum();
2110 let near_zero_tol = NEAR_ZERO_TOL_BASE * self.scale.max(1.0);
2112 let lambda = if ct_w2.abs() > near_zero_tol {
2113 ct_w1 / ct_w2
2114 } else {
2115 0.0
2116 };
2117
2118 let mut u = vec![0.0; self.n];
2119 for i in 0..self.n {
2120 u[i] = w1[i] - lambda * w2[i];
2121 }
2122 Ok(u)
2123 }
2124}
2125pub mod mechanism;
2126pub mod solver;
2127pub mod solvers;
2128#[cfg(test)]
2129mod direct_lagrange_tests {
2130 use super::*;
2131
2132 #[test]
2133 fn csc_from_coo_sums_duplicates() {
2134 let m = CscMatrix::from_coo(3, 3, &[0, 1, 0], &[0, 1, 0], &[1.0, 2.0, 3.5]);
2135 let col0 = &m.vals[m.col_ptr[0]..m.col_ptr[1]];
2137 assert!((col0.iter().sum::<f64>() - 4.5).abs() < 1e-12);
2138 assert_eq!(m.col_ptr[2] - m.col_ptr[1], 1);
2139 }
2140
2141 #[test]
2142 fn augmented_direct_matches_schur() {
2143 let n = 20;
2145 let mut k = SparseMatrix::new(n);
2146 for i in 0..n {
2147 k.add(i, i, 2.0);
2148 if i + 1 < n {
2149 k.add(i, i + 1, -1.0);
2150 k.add(i + 1, i, -1.0);
2151 }
2152 }
2153 let c: Vec<f64> = vec![1.0; n];
2154 let f: Vec<f64> = (0..n).map(|i| (i as f64) * 0.1 + 0.05).collect();
2155
2156 let u_ref = {
2157 let mut k2 = k.clone();
2158 k2.compress();
2159 SkylineLdlt::factor(&k2)
2160 .unwrap()
2161 .solve_lagrange(&c, &f)
2162 .unwrap()
2163 };
2164
2165 let k_lg = DirectLagrangeSolver::assemble_torsion_lagrange(&k, &c);
2167 assert_eq!(k_lg.n_rows, n + 1);
2168 assert_eq!(k_lg.n_cols, n + 1);
2169 let solver = DirectLagrangeSolver::new(&k, &c).unwrap();
2170 let u_dir = solver.solve(&f).unwrap();
2171
2172 for i in 0..n {
2173 assert!(
2174 (u_ref[i] - u_dir[i]).abs() < 5e-6,
2175 "i={} ref={} dir={}",
2176 i,
2177 u_ref[i],
2178 u_dir[i]
2179 );
2180 }
2181
2182 }
2186}
2187#[cfg(test)]
2188mod skyline_tests {
2189 use super::*;
2190
2191 #[test]
2192 fn skyline_debug_4x4() {
2193 let mut a = SparseMatrix::new(4);
2194 let entries = [
2195 (0, 0, 4.0),
2196 (1, 1, 5.0),
2197 (2, 2, 6.0),
2198 (3, 3, 7.0),
2199 (0, 1, 1.0),
2200 (1, 0, 1.0),
2201 (1, 2, 2.0),
2202 (2, 1, 2.0),
2203 (2, 3, 1.0),
2204 (3, 2, 1.0),
2205 ];
2206 for &(r, c, v) in &entries {
2207 a.add(r, c, v);
2208 }
2209 let s = SkylineLdlt::factor(&a).unwrap();
2210 eprintln!(
2211 "perm-like diag={:?} first={:?} lower={:?}",
2212 s.diag, s.first, s.lower
2213 );
2214 let x = s.solve(&[1.0, 2.0, 3.0, 4.0]);
2215 eprintln!("x={:?} expected~[0.1934,0.2264,0.3374,0.5232]", x);
2216 }
2217
2218 #[test]
2219 fn skyline_matches_direct_dense_solve() {
2220 let mut a = SparseMatrix::new(4);
2222 let entries = [
2223 (0, 0, 4.0),
2224 (1, 1, 5.0),
2225 (2, 2, 6.0),
2226 (3, 3, 7.0),
2227 (0, 1, 1.0),
2228 (1, 0, 1.0),
2229 (1, 2, 2.0),
2230 (2, 1, 2.0),
2231 (2, 3, 1.0),
2232 (3, 2, 1.0),
2233 ];
2234 for &(r, c, v) in &entries {
2235 a.add(r, c, v);
2236 }
2237 let solver = SkylineLdlt::factor(&a).unwrap();
2238
2239 for b in [vec![1.0, 2.0, 3.0, 4.0], vec![4.0, 3.0, 2.0, 1.0]] {
2241 let x = solver.solve(&b).unwrap();
2242 for row in 0..4 {
2244 let mut sum = 0.0;
2245 for &(r, c, v) in &entries {
2246 if r == row {
2247 sum += v * x[c];
2248 }
2249 }
2250 assert!(
2251 (sum - b[row]).abs() < 1e-10,
2252 "row {} got {} (x={:?})",
2253 row,
2254 sum - b[row],
2255 x
2256 );
2257 }
2258 }
2259
2260 let mut a2 = a.clone();
2262 a2.compress();
2263 let b = vec![1.0, 2.0, 3.0, 4.0];
2264 let x_cg = cg_solve(&a2, &b, 2000, SOLVER_TOL).x;
2265 let x_dir = SkylineLdlt::factor(&a).unwrap().solve(&b).unwrap();
2266 for i in 0..4 {
2267 assert!((x_cg[i] - x_dir[i]).abs() < 1e-8);
2268 }
2269 }
2270
2271 #[test]
2272 fn skyline_lagrange_matches_sparse() {
2273 let n = 20;
2275 let mut k = SparseMatrix::new(n);
2276 for i in 0..n {
2277 k.add(i, i, 2.0);
2278 if i + 1 < n {
2279 k.add(i, i + 1, -1.0);
2280 k.add(i + 1, i, -1.0);
2281 }
2282 }
2283 let c: Vec<f64> = (0..n).map(|_i| 1.0).collect();
2284 let f: Vec<f64> = (0..n).map(|i| (i as f64) * 0.1 + 0.05).collect();
2285
2286 let k_reg = {
2288 let mut m = k.clone();
2289 m.compress();
2290 for i in 0..n {
2291 m.add(i, i, 1e-10);
2292 }
2293 m.compress();
2294 m
2295 };
2296 let w1 = cg_solve(&k_reg, &f, 200000, 1e-13).x;
2297 let w2 = cg_solve(&k_reg, &c, 200000, 1e-13).x;
2298 let ct_w2: f64 = c.iter().zip(w2.iter()).map(|(&a, &b)| a * b).sum();
2299 let ct_w1: f64 = c.iter().zip(w1.iter()).map(|(&a, &b)| a * b).sum();
2300 let lambda = ct_w1 / ct_w2;
2303 let u_ref: Vec<f64> = w1
2304 .iter()
2305 .zip(w2.iter())
2306 .map(|(&a, &b)| a - lambda * b)
2307 .collect();
2308
2309 let u_dir = SkylineLdlt::factor(&k)
2310 .unwrap()
2311 .solve_lagrange(&c, &f)
2312 .unwrap();
2313 for i in 0..n {
2314 assert!(
2315 (u_ref[i] - u_dir[i]).abs() < 5e-6,
2316 "i={} ref={:.12} dir={:.12}",
2317 i,
2318 u_ref[i],
2319 u_dir[i]
2320 );
2321 }
2322 }
2323}
2324#[cfg(test)]
2325mod tests {
2326 use super::*;
2327
2328 fn rect_tri6() -> Tri6 {
2329 let points = [
2331 Point::new(0.0, 0.0),
2332 Point::new(1.0, 0.0),
2333 Point::new(0.0, 1.0),
2334 Point::new(0.5, 0.0),
2335 Point::new(0.5, 0.5),
2336 Point::new(0.0, 0.5),
2337 ];
2338 Tri6::from_points(0, points, [0, 1, 2, 3, 4, 5], 1.0, 1.0, 1.0).unwrap()
2339 }
2340
2341 #[test]
2342 fn gauss_points_sum_to_one() {
2343 for &n in &[1, 3, 4, 6] {
2344 let gps = gauss_points(n);
2345 assert_eq!(gps.len(), n, "gauss({n}) returned {} points", gps.len());
2346 let sum: f64 = gps.iter().map(|&(w, _, _, _)| w).sum();
2347 assert!((sum - 1.0).abs() < 1e-12, "gauss({n}): weights sum = {sum}");
2348 }
2349 }
2350
2351 #[test]
2352 fn torsion_constraint_is_elastic_modulus_weighted() {
2353 let points = [
2354 Point::new(0.0, 0.0),
2355 Point::new(1.0, 0.0),
2356 Point::new(0.0, 1.0),
2357 Point::new(0.5, 0.0),
2358 Point::new(0.5, 0.5),
2359 Point::new(0.0, 0.5),
2360 ];
2361 let tri = Tri6::from_points(0, points, [0, 1, 2, 3, 4, 5], 7.0, 1.0, 1.0).unwrap();
2362 let (_, _, constraint) = tri.torsion_properties();
2363
2364 for i in 0..3 {
2367 assert!(
2368 constraint[i].abs() < 1e-12,
2369 "corner constraint[{}] = {}",
2370 i,
2371 constraint[i]
2372 );
2373 }
2374 for value in &constraint[3..] {
2375 assert!(
2376 (*value - 7.0 / 6.0).abs() < 1e-12,
2377 "mid-side constraint = {value}"
2378 );
2379 }
2380 }
2381
2382 #[test]
2383 fn tri6_area_unit_triangle() {
2384 let tri = rect_tri6();
2385 let (area, _, _, _, _, _) = tri.geometric_properties();
2386 assert!((area - 0.5).abs() < 1e-10, "area = {area}");
2387 }
2388
2389 #[test]
2390 fn torsion_shear_stress_uses_warping_solution() {
2391 let tri = rect_tri6();
2392 let zero_warping = [0.0; 6];
2393 let linear_warping = [0.0, 1.0, 0.0, 0.5, 0.5, 0.0];
2395
2396 let (zero_factor, _) = tri.torsion_shear_stress(&zero_warping);
2397 let (linear_factor, _) = tri.torsion_shear_stress(&linear_warping);
2398
2399 assert!(
2400 (linear_factor - zero_factor).abs() > 1e-6,
2401 "the torsion stress factor must vary with the warping solution"
2402 );
2403 }
2404
2405 #[test]
2406 fn tri6_centroid_unit_triangle() {
2407 let tri = rect_tri6();
2408 let (area, qx, qy, _, _, _) = tri.geometric_properties();
2409 let cx = qy / area;
2410 let cy = qx / area;
2411 assert!((cx - 1.0 / 3.0).abs() < 1e-10, "cx = {cx}");
2412 assert!((cy - 1.0 / 3.0).abs() < 1e-10, "cy = {cy}");
2413 }
2414
2415 #[test]
2416 fn tri6_moments_unit_triangle() {
2417 let tri = rect_tri6();
2418 let (_, _, _, ixx, iyy, ixy) = tri.geometric_properties();
2419 assert!((ixx - 1.0 / 12.0).abs() < 1e-10, "ixx = {ixx}");
2422 assert!((iyy - 1.0 / 12.0).abs() < 1e-10, "iyy = {iyy}");
2423 assert!((ixy - 1.0 / 24.0).abs() < 1e-10, "ixy = {ixy}");
2424 }
2425
2426 #[test]
2427 fn principal_coordinate_zero_phi() {
2428 let (x11, y22) = principal_coordinate(0.0, 3.0, 4.0);
2429 assert!((x11 - 3.0).abs() < 1e-12);
2430 assert!((y22 - 4.0).abs() < 1e-12);
2431 }
2432
2433 #[test]
2434 fn principal_coordinate_90_degrees() {
2435 let (x11, y22) = principal_coordinate(std::f64::consts::FRAC_PI_2, 3.0, 0.0);
2436 assert!(x11.abs() < 1e-12);
2437 assert!((y22 + 3.0).abs() < 1e-12);
2438 }
2439
2440 #[test]
2441 fn tri3_to_tri6_creates_midpoints() {
2442 let nodes = vec![
2443 Point::new(0.0, 0.0),
2444 Point::new(1.0, 0.0),
2445 Point::new(0.0, 1.0),
2446 ];
2447 let elements = vec![[0, 1, 2]];
2448 let mesh = tri3_to_tri6(&nodes, &elements);
2449 assert_eq!(mesh.nodes.len(), 6); assert_eq!(mesh.elements.len(), 1);
2451 let mid01 = mesh.nodes[mesh.elements[0][3]];
2453 assert!((mid01.x - 0.5).abs() < 1e-12);
2454 assert!(mid01.y.abs() < 1e-12);
2455 }
2456
2457 #[test]
2458 fn tri3_to_tri6_shares_midpoints() {
2459 let nodes = vec![
2460 Point::new(0.0, 0.0),
2461 Point::new(1.0, 0.0),
2462 Point::new(0.0, 1.0),
2463 Point::new(1.0, 1.0),
2464 ];
2465 let elements = vec![[0, 1, 2], [1, 3, 2]];
2466 let mesh = tri3_to_tri6(&nodes, &elements);
2467 assert_eq!(mesh.nodes.len(), 9);
2469 assert_eq!(mesh.elements[0][4], mesh.elements[1][5]);
2471 }
2472
2473 #[test]
2474 fn solve_dense_simple() {
2475 let mut a = vec![vec![2.0, 1.0], vec![1.0, 3.0]];
2476 let mut b = vec![5.0, 10.0];
2477 solve_dense(&mut a, &mut b).unwrap();
2478 assert!((b[0] - 1.0).abs() < 1e-10, "x = {}", b[0]);
2480 assert!((b[1] - 3.0).abs() < 1e-10, "y = {}", b[1]);
2481 }
2482
2483 #[test]
2484 fn solve_lagrange_simple() {
2485 let k = vec![vec![1.0, 0.0], vec![0.0, 1.0]];
2488 let c = vec![1.0, 1.0];
2489 let f = vec![1.0, 0.0];
2490 let u = solve_lagrange(&k, &c, &f).unwrap();
2491 assert!((u[0] - 0.5).abs() < 1e-10, "u0 = {}", u[0]);
2495 assert!((u[1] + 0.5).abs() < 1e-10, "u1 = {}", u[1]);
2496 let ct_u: f64 = c.iter().zip(u.iter()).map(|(a, b)| a * b).sum();
2498 assert!(ct_u.abs() < 1e-10, "constraint residual c^T u = {}", ct_u);
2499 }
2500
2501 #[test]
2502 fn skyline_lagrange_constraint_residual() {
2503 let n = 10;
2505 let mut k = SparseMatrix::new(n);
2506 for i in 0..n {
2507 k.add(i, i, 2.0);
2508 if i + 1 < n {
2509 k.add(i, i + 1, -1.0);
2510 k.add(i + 1, i, -1.0);
2511 }
2512 }
2513 let c: Vec<f64> = vec![1.0; n];
2514 let f: Vec<f64> = (0..n).map(|i| (i as f64) * 0.1 + 0.05).collect();
2515
2516 let u = {
2517 let mut k2 = k.clone();
2518 k2.compress();
2519 SkylineLdlt::factor(&k2)
2520 .unwrap()
2521 .solve_lagrange(&c, &f)
2522 .unwrap()
2523 };
2524 let ct_u: f64 = c.iter().zip(u.iter()).map(|(a, b)| a * b).sum();
2525 assert!(
2526 ct_u.abs() < 1e-10,
2527 "SkylineLdlt constraint residual c^T u = {}",
2528 ct_u
2529 );
2530 }
2531
2532 #[test]
2533 fn cg_solve_breakdown_detection() {
2534 let mut a = SparseMatrix::new(2);
2537 a.add(0, 0, -1.0);
2538 a.add(1, 1, 1.0);
2539 let b = vec![1.0, 1.0];
2540 a.compress();
2541 let result = cg_solve(&a, &b, 100, SOLVER_TOL);
2542 assert!(
2544 !result.converged(),
2545 "CG should not converge on non-SPD matrix"
2546 );
2547 assert_eq!(result.status, CgStatus::NotPositiveDefinite);
2548 }
2550
2551 #[test]
2552 fn cg_solve_finite_checks() {
2553 let mut a = SparseMatrix::new(2);
2555 a.add(0, 0, 2.0);
2556 a.add(0, 1, 1.0);
2557 a.add(1, 0, 1.0);
2558 a.add(1, 1, 2.0);
2559 let b = vec![1.0, 2.0];
2560 a.compress();
2561 let result = cg_solve(&a, &b, 100, SOLVER_TOL);
2562 assert!(
2563 result.converged(),
2564 "CG should converge on SPD matrix: {}",
2565 result.residual
2566 );
2567 assert_eq!(result.status, CgStatus::Converged);
2568 assert!(
2570 result.x.iter().all(|v| v.is_finite()),
2571 "Solution has non-finite values"
2572 );
2573 assert!(result.residual.is_finite(), "Residual is non-finite");
2574 }
2575
2576 #[test]
2577 fn extrapolate_to_nodes_constant() {
2578 let w = [1.0; 6];
2580 let nodes = extrapolate_to_nodes(&w);
2581 for &v in &nodes {
2582 assert!((v - 1.0).abs() < 1e-10, "v = {v}");
2583 }
2584 }
2585
2586 #[test]
2587 fn tri6_degenerate_element_detection() {
2588 let nodes = vec![
2590 Point::new(0.0, 0.0),
2591 Point::new(1.0, 0.0),
2592 Point::new(2.0, 0.0), Point::new(0.5, 0.0), Point::new(1.5, 0.0), Point::new(1.0, 0.0), ];
2597 let elements = vec![[0, 1, 2, 3, 4, 5]];
2598 let mesh = Tri6Mesh { nodes, elements };
2599
2600 let result = validate_tri6_mesh(&mesh);
2602 assert!(result.is_err());
2603 assert_eq!(
2604 result.unwrap_err(),
2605 crate::mesh::fem::FemError::DegenerateElement
2606 );
2607
2608 let result = build_tri6_elements(&mesh, 200e9, 80e9, 7850.0);
2610 assert!(result.is_err());
2611 assert_eq!(
2612 result.unwrap_err(),
2613 crate::mesh::fem::FemError::DegenerateElement
2614 );
2615 }
2616
2617 #[test]
2618 fn tri6_orientation_check() {
2619 let points_ccw = [
2621 Point::new(0.0, 0.0),
2622 Point::new(1.0, 0.0),
2623 Point::new(0.0, 1.0),
2624 Point::new(0.5, 0.0),
2625 Point::new(0.5, 0.5),
2626 Point::new(0.0, 0.5),
2627 ];
2628 let result = Tri6::from_points(0, points_ccw, [0, 1, 2, 3, 4, 5], 1.0, 1.0, 1.0);
2629 assert!(result.is_ok(), "CCW triangle should be valid");
2630
2631 let points_cw = [
2633 Point::new(0.0, 0.0),
2634 Point::new(0.0, 1.0),
2635 Point::new(1.0, 0.0),
2636 Point::new(0.0, 0.5),
2637 Point::new(0.5, 0.5),
2638 Point::new(0.5, 0.0),
2639 ];
2640 let result = Tri6::from_points(0, points_cw, [0, 1, 2, 3, 4, 5], 1.0, 1.0, 1.0);
2641 assert!(result.is_err(), "CW triangle should fail");
2642 assert_eq!(
2643 result.unwrap_err(),
2644 crate::mesh::fem::FemError::InvalidElementOrientation
2645 );
2646 }
2647}