1use thiserror::Error;
2
3use super::{material::SolidMaterialError, quality::SolidElementQuality, SolidMaterial};
4
5pub const TETRAHEDRON4_NODE_DOF_COUNT: usize = 3;
6pub const TETRAHEDRON4_ELEMENT_NODE_COUNT: usize = 4;
7pub const TETRAHEDRON4_ELEMENT_DOF_COUNT: usize =
8 TETRAHEDRON4_NODE_DOF_COUNT * TETRAHEDRON4_ELEMENT_NODE_COUNT;
9
10pub type Tetrahedron4Matrix12 =
11 [[f64; TETRAHEDRON4_ELEMENT_DOF_COUNT]; TETRAHEDRON4_ELEMENT_DOF_COUNT];
12pub type Tetrahedron4BMatrix = [[f64; TETRAHEDRON4_ELEMENT_DOF_COUNT]; 6];
13pub type ElasticityMatrix = [[f64; 6]; 6];
14
15#[derive(Debug, Clone, Copy, PartialEq)]
16pub struct Tetrahedron4ElementGeometry {
17 pub nodes_m: [[f64; 3]; TETRAHEDRON4_ELEMENT_NODE_COUNT],
18}
19
20#[derive(Debug, Error, Clone, PartialEq)]
21pub enum Tetrahedron4ElementError {
22 #[error("Tetrahedron4 node coordinates must be finite")]
23 NonFiniteCoordinate,
24 #[error("Tetrahedron4 element volume must be positive and finite")]
25 DegenerateOrInverted,
26 #[error("Tetrahedron4 material is invalid: {0}")]
27 InvalidMaterial(#[from] SolidMaterialError),
28}
29
30impl Tetrahedron4ElementGeometry {
31 pub fn volume_m3(self) -> Result<f64, Tetrahedron4ElementError> {
32 validate_nodes(self.nodes_m)?;
33 let volume = signed_volume(self.nodes_m) / 6.0;
34 if volume.is_finite() && volume > 0.0 {
35 Ok(volume)
36 } else {
37 Err(Tetrahedron4ElementError::DegenerateOrInverted)
38 }
39 }
40
41 pub fn shape_function_gradients(self) -> Result<[[f64; 3]; 4], Tetrahedron4ElementError> {
42 validate_nodes(self.nodes_m)?;
43 let inverse = inverse_jacobian(self.nodes_m)?;
44 let reference_gradients = [
45 [-1.0, -1.0, -1.0],
46 [1.0, 0.0, 0.0],
47 [0.0, 1.0, 0.0],
48 [0.0, 0.0, 1.0],
49 ];
50 let mut gradients = [[0.0_f64; 3]; 4];
51 for (node, reference) in reference_gradients.into_iter().enumerate() {
52 for axis in 0..3 {
53 gradients[node][axis] = inverse[0][axis] * reference[0]
54 + inverse[1][axis] * reference[1]
55 + inverse[2][axis] * reference[2];
56 }
57 }
58 Ok(gradients)
59 }
60
61 pub fn quality(self) -> Result<SolidElementQuality, Tetrahedron4ElementError> {
62 Ok(SolidElementQuality::from_tetrahedron4_nodes(
63 self.nodes_m,
64 self.volume_m3()?,
65 ))
66 }
67}
68
69pub fn strain_displacement_matrix(
70 geometry: Tetrahedron4ElementGeometry,
71) -> Result<Tetrahedron4BMatrix, Tetrahedron4ElementError> {
72 let gradients = geometry.shape_function_gradients()?;
73 let mut b = [[0.0_f64; TETRAHEDRON4_ELEMENT_DOF_COUNT]; 6];
74 for (node, gradient) in gradients.into_iter().enumerate() {
75 let col = node * TETRAHEDRON4_NODE_DOF_COUNT;
76 let [dn_dx, dn_dy, dn_dz] = gradient;
77 b[0][col] = dn_dx;
78 b[1][col + 1] = dn_dy;
79 b[2][col + 2] = dn_dz;
80 b[3][col + 1] = dn_dz;
81 b[3][col + 2] = dn_dy;
82 b[4][col] = dn_dz;
83 b[4][col + 2] = dn_dx;
84 b[5][col] = dn_dy;
85 b[5][col + 1] = dn_dx;
86 }
87 Ok(b)
88}
89
90pub fn elasticity_matrix(
91 material: SolidMaterial,
92) -> Result<ElasticityMatrix, Tetrahedron4ElementError> {
93 let lambda = material.lame_lambda_pa()?;
94 let mu = material.shear_modulus_pa()?;
95 let mut d = [[0.0_f64; 6]; 6];
96 for (row, diagonal) in d.iter_mut().enumerate().take(3) {
97 diagonal[..3].fill(lambda);
98 diagonal[row] += 2.0 * mu;
99 }
100 d[3][3] = mu;
101 d[4][4] = mu;
102 d[5][5] = mu;
103 Ok(d)
104}
105
106pub fn global_stiffness_matrix(
107 material: SolidMaterial,
108 geometry: Tetrahedron4ElementGeometry,
109) -> Result<Tetrahedron4Matrix12, Tetrahedron4ElementError> {
110 let volume = geometry.volume_m3()?;
111 let b = strain_displacement_matrix(geometry)?;
112 let d = elasticity_matrix(material)?;
113 let mut db = [[0.0_f64; TETRAHEDRON4_ELEMENT_DOF_COUNT]; 6];
114 for row in 0..6 {
115 for col in 0..TETRAHEDRON4_ELEMENT_DOF_COUNT {
116 db[row][col] = (0..6).map(|idx| d[row][idx] * b[idx][col]).sum();
117 }
118 }
119
120 let mut k = [[0.0_f64; TETRAHEDRON4_ELEMENT_DOF_COUNT]; TETRAHEDRON4_ELEMENT_DOF_COUNT];
121 for row in 0..TETRAHEDRON4_ELEMENT_DOF_COUNT {
122 for col in row..TETRAHEDRON4_ELEMENT_DOF_COUNT {
123 let value = volume * (0..6).map(|idx| b[idx][row] * db[idx][col]).sum::<f64>();
124 k[row][col] = value;
125 k[col][row] = value;
126 }
127 }
128 Ok(k)
129}
130
131fn validate_nodes(nodes_m: [[f64; 3]; 4]) -> Result<(), Tetrahedron4ElementError> {
132 if nodes_m.iter().flatten().all(|value| value.is_finite()) {
133 Ok(())
134 } else {
135 Err(Tetrahedron4ElementError::NonFiniteCoordinate)
136 }
137}
138
139fn inverse_jacobian(nodes_m: [[f64; 3]; 4]) -> Result<[[f64; 3]; 3], Tetrahedron4ElementError> {
140 let j = [
141 sub(nodes_m[1], nodes_m[0]),
142 sub(nodes_m[2], nodes_m[0]),
143 sub(nodes_m[3], nodes_m[0]),
144 ];
145 let det = dot(j[0], cross(j[1], j[2]));
146 if !det.is_finite() || det <= 0.0 {
147 return Err(Tetrahedron4ElementError::DegenerateOrInverted);
148 }
149 let inv_det = 1.0 / det;
150 Ok([
151 [
152 (j[1][1] * j[2][2] - j[1][2] * j[2][1]) * inv_det,
153 (j[0][2] * j[2][1] - j[0][1] * j[2][2]) * inv_det,
154 (j[0][1] * j[1][2] - j[0][2] * j[1][1]) * inv_det,
155 ],
156 [
157 (j[1][2] * j[2][0] - j[1][0] * j[2][2]) * inv_det,
158 (j[0][0] * j[2][2] - j[0][2] * j[2][0]) * inv_det,
159 (j[0][2] * j[1][0] - j[0][0] * j[1][2]) * inv_det,
160 ],
161 [
162 (j[1][0] * j[2][1] - j[1][1] * j[2][0]) * inv_det,
163 (j[0][1] * j[2][0] - j[0][0] * j[2][1]) * inv_det,
164 (j[0][0] * j[1][1] - j[0][1] * j[1][0]) * inv_det,
165 ],
166 ])
167}
168
169fn signed_volume(nodes_m: [[f64; 3]; 4]) -> f64 {
170 dot(
171 sub(nodes_m[1], nodes_m[0]),
172 cross(sub(nodes_m[2], nodes_m[0]), sub(nodes_m[3], nodes_m[0])),
173 )
174}
175
176fn sub(left: [f64; 3], right: [f64; 3]) -> [f64; 3] {
177 [left[0] - right[0], left[1] - right[1], left[2] - right[2]]
178}
179
180fn dot(left: [f64; 3], right: [f64; 3]) -> f64 {
181 left[0] * right[0] + left[1] * right[1] + left[2] * right[2]
182}
183
184fn cross(left: [f64; 3], right: [f64; 3]) -> [f64; 3] {
185 [
186 left[1] * right[2] - left[2] * right[1],
187 left[2] * right[0] - left[0] * right[2],
188 left[0] * right[1] - left[1] * right[0],
189 ]
190}
191
192#[cfg(test)]
193mod tests {
194 use super::*;
195
196 fn unit_tetrahedron() -> Tetrahedron4ElementGeometry {
197 Tetrahedron4ElementGeometry {
198 nodes_m: [
199 [0.0, 0.0, 0.0],
200 [1.0, 0.0, 0.0],
201 [0.0, 1.0, 0.0],
202 [0.0, 0.0, 1.0],
203 ],
204 }
205 }
206
207 fn steel() -> SolidMaterial {
208 SolidMaterial {
209 youngs_modulus_pa: 200.0e9,
210 poisson_ratio: 0.3,
211 }
212 }
213
214 #[test]
215 fn tetrahedron4_volume_for_unit_tetrahedron() {
216 let volume = unit_tetrahedron()
217 .volume_m3()
218 .expect("unit Tetrahedron volume");
219 assert!((volume - 1.0 / 6.0).abs() < 1.0e-14);
220 }
221
222 #[test]
223 fn tetrahedron4_gradients_are_constant_and_partition_unity() {
224 let gradients = unit_tetrahedron()
225 .shape_function_gradients()
226 .expect("unit Tetrahedron gradients");
227 assert_eq!(gradients[0], [-1.0, -1.0, -1.0]);
228 assert_eq!(gradients[1], [1.0, 0.0, 0.0]);
229 assert_eq!(gradients[2], [0.0, 1.0, 0.0]);
230 assert_eq!(gradients[3], [0.0, 0.0, 1.0]);
231 for axis in 0..3 {
232 let sum = gradients.iter().map(|gradient| gradient[axis]).sum::<f64>();
233 assert!(sum.abs() < 1.0e-14);
234 }
235 }
236
237 #[test]
238 fn tetrahedron4_b_matrix_rejects_rigid_translation_and_rotation() {
239 let b = strain_displacement_matrix(unit_tetrahedron()).expect("b matrix");
240 let translation = [
241 2.0, -3.0, 4.0, 2.0, -3.0, 4.0, 2.0, -3.0, 4.0, 2.0, -3.0, 4.0,
242 ];
243 let rotation_z = [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0];
244 for displacement in [translation, rotation_z] {
245 for strain_row in b {
246 let strain = strain_row
247 .iter()
248 .zip(displacement)
249 .map(|(lhs, rhs)| lhs * rhs)
250 .sum::<f64>();
251 assert!(strain.abs() < 1.0e-14);
252 }
253 }
254 }
255
256 #[test]
257 fn tetrahedron4_stiffness_is_symmetric_and_positive_semidefinite_for_samples() {
258 let stiffness = global_stiffness_matrix(steel(), unit_tetrahedron()).expect("stiffness");
259 for row in 0..TETRAHEDRON4_ELEMENT_DOF_COUNT {
260 for col in 0..TETRAHEDRON4_ELEMENT_DOF_COUNT {
261 assert!((stiffness[row][col] - stiffness[col][row]).abs() < 1.0e-5);
262 }
263 }
264
265 for displacement in [
266 [1.0; TETRAHEDRON4_ELEMENT_DOF_COUNT],
267 [0.0, 0.0, 0.0, 0.1, 0.2, 0.3, -0.2, 0.4, 0.1, 0.3, -0.1, 0.2],
268 [
269 1.0, -2.0, 3.0, 4.0, -5.0, 6.0, -7.0, 8.0, -9.0, 10.0, -11.0, 12.0,
270 ],
271 ] {
272 let energy = quadratic_form(&stiffness, displacement);
273 assert!(energy >= -1.0e-3, "energy={energy}");
274 }
275 }
276
277 #[test]
278 fn tetrahedron4_stiffness_has_near_zero_rigid_body_residual() {
279 let stiffness = global_stiffness_matrix(steel(), unit_tetrahedron()).expect("stiffness");
280 let translation_x = [1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0];
281 let residual = mat_vec(&stiffness, translation_x);
282 assert!(residual.iter().all(|value| value.abs() < 1.0e-3));
283 }
284
285 #[test]
286 fn inverted_tetrahedron4_is_rejected() {
287 let inverted = Tetrahedron4ElementGeometry {
288 nodes_m: [
289 [0.0, 0.0, 0.0],
290 [0.0, 1.0, 0.0],
291 [1.0, 0.0, 0.0],
292 [0.0, 0.0, 1.0],
293 ],
294 };
295 assert_eq!(
296 inverted
297 .volume_m3()
298 .expect_err("inverted Tetrahedron should fail"),
299 Tetrahedron4ElementError::DegenerateOrInverted
300 );
301 }
302
303 fn quadratic_form(
304 matrix: &Tetrahedron4Matrix12,
305 displacement: [f64; TETRAHEDRON4_ELEMENT_DOF_COUNT],
306 ) -> f64 {
307 let product = mat_vec(matrix, displacement);
308 product
309 .into_iter()
310 .zip(displacement)
311 .map(|(lhs, rhs)| lhs * rhs)
312 .sum()
313 }
314
315 fn mat_vec(
316 matrix: &Tetrahedron4Matrix12,
317 displacement: [f64; TETRAHEDRON4_ELEMENT_DOF_COUNT],
318 ) -> [f64; TETRAHEDRON4_ELEMENT_DOF_COUNT] {
319 let mut result = [0.0_f64; TETRAHEDRON4_ELEMENT_DOF_COUNT];
320 for row in 0..TETRAHEDRON4_ELEMENT_DOF_COUNT {
321 result[row] = matrix[row]
322 .iter()
323 .zip(displacement)
324 .map(|(lhs, rhs)| lhs * rhs)
325 .sum();
326 }
327 result
328 }
329}