From 22bf63cb714156eafd8a6d3822b903eba4980a8a Mon Sep 17 00:00:00 2001
From: Julian Kent <jkflying@gmail.com>
Date: Thu, 5 Sep 2019 17:34:29 +0200
Subject: [PATCH] A smaller codesize wrap, since it gets inlined in many places

---
 matrix/helper_functions.hpp | 24 +++++-------------------
 test/helper.cpp             |  1 -
 2 files changed, 5 insertions(+), 20 deletions(-)

diff --git a/matrix/helper_functions.hpp b/matrix/helper_functions.hpp
index 029272ce29..88fc56ab8f 100644
--- a/matrix/helper_functions.hpp
+++ b/matrix/helper_functions.hpp
@@ -26,7 +26,7 @@ bool is_finite(Type x) {
  * @param x input possibly outside of the range
  * @param low lower limit of the allowed range
  * @param high upper limit of the allowed range
- * @return wrapped value inside the range, or NAN if value is too far away from range.
+ * @return wrapped value inside the range
  */
 template<typename Type>
 Type wrap(Type x, Type low, Type high) {
@@ -35,24 +35,10 @@ Type wrap(Type x, Type low, Type high) {
         return x;
     }
 
-    // close to range
-    Type range = high - low;
-    if ((high <= x) && (x < high + (range*100))) {
-        while (high <= x) {
-            x -= range;
-        }
-        return x;
-    }
-
-    if ((low - (range*100) <= x) && (x < low)) {
-        while (x < low) {
-            x += range;
-        }
-        return x;
-    }
-
-    // very far from the range -> something went terribly wrong
-    return NAN;
+    const Type range = high - low;
+    const Type inv_range = Type(1) / range; // should evaluate at compile time, multiplies below at runtime
+    const Type num_wraps = floor((x - low) * inv_range);
+    return x - range * num_wraps;
 }
 
 /**
diff --git a/test/helper.cpp b/test/helper.cpp
index bfaa87a596..91935eaffd 100644
--- a/test/helper.cpp
+++ b/test/helper.cpp
@@ -20,7 +20,6 @@ int main()
     TEST(fabs(wrap(360., 0., 360.)) < FLT_EPSILON);
     TEST(fabs(wrap(360. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
     TEST(fabs(wrap(360. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
-    TEST(!is_finite(wrap(1000., 0., .01)));
 
     // wrap pi
     TEST(fabs(wrap_pi(0.)) < FLT_EPSILON);