Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add isNear method to MathUtil (#5353)

Browse Source 
This method is used to check if the given value matches an expected value within a certain tolerance.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Gold856
2023-06-20 02:00:07 -04:00
committed by GitHub
parent 2ce248f66c
commit f5b0d1484b
4 changed files with 189 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
wpimath/src/test/native/cpp/MathUtilTest.cpp
View File

@@ -117,3 +117,49 @@ TEST(MathUtilTest, AngleModulus) {
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-std::numbers::pi / 2}),
units::radian_t{-std::numbers::pi / 2});
}
TEST(MathUtilTest, IsNear) {
// The answer is always 42
// Positive integer checks
EXPECT_TRUE(frc::IsNear(42, 42, 1));
EXPECT_TRUE(frc::IsNear(42, 41, 2));
EXPECT_TRUE(frc::IsNear(42, 43, 2));
EXPECT_FALSE(frc::IsNear(42, 44, 1));
// Negative integer checks
EXPECT_TRUE(frc::IsNear(-42, -42, 1));
EXPECT_TRUE(frc::IsNear(-42, -41, 2));
EXPECT_TRUE(frc::IsNear(-42, -43, 2));
EXPECT_FALSE(frc::IsNear(-42, -44, 1));
// Mixed sign integer checks
EXPECT_FALSE(frc::IsNear(-42, 42, 1));
EXPECT_FALSE(frc::IsNear(-42, 41, 2));
EXPECT_FALSE(frc::IsNear(-42, 43, 2));
EXPECT_FALSE(frc::IsNear(42, -42, 1));
EXPECT_FALSE(frc::IsNear(42, -41, 2));
EXPECT_FALSE(frc::IsNear(42, -43, 2));
// Floating point checks
EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 1));
EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 1));
EXPECT_TRUE(frc::IsNear<double>(42, 41.5, 0.75));
EXPECT_TRUE(frc::IsNear<double>(42, 42.5, 0.75));
// Wraparound checks
EXPECT_TRUE(frc::IsNear(0_deg, 356_deg, 5_deg, 0_deg, 360_deg));
EXPECT_TRUE(frc::IsNear(0, -356, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(0, 4, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(0, -4, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(400, 41, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(400, -319, 5, 0, 360));
EXPECT_TRUE(frc::IsNear(400, 401, 5, 0, 360));
EXPECT_FALSE(frc::IsNear<double>(0, 356, 2.5, 0, 360));
EXPECT_FALSE(frc::IsNear<double>(0, -356, 2.5, 0, 360));
EXPECT_FALSE(frc::IsNear<double>(0, 4, 2.5, 0, 360));
EXPECT_FALSE(frc::IsNear<double>(0, -4, 2.5, 0, 360));
EXPECT_FALSE(frc::IsNear(400, 35, 5, 0, 360));
EXPECT_FALSE(frc::IsNear(400, -315, 5, 0, 360));
EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
}