use crate::linear_elastic_fem::{
BoundaryConditions, ElasticMaterial, FemError, Preconditioner, SolverConfig, StressTensor,
RESIDUAL_NORM_FLOOR,
};
use crate::math::Fix128;
use crate::sdf_fem_mesh::SdfTetMesh;
#[inline]
fn sub3(a: [Fix128; 3], b: [Fix128; 3]) -> [Fix128; 3] {
[a[0] - b[0], a[1] - b[1], a[2] - b[2]]
}
#[inline]
fn cross3(a: [Fix128; 3], b: [Fix128; 3]) -> [Fix128; 3] {
[
a[1] * b[2] - a[2] * b[1],
a[2] * b[0] - a[0] * b[2],
a[0] * b[1] - a[1] * b[0],
]
}
#[inline]
fn dot3(a: [Fix128; 3], b: [Fix128; 3]) -> Fix128 {
a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}
#[inline]
fn div3(a: [Fix128; 3], d: Fix128) -> [Fix128; 3] {
[a[0] / d, a[1] / d, a[2] / d]
}
#[derive(Clone, Copy)]
struct Element {
nodes: [usize; 4],
grad: [[Fix128; 3]; 4],
volume: Fix128,
}
fn build_elements(mesh: &SdfTetMesh) -> Result<Vec<Element>, FemError> {
let vertex_count = mesh.vertices.len();
let six = Fix128::from_int(6);
let mut elements = Vec::with_capacity(mesh.tets.len());
for (t, tet) in mesh.tets.iter().enumerate() {
let mut nodes = [0usize; 4];
let mut p = [[Fix128::ZERO; 3]; 4];
for (i, &v) in tet.vertices.iter().enumerate() {
let idx = v as usize;
if idx >= vertex_count {
return Err(FemError::VertexOutOfRange {
vertex: v,
vertex_count,
});
}
nodes[i] = idx;
let q = mesh.vertices[idx];
p[i] = [
Fix128::from_f32(q[0]),
Fix128::from_f32(q[1]),
Fix128::from_f32(q[2]),
];
}
let e1 = sub3(p[1], p[0]);
let e2 = sub3(p[2], p[0]);
let e3 = sub3(p[3], p[0]);
let det = dot3(e1, cross3(e2, e3));
if det.is_zero() {
return Err(FemError::DegenerateElement { tet: t });
}
let g1 = div3(cross3(e2, e3), det);
let g2 = div3(cross3(e3, e1), det);
let g3 = div3(cross3(e1, e2), det);
let g0 = [
-(g1[0] + g2[0] + g3[0]),
-(g1[1] + g2[1] + g3[1]),
-(g1[2] + g2[2] + g3[2]),
];
elements.push(Element {
nodes,
grad: [g0, g1, g2, g3],
volume: det.abs() / six,
});
}
Ok(elements)
}
#[inline]
fn gather(element: &Element, u: &[Fix128]) -> [[Fix128; 3]; 4] {
let mut local = [[Fix128::ZERO; 3]; 4];
for (slot, &node) in local.iter_mut().zip(element.nodes.iter()) {
let base = node * 3;
*slot = [u[base], u[base + 1], u[base + 2]];
}
local
}
fn element_stress_local(
element: &Element,
u: &[[Fix128; 3]; 4],
lambda: Fix128,
mu: Fix128,
) -> StressTensor {
let mut exx = Fix128::ZERO;
let mut eyy = Fix128::ZERO;
let mut ezz = Fix128::ZERO;
let mut gxy = Fix128::ZERO;
let mut gyz = Fix128::ZERO;
let mut gzx = Fix128::ZERO;
for (g, node_u) in element.grad.iter().zip(u.iter()) {
let (ux, uy, uz) = (node_u[0], node_u[1], node_u[2]);
exx = exx + g[0] * ux;
eyy = eyy + g[1] * uy;
ezz = ezz + g[2] * uz;
gxy = gxy + g[1] * ux + g[0] * uy;
gyz = gyz + g[2] * uy + g[1] * uz;
gzx = gzx + g[2] * ux + g[0] * uz;
}
let trace = exx + eyy + ezz;
let two_mu = mu + mu;
StressTensor {
xx: lambda * trace + two_mu * exx,
yy: lambda * trace + two_mu * eyy,
zz: lambda * trace + two_mu * ezz,
xy: mu * gxy,
yz: mu * gyz,
zx: mu * gzx,
}
}
fn element_force_from_stress(element: &Element, s: StressTensor) -> [[Fix128; 3]; 4] {
let mut force = [[Fix128::ZERO; 3]; 4];
for (slot, g) in force.iter_mut().zip(element.grad.iter()) {
*slot = [
element.volume * (g[0] * s.xx + g[1] * s.xy + g[2] * s.zx),
element.volume * (g[1] * s.yy + g[0] * s.xy + g[2] * s.yz),
element.volume * (g[2] * s.zz + g[1] * s.yz + g[0] * s.zx),
];
}
force
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
pub enum MassLumping {
#[default]
Consistent,
Lumped,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct DynamicsConfig {
density: Fix128,
dt: Fix128,
inv_beta_dt2: Fix128,
lumping: MassLumping,
}
impl DynamicsConfig {
pub fn try_new(density: Fix128, dt: Fix128, lumping: MassLumping) -> Result<Self, FemError> {
if density <= Fix128::ZERO {
return Err(FemError::InvalidConfig("density must be positive"));
}
if dt <= Fix128::ZERO {
return Err(FemError::InvalidConfig("dt must be positive"));
}
let two_over_dt = Fix128::from_int(2)
.checked_div(dt)
.ok_or(FemError::InvalidConfig("2/dt overflows Fix128"))?;
if two_over_dt.is_zero() {
return Err(FemError::InvalidConfig("2/dt underflows to zero"));
}
let inv_beta_dt2 = two_over_dt
.checked_mul(two_over_dt)
.ok_or(FemError::InvalidConfig("4/dt squared overflows Fix128"))?;
Ok(Self {
density,
dt,
inv_beta_dt2,
lumping,
})
}
}
struct Operators {
elements: Vec<Element>,
lambda: Fix128,
mu: Fix128,
mass_scale: Vec<Fix128>,
lumping: MassLumping,
}
impl Operators {
fn new(
elements: Vec<Element>,
material: &ElasticMaterial,
dynamics: &DynamicsConfig,
) -> Result<Self, FemError> {
let (lambda, mu) = material.lame();
let rho_c0 = dynamics
.density
.checked_mul(dynamics.inv_beta_dt2)
.ok_or(FemError::InvalidConfig("4 rho / dt squared overflows"))?;
let divisor = match dynamics.lumping {
MassLumping::Consistent => Fix128::from_int(20),
MassLumping::Lumped => Fix128::from_int(4),
};
let per_volume = rho_c0 / divisor;
if per_volume.is_zero() {
return Err(FemError::InvalidConfig(
"4 rho / dt squared underflows: the mass term would vanish",
));
}
let mut mass_scale = Vec::with_capacity(elements.len());
for element in &elements {
let scale = per_volume
.checked_mul(element.volume)
.ok_or(FemError::InvalidConfig("an element mass overflows Fix128"))?;
if scale.is_zero() {
return Err(FemError::InvalidConfig(
"an element mass underflows to zero",
));
}
mass_scale.push(scale);
}
Ok(Self {
elements,
lambda,
mu,
mass_scale,
lumping: dynamics.lumping,
})
}
#[inline]
fn element_mass_force(
lumping: MassLumping,
scale: Fix128,
local: &[[Fix128; 3]; 4],
) -> [[Fix128; 3]; 4] {
let mut out = [[Fix128::ZERO; 3]; 4];
match lumping {
MassLumping::Lumped => {
for (slot, node_u) in out.iter_mut().zip(local.iter()) {
for axis in 0..3 {
slot[axis] = scale * node_u[axis];
}
}
}
MassLumping::Consistent => {
let mut total = [Fix128::ZERO; 3];
for node_u in local {
for axis in 0..3 {
total[axis] = total[axis] + node_u[axis];
}
}
for (slot, node_u) in out.iter_mut().zip(local.iter()) {
for axis in 0..3 {
slot[axis] = scale * (total[axis] + node_u[axis]);
}
}
}
}
out
}
#[inline]
fn element_mass_diagonal(lumping: MassLumping, scale: Fix128) -> Fix128 {
match lumping {
MassLumping::Lumped => scale,
MassLumping::Consistent => scale + scale,
}
}
fn apply_mass(&self, u: &[Fix128], out: &mut [Fix128]) {
out.fill(Fix128::ZERO);
for (element, &scale) in self.elements.iter().zip(self.mass_scale.iter()) {
let local = gather(element, u);
let force = Self::element_mass_force(self.lumping, scale, &local);
for (f, &node) in force.iter().zip(element.nodes.iter()) {
let base = node * 3;
for axis in 0..3 {
out[base + axis] = out[base + axis] + f[axis];
}
}
}
}
fn apply_effective(&self, u: &[Fix128], out: &mut [Fix128]) {
out.fill(Fix128::ZERO);
for (element, &scale) in self.elements.iter().zip(self.mass_scale.iter()) {
let local = gather(element, u);
let stress = element_stress_local(element, &local, self.lambda, self.mu);
let stiffness = element_force_from_stress(element, stress);
let mass = Self::element_mass_force(self.lumping, scale, &local);
for ((k, m), &node) in stiffness.iter().zip(mass.iter()).zip(element.nodes.iter()) {
let base = node * 3;
for axis in 0..3 {
out[base + axis] = out[base + axis] + k[axis] + m[axis];
}
}
}
}
fn mass_diagonal(&self, ndof: usize) -> Vec<Fix128> {
let mut diag = vec![Fix128::ZERO; ndof];
for (element, &scale) in self.elements.iter().zip(self.mass_scale.iter()) {
let m = Self::element_mass_diagonal(self.lumping, scale);
for &node in &element.nodes {
let base = node * 3;
for axis in 0..3 {
diag[base + axis] = diag[base + axis] + m;
}
}
}
diag
}
fn effective_diagonal(&self, ndof: usize) -> Vec<Fix128> {
let mut diag = self.mass_diagonal(ndof);
let lambda_2mu = self.lambda + self.mu + self.mu;
for element in &self.elements {
for (g, &node) in element.grad.iter().zip(element.nodes.iter()) {
let sq = [g[0] * g[0], g[1] * g[1], g[2] * g[2]];
let base = node * 3;
for axis in 0..3 {
let others = sq[(axis + 1) % 3] + sq[(axis + 2) % 3];
diag[base + axis] = diag[base + axis]
+ element.volume * (lambda_2mu * sq[axis] + self.mu * others);
}
}
}
diag
}
}
fn dot(a: &[Fix128], b: &[Fix128]) -> Fix128 {
let mut acc = Fix128::ZERO;
for (x, y) in a.iter().zip(b.iter()) {
acc = acc + *x * *y;
}
acc
}
fn build_preconditioner(
diag: &[Fix128],
is_free: &[bool],
config: &SolverConfig,
) -> Result<Vec<Fix128>, FemError> {
let ndof = diag.len();
let mut diag_sum = Fix128::ZERO;
let mut free_count = 0u32;
for (d, value) in diag.iter().enumerate() {
if !is_free[d] {
continue;
}
if *value <= Fix128::ZERO {
return Err(FemError::UnderConstrained);
}
diag_sum = diag_sum + *value;
free_count += 1;
}
let mean_diag = if free_count == 0 {
Fix128::ONE
} else {
diag_sum / Fix128::from_int(i64::from(free_count))
};
let mut precond = vec![Fix128::ZERO; ndof];
for d in 0..ndof {
if is_free[d] {
precond[d] = match config.preconditioner() {
Preconditioner::JacobiScaled => mean_diag / diag[d],
_ => Fix128::ONE,
};
}
}
Ok(precond)
}
fn conjugate_gradient<A>(
b: &[Fix128],
is_free: &[bool],
precond: &[Fix128],
config: &SolverConfig,
mut apply: A,
) -> Result<(Vec<Fix128>, u32), FemError>
where
A: FnMut(&[Fix128], &mut [Fix128]),
{
let ndof = b.len();
let mut scratch = vec![Fix128::ZERO; ndof];
let mut x = vec![Fix128::ZERO; ndof];
let mut r = b.to_vec();
let mut z = vec![Fix128::ZERO; ndof];
for d in 0..ndof {
if is_free[d] {
z[d] = r[d] * precond[d];
}
}
let mut p = z.clone();
let mut rz = dot(&r, &z);
let b_norm = dot(b, b).sqrt();
let requested = config.relative_tolerance() * b_norm;
let target = if requested > RESIDUAL_NORM_FLOOR {
requested
} else {
RESIDUAL_NORM_FLOOR
};
let mut iterations = 0u32;
let mut residual_norm = dot(&r, &r).sqrt();
let mut best_residual = residual_norm;
let mut since_improvement = 0u32;
while residual_norm > target {
if iterations >= config.max_iterations() {
return Err(FemError::NotConverged {
iterations,
relative_residual: relative(residual_norm, b_norm),
});
}
let window = {
let scaled =
config.stagnation_window_fraction() * Fix128::from_int(i64::from(iterations));
let scaled = if scaled.is_negative() {
0
} else {
u32::try_from(scaled.hi).unwrap_or(u32::MAX)
};
scaled.max(config.stagnation_min_window())
};
if since_improvement >= window {
return Err(FemError::Stagnated {
iterations,
relative_residual: relative(best_residual, b_norm),
without_improvement: since_improvement,
});
}
apply(&p, &mut scratch);
for (d, value) in scratch.iter_mut().enumerate() {
if !is_free[d] {
*value = Fix128::ZERO;
}
}
let pkp = dot(&p, &scratch);
if pkp <= Fix128::ZERO {
if iterations == 0 {
return Err(FemError::UnderConstrained);
}
return Err(FemError::Stagnated {
iterations,
relative_residual: relative(best_residual, b_norm),
without_improvement: since_improvement,
});
}
let alpha = rz / pkp;
for d in 0..ndof {
if is_free[d] {
x[d] = x[d] + alpha * p[d];
r[d] = r[d] - alpha * scratch[d];
}
}
for d in 0..ndof {
if is_free[d] {
z[d] = r[d] * precond[d];
}
}
let rz_next = dot(&r, &z);
let beta = rz_next / rz;
for d in 0..ndof {
if is_free[d] {
p[d] = z[d] + beta * p[d];
}
}
rz = rz_next;
residual_norm = dot(&r, &r).sqrt();
iterations += 1;
if residual_norm < best_residual - best_residual * config.stagnation_min_improvement() {
best_residual = residual_norm;
since_improvement = 0;
} else {
if residual_norm < best_residual {
best_residual = residual_norm;
}
since_improvement += 1;
}
}
Ok((x, iterations))
}
#[inline]
fn relative(residual_norm: Fix128, b_norm: Fix128) -> Fix128 {
if b_norm.is_zero() {
Fix128::ZERO
} else {
residual_norm / b_norm
}
}
#[inline]
fn newmark_predictor(u: Fix128, v: Fix128, a: Fix128, dt: Fix128, half_dt: Fix128) -> Fix128 {
u + dt * v + half_dt * (half_dt * a)
}
#[inline]
fn newmark_corrector(
next_u: Fix128,
predictor: Fix128,
v: Fix128,
a: Fix128,
inv_beta_dt2: Fix128,
half_dt: Fix128,
) -> (Fix128, Fix128) {
let next_a = (next_u - predictor) * inv_beta_dt2;
(v + half_dt * (a + next_a), next_a)
}
pub struct TransientSolver {
ops: Operators,
dt: Fix128,
inv_beta_dt2: Fix128,
half_dt: Fix128,
is_free: Vec<bool>,
prescribed_value: Vec<Fix128>,
load: Vec<Fix128>,
precond: Vec<Fix128>,
config: SolverConfig,
u: Vec<Fix128>,
v: Vec<Fix128>,
a: Vec<Fix128>,
}
impl TransientSolver {
pub fn new(
mesh: &SdfTetMesh,
material: &ElasticMaterial,
boundary: &BoundaryConditions,
dynamics: &DynamicsConfig,
config: &SolverConfig,
) -> Result<Self, FemError> {
let vertex_count = mesh.vertices.len();
if vertex_count == 0 || mesh.tets.is_empty() {
return Err(FemError::EmptyMesh);
}
for &(vertex, _, _) in boundary.prescribed().iter().chain(boundary.loads().iter()) {
if vertex as usize >= vertex_count {
return Err(FemError::VertexOutOfRange {
vertex,
vertex_count,
});
}
}
let elements = build_elements(mesh)?;
let ops = Operators::new(elements, material, dynamics)?;
let ndof = vertex_count * 3;
let mut prescribed_value = vec![Fix128::ZERO; ndof];
let mut is_free = vec![true; ndof];
for &(vertex, axis, value) in boundary.prescribed() {
let d = vertex as usize * 3 + axis.index();
is_free[d] = false;
prescribed_value[d] = value;
}
let mut load = vec![Fix128::ZERO; ndof];
for &(vertex, axis, force) in boundary.loads() {
let d = vertex as usize * 3 + axis.index();
if is_free[d] {
load[d] = load[d] + force;
}
}
let u = prescribed_value.clone();
let v = vec![Fix128::ZERO; ndof];
let mut keff_u0 = vec![Fix128::ZERO; ndof];
ops.apply_effective(&u, &mut keff_u0);
let mut mass_u0 = vec![Fix128::ZERO; ndof];
ops.apply_mass(&u, &mut mass_u0);
let mut rhs = vec![Fix128::ZERO; ndof];
for d in 0..ndof {
rhs[d] = if is_free[d] {
load[d] - (keff_u0[d] - mass_u0[d])
} else {
Fix128::ZERO
};
}
let mass_diag = ops.mass_diagonal(ndof);
let mass_precond = build_preconditioner(&mass_diag, &is_free, config)?;
let (scaled_a, _) = conjugate_gradient(&rhs, &is_free, &mass_precond, config, |p, out| {
ops.apply_mass(p, out);
})?;
let a = scaled_a
.iter()
.map(|x| *x * dynamics.inv_beta_dt2)
.collect::<Vec<_>>();
let diag = ops.effective_diagonal(ndof);
let precond = build_preconditioner(&diag, &is_free, config)?;
Ok(Self {
ops,
dt: dynamics.dt,
inv_beta_dt2: dynamics.inv_beta_dt2,
half_dt: dynamics.dt.half(),
is_free,
prescribed_value,
load,
precond,
config: *config,
u,
v,
a,
})
}
pub fn step(&mut self) -> Result<u32, FemError> {
let ndof = self.u.len();
let mut predictor = vec![Fix128::ZERO; ndof];
for (d, slot) in predictor.iter_mut().enumerate() {
*slot = if self.is_free[d] {
newmark_predictor(self.u[d], self.v[d], self.a[d], self.dt, self.half_dt)
} else {
self.prescribed_value[d]
};
}
let mut mass_predictor = vec![Fix128::ZERO; ndof];
self.ops.apply_mass(&predictor, &mut mass_predictor);
let mut keff_prescribed = vec![Fix128::ZERO; ndof];
self.ops
.apply_effective(&self.prescribed_value, &mut keff_prescribed);
let mut b = vec![Fix128::ZERO; ndof];
for d in 0..ndof {
b[d] = if self.is_free[d] {
self.load[d] + mass_predictor[d] - keff_prescribed[d]
} else {
Fix128::ZERO
};
}
let ops = &self.ops;
let (mut next_u, iterations) =
conjugate_gradient(&b, &self.is_free, &self.precond, &self.config, |p, out| {
ops.apply_effective(p, out);
})?;
for d in 0..ndof {
if self.is_free[d] {
let (next_v, next_a) = newmark_corrector(
next_u[d],
predictor[d],
self.v[d],
self.a[d],
self.inv_beta_dt2,
self.half_dt,
);
self.v[d] = next_v;
self.a[d] = next_a;
} else {
next_u[d] = self.prescribed_value[d];
}
}
self.u = next_u;
Ok(iterations)
}
#[must_use]
pub fn displacements(&self) -> &[Fix128] {
&self.u
}
#[must_use]
pub fn velocities(&self) -> &[Fix128] {
&self.v
}
#[must_use]
pub fn accelerations(&self) -> &[Fix128] {
&self.a
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::sdf_fem_mesh::Tetrahedron;
fn dyadic_volume_tet() -> SdfTetMesh {
let mut mesh = SdfTetMesh::default();
mesh.vertices.push([0.0, 0.0, 0.0]);
mesh.vertices.push([1.0, 0.0, 0.0]);
mesh.vertices.push([0.0, 1.0, 0.0]);
mesh.vertices.push([0.0, 0.0, 3.0]);
mesh.tets.push(Tetrahedron {
vertices: [0, 1, 2, 3],
});
mesh
}
fn ops_for(lumping: MassLumping, density: Fix128, dt: Fix128) -> Operators {
let mesh = dyadic_volume_tet();
let elements = build_elements(&mesh).expect("non-degenerate");
let material = ElasticMaterial::new(Fix128::from_int(3500), Fix128::from_f64(0.35))
.expect("valid material");
let dynamics = DynamicsConfig::try_new(density, dt, lumping).expect("valid dynamics");
Operators::new(elements, &material, &dynamics).expect("mass fits")
}
#[test]
fn the_consistent_element_mass_is_the_closed_form_moment() {
let ops = ops_for(
MassLumping::Consistent,
Fix128::from_int(20),
Fix128::from_int(2),
);
assert_eq!(ops.mass_scale.len(), 1);
assert_eq!(ops.mass_scale[0], Fix128::ONE.half());
}
#[test]
fn the_lumped_element_mass_is_a_quarter_of_the_element_mass() {
let ops = ops_for(
MassLumping::Lumped,
Fix128::from_int(24),
Fix128::from_int(2),
);
assert_eq!(ops.mass_scale[0], Fix128::from_int(3));
}
#[test]
fn both_distributions_carry_the_same_total_mass() {
for lumping in [MassLumping::Consistent, MassLumping::Lumped] {
let ops = ops_for(lumping, Fix128::from_int(60), Fix128::from_int(2));
let ones = vec![Fix128::ONE; 12];
let mut out = vec![Fix128::ZERO; 12];
ops.apply_mass(&ones, &mut out);
let total = out[0] + out[3] + out[6] + out[9];
assert_eq!(total, Fix128::from_int(30), "{lumping:?}");
}
}
#[test]
fn the_consistent_diagonal_is_twice_the_off_diagonal() {
let ops = ops_for(
MassLumping::Consistent,
Fix128::from_int(20),
Fix128::from_int(2),
);
let scale = ops.mass_scale[0];
let mut unit = vec![Fix128::ZERO; 12];
unit[0] = Fix128::ONE;
let mut out = vec![Fix128::ZERO; 12];
ops.apply_mass(&unit, &mut out);
assert_eq!(out[0], scale + scale, "diagonal");
assert_eq!(out[3], scale, "off-diagonal");
assert_eq!(out[6], scale, "off-diagonal");
assert_eq!(out[9], scale, "off-diagonal");
assert_eq!(
ops.mass_diagonal(12)[0],
out[0],
"mass_diagonal must agree with apply_mass on a unit impulse"
);
}
#[test]
fn the_effective_diagonal_is_the_operator_applied_to_a_unit_vector() {
const BOUND: i128 = 12_480; let mut worst = 0i128;
for lumping in [MassLumping::Consistent, MassLumping::Lumped] {
let ops = ops_for(lumping, Fix128::from_int(60), Fix128::from_int(2));
let diag = ops.effective_diagonal(12);
for d in 0..12 {
let mut unit = vec![Fix128::ZERO; 12];
unit[d] = Fix128::ONE;
let mut out = vec![Fix128::ZERO; 12];
ops.apply_effective(&unit, &mut out);
let delta = raw(diag[d]) - raw(out[d]);
worst = worst.max(delta.abs());
assert!(
delta.abs() <= BOUND,
"{lumping:?} dof {d}: diag {:?} but eᵀAe {:?}, {delta} raw units apart",
diag[d],
out[d]
);
assert!(
diag[d] > Fix128::ZERO,
"{lumping:?} dof {d}: the diagonal of a positive definite operator \
must be positive"
);
}
}
assert!(
worst > 0,
"the two paths agreed bit for bit, so the {BOUND}-unit bound is vacuous and \
the comment above about rounding orders is wrong"
);
}
fn raw(v: Fix128) -> i128 {
(i128::from(v.hi) << 64) | i128::from(v.lo)
}
fn dyadic_dt(shift: u32) -> Fix128 {
Fix128::from_raw(0, 1u64 << (64 - shift))
}
struct Sdof {
m: Fix128,
k: Fix128,
dt: Fix128,
inv_beta_dt2: Fix128,
half_dt: Fix128,
x: Fix128,
v: Fix128,
a: Fix128,
}
impl Sdof {
fn new(m: Fix128, k: Fix128, dt: Fix128, x0: Fix128) -> Self {
let two_over_dt = Fix128::from_int(2) / dt;
let inv_beta_dt2 = two_over_dt * two_over_dt;
Self {
m,
k,
dt,
inv_beta_dt2,
half_dt: dt.half(),
x: x0,
v: Fix128::ZERO,
a: (Fix128::ZERO - k * x0) / m,
}
}
fn step(&mut self) {
let predictor = newmark_predictor(self.x, self.v, self.a, self.dt, self.half_dt);
let m_dot = self.m * self.inv_beta_dt2;
let next_x = (m_dot * predictor) / (self.k + m_dot);
let (next_v, next_a) = newmark_corrector(
next_x,
predictor,
self.v,
self.a,
self.inv_beta_dt2,
self.half_dt,
);
self.x = next_x;
self.v = next_v;
self.a = next_a;
}
fn energy(&self) -> Fix128 {
(self.m * self.v * self.v + self.k * self.x * self.x).half()
}
}
#[test]
fn the_newmark_coefficients_are_exact_for_a_dyadic_step() {
for shift in 1u32..=10 {
let dt = dyadic_dt(shift);
let half_dt = dt.half();
let two_over_dt = Fix128::from_int(2) / dt;
let inv_beta_dt2 = two_over_dt * two_over_dt;
assert_eq!(
inv_beta_dt2,
Fix128::from_int(1i64 << (2 * shift + 2)),
"(2/dt)² at dt = 2^-{shift}"
);
assert_eq!(
inv_beta_dt2,
Fix128::from_int(4) / (dt * dt),
"invert-first and direct disagree at dt = 2^-{shift}, \
so the rewrite is not free after all"
);
for a in [Fix128::ONE, Fix128::from_int(-7), Fix128::from_int(1024)] {
assert_eq!(
half_dt * (half_dt * a) * inv_beta_dt2,
a,
"the predictor and corrector weights do not invert at \
dt = 2^-{shift}, a = {a:?}"
);
}
assert_eq!(half_dt + half_dt, dt, "dt/2 at dt = 2^-{shift}");
}
}
#[test]
fn the_omega_dt_equal_two_orbit_is_the_closed_form_four_cycle() {
let m = Fix128::ONE;
let k = Fix128::from_int(256);
let omega = Fix128::from_int(16);
let mut s = Sdof::new(m, k, dyadic_dt(3), Fix128::ONE);
let want = [
(Fix128::ZERO, Fix128::ZERO - omega),
(Fix128::NEG_ONE, Fix128::ZERO),
(Fix128::ZERO, omega),
(Fix128::ONE, Fix128::ZERO),
];
for cycle in 0..4 {
for (phase, &(wx, wv)) in want.iter().enumerate() {
s.step();
assert_eq!(s.x, wx, "x at cycle {cycle} phase {phase}");
assert_eq!(s.v, wv, "v at cycle {cycle} phase {phase}");
}
}
for _ in 0..4096 {
s.step();
}
assert_eq!(s.x, Fix128::ONE, "x after 4112 steps");
assert_eq!(s.v, Fix128::ZERO, "v after 4112 steps");
}
#[test]
fn a_general_step_drifts_in_energy_within_the_measured_bound() {
const WORST_MEASURED: i128 = 925_109;
let bound = WORST_MEASURED * 4;
let mut any_nonzero = false;
for (mi, ki, shift) in [(1i64, 100i64, 6u32), (3, 7, 5), (1, 1, 4), (5, 1000, 7)] {
let m = Fix128::from_int(mi);
let k = Fix128::from_int(ki);
let mut s = Sdof::new(m, k, dyadic_dt(shift), Fix128::ONE);
let e0 = s.energy();
let mut worst = 0i128;
let mut peak = Fix128::ZERO;
for _ in 0..4096 {
s.step();
let d = raw(s.energy()) - raw(e0);
if d.abs() > worst.abs() {
worst = d;
}
if s.x.abs() > peak {
peak = s.x.abs();
}
}
any_nonzero |= worst != 0;
assert!(
worst.abs() <= bound,
"m={mi} k={ki} dt=2^-{shift}: energy drifted {worst} raw units, bound {bound}"
);
assert!(
peak <= Fix128::ONE,
"m={mi} k={ki} dt=2^-{shift}: amplitude grew to {} > 1, \
so the truncation is no longer toward zero",
peak.to_f64()
);
}
assert!(
any_nonzero,
"every scene conserved energy exactly, so this test is measuring nothing \
and the bound above is vacuous"
);
}
}