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

[wpimath] Deduplicate angle modulus functions (#2998)

Browse Source 
frc::NormalizeAngle(), units::math::NormalizeAngle(), and
frc::GetModulusError() were replaced with frc::InputModulus() and
frc::AngleModulus().

They were placed in wpimath/src/main/native/include/frc/MathUtil.h for
C++ and wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
for Java.
This commit is contained in:
Tyler Veness
2021-01-01 16:22:00 -08:00
committed by GitHub
parent bf8c0da4be
commit 62f0f8190d
23 changed files with 210 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
wpimath/src/main/native/include/units/math.h
View File

@@ -751,24 +751,4 @@ auto fma(const UnitTypeLhs x, const UnitMultiply y, const UnitAdd z) noexcept
"Unit types are not compatible.");
return resultType(std::fma(x(), y(), resultType(z)()));
}
/**
* Constrains theta to within the range (-pi, pi].
*
* @param theta Angle to normalize.
*/
constexpr units::radian_t NormalizeAngle(units::radian_t theta) {
units::radian_t pi(wpi::math::pi);
// Constrain theta to within (-3pi, pi)
const int n_pi_pos = (theta + pi) / 2.0 / pi;
theta = theta - units::radian_t{n_pi_pos * 2.0 * wpi::math::pi};
// Cut off the bottom half of the above range to constrain within
// (-pi, pi]
const int n_pi_neg = (theta - pi) / 2.0 / pi;
theta = theta - units::radian_t{n_pi_neg * 2.0 * wpi::math::pi};
return theta;
}
} // namespace units::math