hotfix: pi2pi efficient for jets

include/core/ceres_wrapper/wolf_jet.h

@@ -27,14 +27,25 @@
 
 #include "core/math/rotations.h"
 
-namespace wolf{
+namespace ceres{
+
+// // efficient implementation of pi2pi for jets (without while)
+// template<int N>
+// inline ceres::Jet<double, N> pi2pi(const ceres::Jet<double, N>& _angle)
+// {
+//     ceres::Jet<double, N> angle = _angle;
+//     angle.a = pi2pi(_angle.a);
+//     return angle;
+// }
+// }
+
+using std::remainder;
 
-// efficient implementation of pi2pi for jets (without while)
 template<int N>
-inline ceres::Jet<double, N> pi2pi(const ceres::Jet<double, N>& _angle)
+inline Jet<double, N> remainder(const Jet<double, N>& _x, long double _y)
 {
-    ceres::Jet<double, N> angle = _angle;
-    angle.a = pi2pi(_angle.a);
-    return angle;
+    Jet<double, N> res = _x;
+    res.a = remainder(_x.a, _y);
+    return res;
 }
 }
\ No newline at end of file

include/core/math/rotations.h

@@ -41,18 +41,23 @@ namespace wolf
  * @param angle
  * @return formatted angle
  */
-inline long double pi2pi(const long double& _angle)
-{
-    return std::remainder(_angle,2*M_PI);
-}
-inline double pi2pi(const double& _angle)
-{
-    return std::remainder(_angle,2*M_PI);
-}
-inline float pi2pi(const float& _angle)
-{
-    return std::remainder(_angle,2*M_PI);
-}
+template<typename T>
+inline T pi2pi(const T& _angle)
+{
+    return remainder(_angle,2*M_PI);
+}
+// inline long double pi2pi(const long double& _angle)
+// {
+//     return std::remainder(_angle,2*M_PI);
+// }
+// inline double pi2pi(const double& _angle)
+// {
+//     return std::remainder(_angle,2*M_PI);
+// }
+// inline float pi2pi(const float& _angle)
+// {
+//     return std::remainder(_angle,2*M_PI);
+// }
 
 /** \brief Conversion to radians
  *