diff --git a/Cargo.lock b/Cargo.lock index dde569d..b658331 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2984,6 +2984,7 @@ dependencies = [ "num-complex", "num-rational", "num-traits", + "serde", "simba 0.8.0", "typenum", ] @@ -3160,6 +3161,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "02e0d21255c828d6f128a1e41534206671e8c3ea0c62f32291e808dc82cff17d" dependencies = [ "num-traits", + "serde", ] [[package]] diff --git a/Cargo.toml b/Cargo.toml index 08b93a7..5fe05ea 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -44,7 +44,7 @@ csv = "1.1" serde = { version = "1.0.152", features = ["derive"] } serde_json = "1.0.93" kd-tree = { version = "0.5.1", features = ["nalgebra"] } -nalgebra = "0.32.2" +nalgebra = { version = "0.32.2", features = ["serde-serialize"] } kiddo = "0.2.5" anyhow = "1.0.69" itertools = "0.10.5" diff --git a/src/system/spaces/mod.rs b/src/system/spaces/mod.rs index 1c34f9a..33007ab 100644 --- a/src/system/spaces/mod.rs +++ b/src/system/spaces/mod.rs @@ -7,3 +7,4 @@ pub mod hexagonal; pub mod continuous_3d; pub mod continuous_2d; +pub mod nalg; diff --git a/src/system/spaces/nalg.rs b/src/system/spaces/nalg.rs new file mode 100644 index 0000000..411c372 --- /dev/null +++ b/src/system/spaces/nalg.rs @@ -0,0 +1,148 @@ +use std::ops::Add; +use itertools::Itertools; +use nalgebra::{EuclideanNorm, LpNorm, Matrix, Norm, OMatrix, SVector}; +use num_traits::Pow; +use serde::{Serialize, Deserialize}; +use crate::system::{GriddedPosition, Position, Storage}; + +#[derive(Clone, PartialEq, Eq, Serialize, Deserialize)] +#[serde(transparent)] +pub struct Gridded(SVector); + +#[derive(Clone, PartialEq, Serialize, Deserialize)] +#[serde(transparent)] +pub struct Continuous(SVector); + +impl Add for Continuous { + type Output = Continuous; + + fn add(self, rhs: Self) -> Self::Output { + Continuous(self.0 + rhs.0) + } +} + +impl Position for Continuous { + const DIM: usize = 0; + + fn zero() -> Self { + Continuous(SVector::::zeros()) + } + + fn abs(&self) -> f32 { + self.0.norm() + } + + fn from_cartesian(cartesian: &[f32]) -> Self { + Continuous(SVector::::from_fn(|i, _| cartesian[i])) + } + + fn to_cartesian(&self) -> Vec { + self.0.as_slice().to_vec() + } +} + +impl Add for Gridded { + type Output = Gridded; + + fn add(self, rhs: Self) -> Self::Output { + Gridded(self.0 + rhs.0) + } +} + +impl Position for Gridded { + const DIM: usize = 0; + + fn zero() -> Self { + Gridded(SVector::::zeros()) + } + + fn abs(&self) -> f32 { + (self.0.fold(0, |r, c| r + c.pow(2)) as f32).sqrt() + } + + fn from_cartesian(cartesian: &[f32]) -> Self { + Gridded(SVector::::from_fn(|i, _| cartesian[i] as i32)) + } + + fn to_cartesian(&self) -> Vec { + self.0.as_slice() + .iter() + .map(|a| *a as f32) + .collect_vec() + } +} + +pub struct KDSpace(pub(crate) kiddo::KdTree); + +impl Storage> for KDSpace { + fn is_occupied(&self, position: &Gridded) -> bool { + let a = self.0.best_n_within( + &position.0.data.0[0].map(|i| i as f32), + 0f32, + 1, + &|a, b| { + LpNorm(1).metric_distance( + &SVector::::from_row_slice(a), + &SVector::::from_row_slice(b), + ) + }, + ).unwrap(); + + !a.is_empty() + } + + fn deposit(&mut self, position: &Gridded) { + self.0.add(&position.0.data.0[0].map(|i| i as f32), ()) + .expect("Failed to write to space") + } +} + +impl Storage> for KDSpace { + fn is_occupied(&self, position: &Continuous) -> bool { + let a = self.0.best_n_within( + &position.0.data.0[0], + 0f32, + 1, + &|a, b| { + EuclideanNorm.metric_distance( + &SVector::::from_row_slice(a), + &SVector::::from_row_slice(b), + ) + }, + ).unwrap(); + + !a.is_empty() + } + + fn deposit(&mut self, position: &Continuous) { + self.0.add(&position.0.data.0[0], ()) + .expect("Failed to write to space") + } +} + +pub struct VectorStorage { + backing: Vec, + grid_size: usize, +} + +impl Storage> for VectorStorage { + fn is_occupied(&self, position: &Gridded) -> bool { + let mut index: usize = 0; + + for i in 0..D { + index += (position.0[i] + (self.grid_size as i32 / 2)) as usize * self.grid_size * i; + } + + return self.backing[index]; + } + + fn deposit(&mut self, position: &Gridded) { + let mut index: usize = 0; + + for i in 0..D { + index += (position.0[i] + (self.grid_size as i32 / 2)) as usize * self.grid_size * i; + } + + self.backing[index] = true; + } +}