Mercurial > repos > alg_tools / changeset

--- a/src/loc.rs	Fri Nov 22 16:38:36 2024 -0500
+++ b/src/loc.rs	Fri Dec 06 13:06:22 2024 -0500
@@ -480,9 +480,94 @@
 }
 
 impl<F : Num, const N : usize> Loc<F, N> {
+    /// Origin point
     pub const ORIGIN : Self = Loc([F::ZERO; N]);
 }
 
+impl<F : Num + std::ops::Neg<Output=F>, const N : usize> Loc<F, N> {
+    /// Reflects along the given coordinate
+    pub fn reflect(mut self, i : usize) -> Self {
+        self[i] = -self[i];
+        self
+    }
+
+    /// Reflects along the given coordinate sequences
+    pub fn reflect_many<I : IntoIterator<Item=usize>>(mut self, idxs : I) -> Self {
+        for i in idxs {
+            self[i]=-self[i]
+        }
+        self
+    }
+}
+
+impl<F : std::ops::Neg<Output=F>> Loc<F, 2> {
+    /// Reflect x coordinate
+    pub fn reflect_x(self) -> Self {
+        let Loc([x, y]) = self;
+        [-x, y].into()
+    }
+
+    /// Reflect y coordinate
+    pub fn reflect_y(self) -> Self {
+        let Loc([x, y]) = self;
+        [x, -y].into()
+    }
+}
+
+impl<F : Float> Loc<F, 2> {
+    /// Rotate by angle φ
+    pub fn rotate(self, φ : F) -> Self {
+        let Loc([x, y]) = self;
+        let sin_φ = φ.sin();
+        let cos_φ = φ.cos();
+        [cos_φ * x - sin_φ * y,
+         sin_φ * x + cos_φ * y].into()
+    }
+}
+
+impl<F : std::ops::Neg<Output=F>> Loc<F, 3> {
+    /// Reflect x coordinate
+    pub fn reflect_x(self) -> Self {
+        let Loc([x, y, z]) = self;
+        [-x, y, z].into()
+    }
+
+    /// Reflect y coordinate
+    pub fn reflect_y(self) -> Self {
+        let Loc([x, y, z]) = self;
+        [x, -y, z].into()
+    }
+
+    /// Reflect y coordinate
+    pub fn reflect_z(self) -> Self {
+        let Loc([x, y, z]) = self;
+        [x, y, -z].into()
+    }
+}
+
+impl<F : Float> Loc<F, 3> {
+    /// Rotate by angles (yaw, pitch, roll)
+    pub fn rotate(self, Loc([φ, ψ, θ]) : Self) -> Self {
+        let Loc([mut x, mut y, mut z]) = self;
+        let sin_φ = φ.sin();
+        let cos_φ = φ.cos();
+        [x, y, z] = [cos_φ * x - sin_φ *y,
+                     sin_φ * x + cos_φ * y,
+                     z];
+        let sin_ψ = ψ.sin();
+        let cos_ψ = ψ.cos();
+        [x, y, z] = [cos_ψ * x + sin_ψ * z,
+                     y,
+                     -sin_ψ * x + cos_ψ * z];
+        let sin_θ = θ.sin();
+        let cos_θ = θ.cos();
+        [x, y, z] = [x,
+                     cos_θ * y - sin_θ * z,
+                     sin_θ * y + cos_θ * z];
+        [x, y, z].into()
+    }
+}
+
 impl<F : Float,const N : usize> StaticEuclidean<F> for Loc<F, N> {
     #[inline]
     fn origin() -> Self {