From 7cd105ad231b03575dcf824b97776ca3bbba3688 Mon Sep 17 00:00:00 2001 From: ninnghazad Date: Mon, 1 Jul 2019 22:57:01 +0200 Subject: [PATCH 1/6] Fix GetPosition's interpolation to properly handle the angular part. --- PlayRho/Common/Position.hpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/PlayRho/Common/Position.hpp b/PlayRho/Common/Position.hpp index 3dce1c869e..6d338e7cb3 100644 --- a/PlayRho/Common/Position.hpp +++ b/PlayRho/Common/Position.hpp @@ -145,12 +145,23 @@ 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. + PLAYRHO_CONSTEXPR const auto twoPi = Pi+Pi; + const auto da = pos1.angular - pos0.angular; + return { + pos0.linear + (pos1.linear - pos0.linear) * beta, + pos0.angular + (da - twoPi * std::floor((da + Pi) / twoPi)) * beta + }; } } // namespace d2 From 90d73101c1b105aa0ad00828336da0f3c2b97e5e Mon Sep 17 00:00:00 2001 From: ninnghazad Date: Wed, 3 Jul 2019 16:37:50 +0200 Subject: [PATCH 2/6] Update Position.hpp --- PlayRho/Common/Position.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PlayRho/Common/Position.hpp b/PlayRho/Common/Position.hpp index 6d338e7cb3..2a30b2f928 100644 --- a/PlayRho/Common/Position.hpp +++ b/PlayRho/Common/Position.hpp @@ -156,7 +156,7 @@ PLAYRHO_CONSTEXPR inline Position GetPosition(const Position pos0, const Positio // 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. - PLAYRHO_CONSTEXPR const auto twoPi = Pi+Pi; + const auto twoPi = Pi+Pi; const auto da = pos1.angular - pos0.angular; return { pos0.linear + (pos1.linear - pos0.linear) * beta, From 1efed2ed5e9050bc46fa6b0c3adb8c83985d697b Mon Sep 17 00:00:00 2001 From: ninnghazad Date: Wed, 3 Jul 2019 20:18:36 +0200 Subject: [PATCH 3/6] Updated tests and added one to check angle interpolation wrapping over Pi --- PlayRho/Common/Position.hpp | 3 ++- UnitTests/Sweep.cpp | 9 ++++++++- UnitTests/TimeOfImpact.cpp | 6 +++--- UnitTests/World.cpp | 17 ++++++++++++----- 4 files changed, 25 insertions(+), 10 deletions(-) diff --git a/PlayRho/Common/Position.hpp b/PlayRho/Common/Position.hpp index 2a30b2f928..cbe7588c39 100644 --- a/PlayRho/Common/Position.hpp +++ b/PlayRho/Common/Position.hpp @@ -158,9 +158,10 @@ PLAYRHO_CONSTEXPR inline Position GetPosition(const Position pos0, const Positio // 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 * std::floor((da + Pi) / twoPi)) * beta; return { pos0.linear + (pos1.linear - pos0.linear) * beta, - pos0.angular + (da - twoPi * std::floor((da + Pi) / twoPi)) * beta + na - twoPi * std::floor((na + Pi) / twoPi) }; } 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..aa2f82ef41 100644 --- a/UnitTests/World.cpp +++ b/UnitTests/World.cpp @@ -2708,12 +2708,19 @@ 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); - EXPECT_EQ(sumRegVelIters, 46684ul); - EXPECT_EQ(sumToiPosIters, 43814ul); - EXPECT_EQ(sumToiVelIters, 113452ul); + //EXPECT_EQ(numSteps, 1768ul); + //EXPECT_EQ(sumRegPosIters, 36419ul); + //EXPECT_EQ(sumRegVelIters, 46684ul); + //EXPECT_EQ(sumToiPosIters, 43814ul); + //EXPECT_EQ(sumToiVelIters, 113452ul); // From commit 0b049bd28d1bbb01d1750ec1fc9498105f13d192 onward: //EXPECT_EQ(numSteps, 1799ul); From d3ac28df9849d5ccb0ee69ff5834ef6cf99b3c04 Mon Sep 17 00:00:00 2001 From: ninnghazad Date: Wed, 3 Jul 2019 22:34:05 +0200 Subject: [PATCH 4/6] Try just disabling constexpr for GetPosition --- PlayRho/Common/Position.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PlayRho/Common/Position.hpp b/PlayRho/Common/Position.hpp index cbe7588c39..ad9285a721 100644 --- a/PlayRho/Common/Position.hpp +++ b/PlayRho/Common/Position.hpp @@ -134,7 +134,7 @@ namespace d2 { /// position 1 if beta == 1, or at the given unit interval value /// between position 0 and position 1. /// @relatedalso Position -PLAYRHO_CONSTEXPR inline Position GetPosition(const Position pos0, const Position pos1, +/*PLAYRHO_CONSTEXPR*/ inline Position GetPosition(const Position pos0, const Position pos1, const Real beta) noexcept { assert(IsValid(pos0)); From 2d505374cf5b314d0f4b65a66b2536e85191702e Mon Sep 17 00:00:00 2001 From: ninnghazad Date: Wed, 3 Jul 2019 22:48:14 +0200 Subject: [PATCH 5/6] Reenabled constexpr on GetPosition and wrote a constexpr floor instead. --- PlayRho/Common/Position.hpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/PlayRho/Common/Position.hpp b/PlayRho/Common/Position.hpp index ad9285a721..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. @@ -134,7 +143,7 @@ namespace d2 { /// position 1 if beta == 1, or at the given unit interval value /// between position 0 and position 1. /// @relatedalso Position -/*PLAYRHO_CONSTEXPR*/ inline Position GetPosition(const Position pos0, const Position pos1, +PLAYRHO_CONSTEXPR inline Position GetPosition(const Position pos0, const Position pos1, const Real beta) noexcept { assert(IsValid(pos0)); @@ -158,10 +167,10 @@ namespace d2 { // 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 * std::floor((da + Pi) / twoPi)) * beta; + const auto na = pos0.angular + (da - twoPi * cfloor((da + Pi) / twoPi)) * beta; return { pos0.linear + (pos1.linear - pos0.linear) * beta, - na - twoPi * std::floor((na + Pi) / twoPi) + na - twoPi * cfloor((na + Pi) / twoPi) }; } From d34a825901549a3b0692b33ec4a58c3b5b8b72fb Mon Sep 17 00:00:00 2001 From: ninnghazad Date: Wed, 3 Jul 2019 23:12:35 +0200 Subject: [PATCH 6/6] World_Longer.TilesComesToRest still not working --- UnitTests/World.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/UnitTests/World.cpp b/UnitTests/World.cpp index aa2f82ef41..e015da3eb7 100644 --- a/UnitTests/World.cpp +++ b/UnitTests/World.cpp @@ -2709,18 +2709,18 @@ 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); + //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); - //EXPECT_EQ(sumRegVelIters, 46684ul); - //EXPECT_EQ(sumToiPosIters, 43814ul); - //EXPECT_EQ(sumToiVelIters, 113452ul); + EXPECT_EQ(numSteps, 1768ul); + EXPECT_EQ(sumRegPosIters, 36419ul); + EXPECT_EQ(sumRegVelIters, 46684ul); + EXPECT_EQ(sumToiPosIters, 43814ul); + EXPECT_EQ(sumToiVelIters, 113452ul); // From commit 0b049bd28d1bbb01d1750ec1fc9498105f13d192 onward: //EXPECT_EQ(numSteps, 1799ul);