Merge pull request #99 from staszewski/checked-vector-normalization

Author: Colin1860

CHANGELOG.md

@@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+- Add `Vector::try_normalized` ([#99](https://github.com/helsing-ai/sguaba/pull/99)).
 ### Changed
 
 ### Deprecated

src/vectors.rs

@@ -751,6 +751,16 @@ where
         Self::from_nalgebra_vector(self.inner.normalize())
     }
 
+    /// Returns a unit vector with the same direction as this vector.
+    ///
+    /// Returns `None` if this vector's magnitude is less than or equal to `min_norm`.
+    #[must_use]
+    pub fn try_normalized(&self, min_norm: f64) -> Option<Self> {
+        self.inner
+            .try_normalize(min_norm)
+            .map(Self::from_nalgebra_vector)
+    }
+
     /// Computes the dot (scalar) product between this vector and another.
     ///
     /// Note that this method's return value is unitless since the unit of the dot product is not
@@ -1392,6 +1402,29 @@ mod tests {
             assert_eq!(v.to_cartesian(), [m(0.0), m(0.0), m(0.0)]);
         }
 
+        #[test]
+        fn try_normalized_zero_vector_returns_none() {
+            let v = Vector::<TestFrd>::zero();
+
+            assert_eq!(v.try_normalized(0.0), None);
+        }
+
+        #[test]
+        fn try_normalized_nonzero_vector_returns_unit_vector() {
+            let v = vector!(f = m(3.0), r = m(0.0), d = m(4.0); in TestFrd);
+
+            let normalized = v.try_normalized(0.0).expect("non-zero vector");
+
+            assert_abs_diff_eq!(normalized.magnitude().get::<meter>(), 1.0);
+        }
+
+        #[test]
+        fn try_normalized_returns_none_below_min_norm() {
+            let v = vector!(f = m(3.0), r = m(0.0), d = m(4.0); in TestFrd); // magnitude is 5, but we set min_norm to 6
+
+            assert_eq!(v.try_normalized(6.0), None);
+        }
+
         #[test]
         fn lerp_works() {
             let v1 = vector!(f = m(0.0), r = m(0.0), d = m(0.0); in TestFrd);