use crate::math::{Fix128, Vec3Fix};
#[cfg(feature = "std")]
use crate::sim_field::ScalarField3D;
use core::fmt;
#[cfg(not(feature = "std"))]
use alloc::vec;
#[cfg(not(feature = "std"))]
use alloc::vec::Vec;
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum CoupledFieldError {
EmptyGrid {
resolution: (usize, usize, usize),
},
ResolutionMismatch {
channel: (usize, usize, usize),
other: (usize, usize, usize),
},
BoundsMismatch,
NoParticipants,
}
impl fmt::Display for CoupledFieldError {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Self::EmptyGrid { resolution } => {
let (nx, ny, nz) = resolution;
write!(f, "empty coupled field grid ({nx}x{ny}x{nz})")
}
Self::ResolutionMismatch { channel, other } => {
let (cx, cy, cz) = channel;
let (ox, oy, oz) = other;
write!(
f,
"coupled field resolution mismatch: channel {cx}x{cy}x{cz}, other {ox}x{oy}x{oz}"
)
}
Self::BoundsMismatch => {
write!(f, "coupled field bounds mismatch")
}
Self::NoParticipants => {
write!(f, "no participants to reconcile")
}
}
}
}
#[cfg(feature = "std")]
impl std::error::Error for CoupledFieldError {}
#[derive(Clone, Debug)]
pub struct CoupledField {
data: Vec<Fix128>,
nx: usize,
ny: usize,
nz: usize,
min: (Fix128, Fix128, Fix128),
max: (Fix128, Fix128, Fix128),
cell: (Fix128, Fix128, Fix128),
scratch: Vec<Fix128>,
}
impl CoupledField {
pub fn try_new(
nx: usize,
ny: usize,
nz: usize,
min: (Fix128, Fix128, Fix128),
max: (Fix128, Fix128, Fix128),
) -> Result<Self, CoupledFieldError> {
Self::try_new_filled(nx, ny, nz, min, max, Fix128::ZERO)
}
pub fn try_new_filled(
nx: usize,
ny: usize,
nz: usize,
min: (Fix128, Fix128, Fix128),
max: (Fix128, Fix128, Fix128),
value: Fix128,
) -> Result<Self, CoupledFieldError> {
if nx == 0 || ny == 0 || nz == 0 {
return Err(CoupledFieldError::EmptyGrid {
resolution: (nx, ny, nz),
});
}
let span = |lo: Fix128, hi: Fix128, n: usize| -> Fix128 {
if n > 1 && hi > lo {
(hi - lo) / Fix128::from_int((n - 1) as i64)
} else {
Fix128::ONE
}
};
let cell = (
span(min.0, max.0, nx),
span(min.1, max.1, ny),
span(min.2, max.2, nz),
);
let n = nx * ny * nz;
Ok(Self {
data: vec![value; n],
nx,
ny,
nz,
min,
max,
cell,
scratch: vec![Fix128::ZERO; n],
})
}
#[cfg(feature = "std")]
pub fn try_matching(src: &ScalarField3D) -> Result<Self, CoupledFieldError> {
Self::try_new(
src.nx,
src.ny,
src.nz,
(
Fix128::from_f32(src.min.0),
Fix128::from_f32(src.min.1),
Fix128::from_f32(src.min.2),
),
(
Fix128::from_f32(src.max.0),
Fix128::from_f32(src.max.1),
Fix128::from_f32(src.max.2),
),
)
}
#[inline]
#[must_use]
pub const fn nx(&self) -> usize {
self.nx
}
#[inline]
#[must_use]
pub const fn ny(&self) -> usize {
self.ny
}
#[inline]
#[must_use]
pub const fn nz(&self) -> usize {
self.nz
}
#[inline]
#[must_use]
pub const fn cell_count(&self) -> usize {
self.nx * self.ny * self.nz
}
#[inline]
#[must_use]
pub const fn min(&self) -> (Fix128, Fix128, Fix128) {
self.min
}
#[inline]
#[must_use]
pub const fn max(&self) -> (Fix128, Fix128, Fix128) {
self.max
}
#[inline]
#[must_use]
pub const fn cell_size(&self) -> (Fix128, Fix128, Fix128) {
self.cell
}
#[inline]
#[must_use]
pub fn as_slice(&self) -> &[Fix128] {
&self.data
}
#[inline]
pub fn as_mut_slice(&mut self) -> &mut [Fix128] {
&mut self.data
}
#[inline]
#[must_use]
pub const fn index(&self, ix: usize, iy: usize, iz: usize) -> usize {
iz * self.ny * self.nx + iy * self.nx + ix
}
#[inline]
#[must_use]
pub fn get(&self, ix: usize, iy: usize, iz: usize) -> Fix128 {
let ix = ix.min(self.nx - 1);
let iy = iy.min(self.ny - 1);
let iz = iz.min(self.nz - 1);
self.data[self.index(ix, iy, iz)]
}
#[inline]
pub fn set(&mut self, ix: usize, iy: usize, iz: usize, value: Fix128) {
if ix < self.nx && iy < self.ny && iz < self.nz {
let idx = self.index(ix, iy, iz);
self.data[idx] = value;
}
}
#[inline]
pub fn add(&mut self, ix: usize, iy: usize, iz: usize, delta: Fix128) {
if ix < self.nx && iy < self.ny && iz < self.nz {
let idx = self.index(ix, iy, iz);
self.data[idx] = self.data[idx] + delta;
}
}
#[inline]
#[must_use]
pub fn contains(&self, p: Vec3Fix) -> bool {
p.x >= self.min.0
&& p.x <= self.max.0
&& p.y >= self.min.1
&& p.y <= self.max.1
&& p.z >= self.min.2
&& p.z <= self.max.2
}
#[inline]
fn world_to_grid(&self, p: Vec3Fix) -> (Fix128, Fix128, Fix128) {
(
(p.x - self.min.0) / self.cell.0,
(p.y - self.min.1) / self.cell.1,
(p.z - self.min.2) / self.cell.2,
)
}
#[inline]
fn locate(g: Fix128, n: usize) -> (usize, usize, Fix128) {
let top = Fix128::from_int((n - 1) as i64);
let g = if g.is_negative() {
Fix128::ZERO
} else if g > top {
top
} else {
g
};
let floor = g.floor();
let i0 = (floor.hi as usize).min(n - 1);
let i1 = (i0 + 1).min(n - 1);
(i0, i1, g - floor)
}
#[inline]
fn cell_of(&self, p: Vec3Fix) -> ([usize; 8], Fix128, Fix128, Fix128) {
let (gx, gy, gz) = self.world_to_grid(p);
let (ix0, ix1, fx) = Self::locate(gx, self.nx);
let (iy0, iy1, fy) = Self::locate(gy, self.ny);
let (iz0, iz1, fz) = Self::locate(gz, self.nz);
let nx = self.nx;
let ny_nx = self.ny * nx;
let corners = [
iz0 * ny_nx + iy0 * nx + ix0, iz0 * ny_nx + iy0 * nx + ix1, iz0 * ny_nx + iy1 * nx + ix0, iz0 * ny_nx + iy1 * nx + ix1, iz1 * ny_nx + iy0 * nx + ix0, iz1 * ny_nx + iy0 * nx + ix1, iz1 * ny_nx + iy1 * nx + ix0, iz1 * ny_nx + iy1 * nx + ix1, ];
(corners, fx, fy, fz)
}
#[must_use]
pub fn sample(&self, p: Vec3Fix) -> Fix128 {
let (c, fx, fy, fz) = self.cell_of(p);
let d = &self.data;
let c00 = d[c[0]] + (d[c[1]] - d[c[0]]) * fx;
let c10 = d[c[2]] + (d[c[3]] - d[c[2]]) * fx;
let c01 = d[c[4]] + (d[c[5]] - d[c[4]]) * fx;
let c11 = d[c[6]] + (d[c[7]] - d[c[6]]) * fx;
let c0 = c00 + (c10 - c00) * fy;
let c1 = c01 + (c11 - c01) * fy;
c0 + (c1 - c0) * fz
}
#[must_use]
pub fn gradient(&self, p: Vec3Fix) -> Vec3Fix {
let two = Fix128::from_int(2);
let ex = self.cell.0 / two;
let ey = self.cell.1 / two;
let ez = self.cell.2 / two;
let dx = (self.sample(Vec3Fix::new(p.x + ex, p.y, p.z))
- self.sample(Vec3Fix::new(p.x - ex, p.y, p.z)))
/ self.cell.0;
let dy = (self.sample(Vec3Fix::new(p.x, p.y + ey, p.z))
- self.sample(Vec3Fix::new(p.x, p.y - ey, p.z)))
/ self.cell.1;
let dz = (self.sample(Vec3Fix::new(p.x, p.y, p.z + ez))
- self.sample(Vec3Fix::new(p.x, p.y, p.z - ez)))
/ self.cell.2;
Vec3Fix::new(dx, dy, dz)
}
pub fn splat(&mut self, p: Vec3Fix, value: Fix128) {
let (c, fx, fy, fz) = self.cell_of(p);
let one = Fix128::ONE;
let (gx, gy, gz) = (one - fx, one - fy, one - fz);
let w = [
gx * gy * gz, fx * gy * gz, gx * fy * gz, fx * fy * gz, gx * gy * fz, fx * gy * fz, gx * fy * fz, fx * fy * fz, ];
for (idx, weight) in c.iter().zip(w.iter()) {
self.data[*idx] = self.data[*idx] + value * *weight;
}
}
pub fn diffuse(&mut self, dt: Fix128, rate: Fix128) {
let two = Fix128::from_int(2);
let hx2 = self.cell.0 * self.cell.0;
let hy2 = self.cell.1 * self.cell.1;
let hz2 = self.cell.2 * self.cell.2;
let rate_dt = rate * dt;
let (nx, ny, nz) = (self.nx, self.ny, self.nz);
let ny_nx = ny * nx;
for iz in 0..nz {
let iz_off = iz * ny_nx;
for iy in 0..ny {
let iy_off = iz_off + iy * nx;
for ix in 0..nx {
let idx = iy_off + ix;
let c = self.data[idx];
let two_c = two * c;
let mut lap = Fix128::ZERO;
if nx > 1 {
let xm = if ix > 0 {
self.data[idx - 1]
} else {
self.data[idx + 1]
};
let xp = if ix + 1 < nx {
self.data[idx + 1]
} else {
self.data[idx - 1]
};
lap = lap + (xm + xp - two_c) / hx2;
}
if ny > 1 {
let ym = if iy > 0 {
self.data[idx - nx]
} else {
self.data[idx + nx]
};
let yp = if iy + 1 < ny {
self.data[idx + nx]
} else {
self.data[idx - nx]
};
lap = lap + (ym + yp - two_c) / hy2;
}
if nz > 1 {
let zm = if iz > 0 {
self.data[idx - ny_nx]
} else {
self.data[idx + ny_nx]
};
let zp = if iz + 1 < nz {
self.data[idx + ny_nx]
} else {
self.data[idx - ny_nx]
};
lap = lap + (zm + zp - two_c) / hz2;
}
self.scratch[idx] = c + rate_dt * lap;
}
}
}
core::mem::swap(&mut self.data, &mut self.scratch);
}
pub fn decay_toward(&mut self, target: Fix128, rate: Fix128, dt: Fix128) {
let factor = (-(rate * dt)).exp();
for v in &mut self.data {
*v = (*v - target) * factor + target;
}
}
pub fn decay(&mut self, rate: Fix128, dt: Fix128) {
self.decay_toward(Fix128::ZERO, rate, dt);
}
pub fn clamp(&mut self, lo: Fix128, hi: Fix128) {
for v in &mut self.data {
if *v < lo {
*v = lo;
} else if *v > hi {
*v = hi;
}
}
}
pub fn fill(&mut self, value: Fix128) {
self.data.fill(value);
}
pub fn clear(&mut self) {
self.fill(Fix128::ZERO);
}
#[must_use]
pub fn sum(&self) -> Fix128 {
self.data.iter().fold(Fix128::ZERO, |acc, v| acc + *v)
}
#[must_use]
pub fn max_value(&self) -> Fix128 {
self.data
.iter()
.copied()
.reduce(|a, b| if b > a { b } else { a })
.unwrap_or(Fix128::ZERO)
}
#[must_use]
pub fn same_grid_as(&self, other: &Self) -> bool {
self.nx == other.nx
&& self.ny == other.ny
&& self.nz == other.nz
&& self.min == other.min
&& self.max == other.max
}
fn require_same_grid(&self, other: &Self) -> Result<(), CoupledFieldError> {
if self.nx != other.nx || self.ny != other.ny || self.nz != other.nz {
return Err(CoupledFieldError::ResolutionMismatch {
channel: (self.nx, self.ny, self.nz),
other: (other.nx, other.ny, other.nz),
});
}
if self.min != other.min || self.max != other.max {
return Err(CoupledFieldError::BoundsMismatch);
}
Ok(())
}
#[cfg(feature = "std")]
pub(crate) fn require_same_grid_f32(
&self,
other: &ScalarField3D,
) -> Result<(), CoupledFieldError> {
if self.nx != other.nx || self.ny != other.ny || self.nz != other.nz {
return Err(CoupledFieldError::ResolutionMismatch {
channel: (self.nx, self.ny, self.nz),
other: (other.nx, other.ny, other.nz),
});
}
let min = (
Fix128::from_f32(other.min.0),
Fix128::from_f32(other.min.1),
Fix128::from_f32(other.min.2),
);
let max = (
Fix128::from_f32(other.max.0),
Fix128::from_f32(other.max.1),
Fix128::from_f32(other.max.2),
);
if self.min != min || self.max != max {
return Err(CoupledFieldError::BoundsMismatch);
}
Ok(())
}
pub fn add_assign(&mut self, other: &Self) -> Result<(), CoupledFieldError> {
self.require_same_grid(other)?;
for (dst, src) in self.data.iter_mut().zip(other.data.iter()) {
*dst = *dst + *src;
}
Ok(())
}
pub fn scale_div(&mut self, divisor: Fix128) {
if divisor.is_zero() {
return;
}
for v in &mut self.data {
*v = *v / divisor;
}
}
pub fn blend_from(&mut self, other: &Self, weight: Fix128) -> Result<(), CoupledFieldError> {
self.require_same_grid(other)?;
for (dst, src) in self.data.iter_mut().zip(other.data.iter()) {
*dst = *dst + (*src - *dst) * weight;
}
Ok(())
}
#[cfg(feature = "std")]
pub fn copy_from_f32(&mut self, src: &ScalarField3D) -> Result<(), CoupledFieldError> {
self.require_same_grid_f32(src)?;
for (dst, v) in self.data.iter_mut().zip(src.data.iter()) {
*dst = Fix128::from_f32(*v);
}
Ok(())
}
#[cfg(feature = "std")]
pub fn write_to_f32(&self, dst: &mut ScalarField3D) -> Result<(), CoupledFieldError> {
self.require_same_grid_f32(dst)?;
for (v, cell) in dst.data.iter_mut().zip(self.data.iter()) {
*v = cell.to_f32();
}
Ok(())
}
}
#[derive(Debug)]
pub struct TemperatureRise {
#[cfg_attr(not(feature = "std"), allow(dead_code))]
field: CoupledField,
}
impl TemperatureRise {
#[must_use]
pub fn from_absolute(absolute: &CoupledField, reference: Fix128) -> Self {
let mut field = absolute.clone();
for cell in field.as_mut_slice() {
*cell = *cell - reference;
}
Self { field }
}
#[cfg(feature = "std")]
pub(crate) const fn field(&self) -> &CoupledField {
&self.field
}
}
pub trait CoupledScalar {
fn coupled_name(&self) -> &'static str;
fn coupled_channel(&self) -> Result<CoupledField, CoupledFieldError>;
fn publish(&self, out: &mut CoupledField) -> Result<(), CoupledFieldError>;
fn adopt(&mut self, src: &CoupledField) -> Result<(), CoupledFieldError>;
fn check_adopt(&self, src: &CoupledField) -> Result<(), CoupledFieldError> {
src.require_same_grid(&self.coupled_channel()?)
}
}
fn adopt_all(
participants: &mut [&mut dyn CoupledScalar],
channel: &CoupledField,
) -> Result<(), CoupledFieldError> {
for p in participants.iter() {
p.check_adopt(channel)?;
}
for p in participants.iter_mut() {
p.adopt(channel)?;
}
Ok(())
}
pub fn reconcile_mean(
participants: &mut [&mut dyn CoupledScalar],
channel: &mut CoupledField,
) -> Result<(), CoupledFieldError> {
let Some(first) = participants.first() else {
return Err(CoupledFieldError::NoParticipants);
};
let mut staging = first.coupled_channel()?;
channel.require_same_grid(&staging)?;
channel.clear();
for p in participants.iter() {
p.publish(&mut staging)?;
channel.add_assign(&staging)?;
}
channel.scale_div(Fix128::from_int(participants.len() as i64));
adopt_all(participants, channel)
}
pub fn reconcile_weighted(
participants: &mut [&mut dyn CoupledScalar],
weights: &[u32],
channel: &mut CoupledField,
) -> Result<(), CoupledFieldError> {
let Some(first) = participants.first() else {
return Err(CoupledFieldError::NoParticipants);
};
if weights.len() != participants.len() || weights.iter().all(|&w| w == 0) {
return Err(CoupledFieldError::NoParticipants);
}
let mut staging = first.coupled_channel()?;
if !channel.same_grid_as(&staging) {
return channel.require_same_grid(&staging);
}
channel.clear();
let mut total: i64 = 0;
for (p, &w) in participants.iter().zip(weights) {
p.publish(&mut staging)?;
if w == 0 {
continue;
}
let weight = Fix128::from_int(i64::from(w));
for cell in staging.as_mut_slice() {
*cell = *cell * weight;
}
total += i64::from(w);
channel.add_assign(&staging)?;
}
channel.scale_div(Fix128::from_int(total));
adopt_all(participants, channel)
}
#[cfg(test)]
mod tests {
use super::*;
fn v(x: i64, y: i64, z: i64) -> Vec3Fix {
Vec3Fix::from_int(x, y, z)
}
fn unit_grid() -> CoupledField {
CoupledField::try_new(
5,
5,
5,
(Fix128::ZERO, Fix128::ZERO, Fix128::ZERO),
(
Fix128::from_int(4),
Fix128::from_int(4),
Fix128::from_int(4),
),
)
.expect("non-degenerate grid")
}
#[test]
fn empty_grid_is_rejected() {
let err = CoupledField::try_new(
0,
2,
2,
(Fix128::ZERO, Fix128::ZERO, Fix128::ZERO),
(Fix128::ONE, Fix128::ONE, Fix128::ONE),
)
.expect_err("a zero dimension must be refused");
assert_eq!(
err,
CoupledFieldError::EmptyGrid {
resolution: (0, 2, 2)
}
);
}
#[test]
fn cell_size_is_the_node_spacing() {
let f = unit_grid();
assert_eq!(f.cell_size().0, Fix128::ONE);
assert_eq!(f.cell_count(), 125);
}
#[test]
fn get_and_set_round_trip() {
let mut f = unit_grid();
f.set(1, 2, 3, Fix128::from_int(7));
assert_eq!(f.get(1, 2, 3), Fix128::from_int(7));
assert_eq!(f.get(0, 0, 0), Fix128::ZERO);
}
#[test]
fn sample_at_a_node_returns_that_node() {
let mut f = unit_grid();
f.set(2, 1, 3, Fix128::from_int(9));
assert_eq!(f.sample(v(2, 1, 3)), Fix128::from_int(9));
}
#[test]
fn grid_mismatch_is_reported_not_silently_resampled() {
let mut a = unit_grid();
let b = CoupledField::try_new(
4,
5,
5,
(Fix128::ZERO, Fix128::ZERO, Fix128::ZERO),
(
Fix128::from_int(4),
Fix128::from_int(4),
Fix128::from_int(4),
),
)
.expect("non-degenerate grid");
assert_eq!(
a.add_assign(&b),
Err(CoupledFieldError::ResolutionMismatch {
channel: (5, 5, 5),
other: (4, 5, 5),
})
);
}
#[test]
fn reconcile_without_participants_is_an_error() {
let mut channel = unit_grid();
let mut none: [&mut dyn CoupledScalar; 0] = [];
assert_eq!(
reconcile_mean(&mut none, &mut channel),
Err(CoupledFieldError::NoParticipants)
);
}
#[test]
fn sum_is_exact_over_many_cells() {
let mut f = unit_grid();
let q = Fix128::from_ratio(1, 4);
for cell in f.as_mut_slice() {
*cell = q;
}
assert_eq!(f.sum(), Fix128::from_ratio(125, 4));
}
}