api/simple-mat: drop pointless, wrong fmod

Also use fabs instead of abs.

1 files changed, 9 insertions, 4 deletions

diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp
index 4f885f4f..9794d2d1 100644
--- a/opentrack/simple-mat.hpp
+++ b/opentrack/simple-mat.hpp
@@ -7,7 +7,6 @@
  */
 
 #pragma once
-#include <algorithm>
 #include <initializer_list>
 #include <type_traits>
 #include <cmath>
@@ -250,15 +249,21 @@ public:
         const double roll_2 = atan2(R(1, 2) / cos_p2, R(2, 2) / cos_p2);
         const double yaw_1 = atan2(R(0, 1) / cos_p1, R(0, 0) / cos_p1);
         const double yaw_2 = atan2(R(0, 1) / cos_p2, R(0, 0) / cos_p2);
-        if (std::abs(pitch_1) + std::abs(roll_1) + std::abs(yaw_1) > std::abs(pitch_2) + std::abs(roll_2) + std::abs(yaw_2))
+
+        using std::fabs;
+
+        const double err1 = fabs(pitch_1) + fabs(roll_1) + fabs(yaw_1);
+        const double err2 = fabs(pitch_2) + fabs(roll_2) + fabs(yaw_2);
+
+        if (err1 > err2)
         {
             bool fix_neg_pitch = pitch_1 < 0;
-            return dmat<3, 1>(yaw_2, std::fmod(fix_neg_pitch ? -pi - pitch_1 : pitch_2, pi), roll_2);
+            return dmat<3, 1>(yaw_2, fix_neg_pitch ? -pi - pitch_1 : pitch_2, roll_2);
         }
         else
             return dmat<3, 1>(yaw_1, pitch_1, roll_1);
     }
-    
+
     // tait-bryan angles, not euler
     static dmat<3, 3> euler_to_rmat(const double* input)
     {