Fix GetPosition's interpolation to properly handle the angular part. Fixes #331

PlayRho/Common/Position.hpp

@@ -125,6 +125,15 @@ bool IsValid(const d2::Position& value) noexcept
 
 namespace d2 {
 
+
+namespace {
+template<typename T>
+constexpr auto cfloor(T x) -> decltype(std::floor(x))
+{
+    return (int(x) == x) ? int(x) : (x >= 0.0) ? int(x) : int(x) - 1;
+}
+}
+
 /// Gets the position between two positions at a given unit interval.
 /// @param pos0 Position at unit interval value of 0.
 /// @param pos1 Position at unit interval value of 1.
@@ -145,12 +154,24 @@ PLAYRHO_CONSTEXPR inline Position GetPosition(const Position pos0, const Positio
     //   If pos0 == pos1 then return value should always be equal to pos0 too.
     //   But if Real is float, pos0 * (1 - beta) + pos1 * beta can fail this requirement.
     //   Meanwhile, pos0 + (pos1 - pos0) * beta always works.
-    
+
     // pos0 * (1 - beta) + pos1 * beta
     // pos0 - pos0 * beta + pos1 * beta
     // pos0 + (pos1 * beta - pos0 * beta)
     // pos0 + (pos1 - pos0) * beta
-    return pos0 + (pos1 - pos0) * beta;
+
+    //return pos0 + (pos1 - pos0) * beta;
+
+    // Note: we have to be doubleplus careful, because we can't just linear interpolate
+    //   angles in radians without normalizing them. Let's try using a classic normalizer
+    //   with the same formula as above.
+    const auto twoPi = Pi+Pi;
+    const auto da = pos1.angular - pos0.angular;
+    const auto na = pos0.angular + (da - twoPi * cfloor((da + Pi) / twoPi)) * beta;
+    return {
+	pos0.linear + (pos1.linear - pos0.linear) * beta,
+        na - twoPi * cfloor((na + Pi) / twoPi)
+    };
 }
 
 } // namespace d2

UnitTests/Sweep.cpp

@@ -56,6 +56,13 @@ TEST(Sweep, GetPosition) {
     EXPECT_EQ(pos1, GetPosition(sweep.pos0, sweep.pos1, 1));
 }
 
+TEST(Sweep, GetPositionAngleEdgeCase) {
+    const auto pos0 = Position{Length2{-0.4_m, +2.34_m}, 3.13_rad};
+    const auto pos1 = Position{Length2{+0.4_m, -2.34_m}, -3.13_rad};
+    Sweep sweep{pos0, pos1, Length2{}, Real(0.6)};
+    EXPECT_NEAR(3.1358_rad, GetPosition(sweep.pos0, sweep.pos1, 0.25).angular,0.0001);
+}
+
 TEST(Sweep, Advance) {
     const auto pos0 = Position{Length2{-0.4_m, +2.34_m}, 3.14_rad};
     const auto pos1 = Position{Length2{+0.4_m, -2.34_m}, -3.14_rad};
@@ -71,7 +78,7 @@ TEST(Sweep, Advance) {
     sweep.Advance0(Real{1}/Real{2});
     EXPECT_EQ(Real{1}/Real{2}, sweep.GetAlpha0());
     EXPECT_EQ(pos1, sweep.pos1);
-    EXPECT_EQ((Position{Length2{}, 0_deg}), sweep.pos0);
+    EXPECT_EQ((Position{Length2{}, -Pi}), sweep.pos0);
 
     sweep.Advance0(0);
     EXPECT_EQ(Real{0}, sweep.GetAlpha0());

UnitTests/TimeOfImpact.cpp

@@ -560,7 +560,7 @@ TEST(TimeOfImpact, RodCircleMissAt360)
     {
         EXPECT_EQ(output.state, TOIOutput::e_separated);
         EXPECT_NEAR(static_cast<double>(output.time), 1.0, 0.001);
-        EXPECT_EQ(output.stats.toi_iters, 4);
+        EXPECT_EQ(output.stats.toi_iters, 3);
     }
 }
 
@@ -602,7 +602,7 @@ TEST(TimeOfImpact, RodCircleHitAt180)
     else
     {
         EXPECT_EQ(output.state, TOIOutput::e_touching);
-        EXPECT_NEAR(double(output.time), 0.4884203672409058, 0.0001);
+        EXPECT_NEAR(double(output.time), 0.4864552915096283, 0.0001);
         EXPECT_EQ(output.stats.toi_iters, 3);
     }
 }
@@ -814,7 +814,7 @@ TEST(TimeOfImpact, ForNonCollidingShapesFails)
             EXPECT_EQ(output.stats.toi_iters, 1);
             EXPECT_EQ(output.stats.max_dist_iters, 4);
             //EXPECT_TRUE(output.stats.max_root_iters == 23 || output.stats.max_root_iters == 14);
-            EXPECT_EQ(output.stats.max_root_iters, 23);
+            EXPECT_EQ(output.stats.max_root_iters, 27);
         }
     }
     else if (std::is_same<Real, Fixed<std::int32_t,9>>::value)

UnitTests/World.cpp

@@ -2708,6 +2708,13 @@ TEST(World_Longer, TilesComesToRest)
     {
         case  4:
         {
+            // From commit 90d73101c1b105aa0ad00828336da0f3c2b97e5e onward:
+            //EXPECT_EQ(numSteps,         1766ul);
+            //EXPECT_EQ(sumRegPosIters,  36412ul);
+            //EXPECT_EQ(sumRegVelIters,  46675ul);
+            //EXPECT_EQ(sumToiPosIters,  43910ul);
+            //EXPECT_EQ(sumToiVelIters, 113049ul);
+
             // From commit 0b049bd28d1bbb01d1750ec1fc9498105f13d192 onward:
             EXPECT_EQ(numSteps,         1768ul);
             EXPECT_EQ(sumRegPosIters,  36419ul);