From 9d930eb21a22ee6628d21b5035458c9909a09a02 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= <developer@crozet.re>
Date: Mon, 22 Feb 2021 14:26:40 +0100
Subject: [PATCH] Add a method to cap the magnitude of a vector.

---
 src/base/norm.rs | 20 ++++++++++++++++++--
 1 file changed, 18 insertions(+), 2 deletions(-)

diff --git a/src/base/norm.rs b/src/base/norm.rs
index a7fa66e9..9717e031 100644
--- a/src/base/norm.rs
+++ b/src/base/norm.rs
@@ -8,7 +8,7 @@ use crate::allocator::Allocator;
 use crate::base::{DefaultAllocator, Dim, DimName, Matrix, MatrixMN, Normed, VectorN};
 use crate::constraint::{SameNumberOfColumns, SameNumberOfRows, ShapeConstraint};
 use crate::storage::{Storage, StorageMut};
-use crate::{ComplexField, Scalar, SimdComplexField, Unit};
+use crate::{ComplexField, RealField, Scalar, SimdComplexField, Unit};
 use simba::scalar::ClosedNeg;
 use simba::simd::{SimdOption, SimdPartialOrd};
 
@@ -334,11 +334,27 @@ impl<N: Scalar, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S> {
     {
         let n = self.norm();
 
-        if n >= min_magnitude {
+        if n > min_magnitude {
             self.scale_mut(magnitude / n)
         }
     }
 
+    /// Returns a new vector with the same magnitude as `self` clamped between `0.0` and `max`.
+    #[inline]
+    pub fn cap_magnitude(&self, max: N::RealField) -> MatrixMN<N, R, C>
+    where
+        N: RealField,
+        DefaultAllocator: Allocator<N, R, C>,
+    {
+        let n = self.norm();
+
+        if n > max {
+            self.scale(max / n)
+        } else {
+            self.clone_owned()
+        }
+    }
+
     /// Returns a normalized version of this matrix unless its norm as smaller or equal to `eps`.
     ///
     /// The components of this matrix cannot be SIMD types (see `simd_try_normalize`) instead.