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