fixed and simplified interpolation of borderless joints

MotionPlanning/CSpace/CSpace.cpp

@@ -630,13 +630,9 @@ namespace Saba
         {
             // borderless mode
             // we map 'start' and 'end' to [0,2pi] (temporarily)
-            float start = q1[dim] - robotJoints[dim]->getJointLimitLo();
-            float end = q2[dim] - robotJoints[dim]->getJointLimitLo();
+            float start = q1[dim];
+            float end = q2[dim];
             float res = interpolateRotational(start, end , step);
-            // using fmod is okay here, because 'start' and 'end' where mapped to [0,2pi] and therefore 'res' is also in [0,2pi]
-            res = (float)fmod((double)res, 2.0 * M_PI);
-            // map back to original interval
-            res = res + robotJoints[dim]->getJointLimitLo();
             return res;
         }
 
@@ -649,38 +645,27 @@ namespace Saba
 
     float CSpace::interpolateRotational(float a, float b, float step)
     {
-        //return (a + step * (b - a));
-
-        float angle;
-
-        if (fabs(a - b) < M_PI)
-        {
-            //std::cout << "interpolateRotational: If 1" << std::endl;
-            angle = interpolateLinear(a, b, step);
-        }
-        else if (a < b)
-        {
-            //std::cout << "interpolateRotational: If 2" << std::endl;
-            angle = a - step * (a + 2.0f * (float)M_PI - b);
-
-        }
-        else
+        auto fmod = [](float value, float boundLow, float boundHigh)
         {
-            //std::cout << "interpolateRotational: If 3" << std::endl;
-            angle = a + step * (b + 2.0f * (float)M_PI - a);
-        }
+            value = std::fmod(value - boundLow, boundHigh - boundLow) + boundLow;
+            if (value < boundLow)
+            {
+                value += boundHigh - boundLow;
+            }
+            return value;
+        };
 
-        if (angle < 0)
+        auto lerp = [](float a, float b, float f)
         {
-            angle += 2.0f * (float)M_PI;
-        }
+            return a * (1 - f) + b * f;
+        };
 
-        if (angle >= 2.0f * (float)M_PI)
+        auto angleLerp = [&](float a, float b, float f)
         {
-            angle -= 2.0f * (float)M_PI;
-        }
-
-        return angle;
+            b = fmod(b, a - M_PI, a + M_PI);
+            return lerp(a, b, f);
+        };
+        return angleLerp(a, b, step);
     }