Nalg support

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]]

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"

src/system/spaces/mod.rs

@@ -7,3 +7,4 @@ pub mod hexagonal;
 
 pub mod continuous_3d;
 pub mod continuous_2d;
+pub mod nalg;

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<const D: usize>(SVector<i32, D>);
+
+#[derive(Clone, PartialEq, Serialize, Deserialize)]
+#[serde(transparent)]
+pub struct Continuous<const D: usize>(SVector<f32, D>);
+
+impl<const D: usize> Add for Continuous<D> {
+    type Output = Continuous<D>;
+
+    fn add(self, rhs: Self) -> Self::Output {
+        Continuous(self.0 + rhs.0)
+    }
+}
+
+impl<const D: usize> Position for Continuous<D> {
+    const DIM: usize = 0;
+
+    fn zero() -> Self {
+        Continuous(SVector::<f32, D>::zeros())
+    }
+
+    fn abs(&self) -> f32 {
+        self.0.norm()
+    }
+
+    fn from_cartesian(cartesian: &[f32]) -> Self {
+        Continuous(SVector::<f32, D>::from_fn(|i, _| cartesian[i]))
+    }
+
+    fn to_cartesian(&self) -> Vec<f32> {
+        self.0.as_slice().to_vec()
+    }
+}
+
+impl<const D: usize> Add for Gridded<D> {
+    type Output = Gridded<D>;
+
+    fn add(self, rhs: Self) -> Self::Output {
+        Gridded(self.0 + rhs.0)
+    }
+}
+
+impl<const D: usize> Position for Gridded<D> {
+    const DIM: usize = 0;
+
+    fn zero() -> Self {
+        Gridded(SVector::<i32, D>::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::<i32, D>::from_fn(|i, _| cartesian[i] as i32))
+    }
+
+    fn to_cartesian(&self) -> Vec<f32> {
+        self.0.as_slice()
+            .iter()
+            .map(|a| *a as f32)
+            .collect_vec()
+    }
+}
+
+pub struct KDSpace<const N: usize>(pub(crate) kiddo::KdTree<f32, (), N>);
+
+impl<const D: usize> Storage<Gridded<D>> for KDSpace<D> {
+    fn is_occupied(&self, position: &Gridded<D>) -> 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::<f32, D>::from_row_slice(a),
+                    &SVector::<f32, D>::from_row_slice(b),
+                )
+            },
+        ).unwrap();
+
+        !a.is_empty()
+    }
+
+    fn deposit(&mut self, position: &Gridded<D>) {
+        self.0.add(&position.0.data.0[0].map(|i| i as f32), ())
+            .expect("Failed to write to space")
+    }
+}
+
+impl<const D: usize> Storage<Continuous<D>> for KDSpace<D> {
+    fn is_occupied(&self, position: &Continuous<D>) -> bool {
+        let a = self.0.best_n_within(
+            &position.0.data.0[0],
+            0f32,
+            1,
+            &|a, b| {
+                EuclideanNorm.metric_distance(
+                    &SVector::<f32, D>::from_row_slice(a),
+                    &SVector::<f32, D>::from_row_slice(b),
+                )
+            },
+        ).unwrap();
+
+        !a.is_empty()
+    }
+
+    fn deposit(&mut self, position: &Continuous<D>) {
+        self.0.add(&position.0.data.0[0], ())
+            .expect("Failed to write to space")
+    }
+}
+
+pub struct VectorStorage {
+    backing: Vec<bool>,
+    grid_size: usize,
+}
+
+impl<const D: usize> Storage<Gridded<D>> for VectorStorage {
+    fn is_occupied(&self, position: &Gridded<D>) -> 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<D>) {
+        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;
+    }
+}