diff --git a/PlayRho/Common/Position.hpp b/PlayRho/Common/Position.hpp index 3dce1c869e..7a90ec51f2 100644 --- a/PlayRho/Common/Position.hpp +++ b/PlayRho/Common/Position.hpp @@ -125,6 +125,15 @@ bool IsValid(const d2::Position& value) noexcept namespace d2 { + +namespace { +template +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 diff --git a/UnitTests/Sweep.cpp b/UnitTests/Sweep.cpp index 0b402b5319..f7bc4e55d9 100644 --- a/UnitTests/Sweep.cpp +++ b/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()); diff --git a/UnitTests/TimeOfImpact.cpp b/UnitTests/TimeOfImpact.cpp index 3798f2ce72..e1e4af3568 100644 --- a/UnitTests/TimeOfImpact.cpp +++ b/UnitTests/TimeOfImpact.cpp @@ -560,7 +560,7 @@ TEST(TimeOfImpact, RodCircleMissAt360) { EXPECT_EQ(output.state, TOIOutput::e_separated); EXPECT_NEAR(static_cast(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>::value) diff --git a/UnitTests/World.cpp b/UnitTests/World.cpp index cfae782013..e015da3eb7 100644 --- a/UnitTests/World.cpp +++ b/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);