diff --git a/wpilibc/src/main/native/cpp/Joystick.cpp b/wpilibc/src/main/native/cpp/Joystick.cpp index 7c95a05803..34afd0ad2a 100644 --- a/wpilibc/src/main/native/cpp/Joystick.cpp +++ b/wpilibc/src/main/native/cpp/Joystick.cpp @@ -10,14 +10,13 @@ #include #include +#include #include "frc/DriverStation.h" #include "frc/WPIErrors.h" using namespace frc; -constexpr double kPi = 3.14159265358979323846; - Joystick::Joystick(int port) : GenericHID(port) { m_axes[Axis::kX] = kDefaultXChannel; m_axes[Axis::kY] = kDefaultYChannel; @@ -91,5 +90,5 @@ double Joystick::GetDirectionRadians() const { } double Joystick::GetDirectionDegrees() const { - return (180 / kPi) * GetDirectionRadians(); + return (180 / wpi::math::pi) * GetDirectionRadians(); } diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp index b1af5de56e..ba5f691f22 100644 --- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -11,14 +11,13 @@ #include #include +#include #include "frc/SpeedController.h" #include "frc/smartdashboard/SendableBuilder.h" using namespace frc; -constexpr double kPi = 3.14159265358979323846; - KilloughDrive::KilloughDrive(SpeedController& leftMotor, SpeedController& rightMotor, SpeedController& backMotor) @@ -30,12 +29,12 @@ KilloughDrive::KilloughDrive(SpeedController& leftMotor, SpeedController& backMotor, double leftMotorAngle, double rightMotorAngle, double backMotorAngle) : m_leftMotor(leftMotor), m_rightMotor(rightMotor), m_backMotor(backMotor) { - m_leftVec = {std::cos(leftMotorAngle * (kPi / 180.0)), - std::sin(leftMotorAngle * (kPi / 180.0))}; - m_rightVec = {std::cos(rightMotorAngle * (kPi / 180.0)), - std::sin(rightMotorAngle * (kPi / 180.0))}; - m_backVec = {std::cos(backMotorAngle * (kPi / 180.0)), - std::sin(backMotorAngle * (kPi / 180.0))}; + m_leftVec = {std::cos(leftMotorAngle * (wpi::math::pi / 180.0)), + std::sin(leftMotorAngle * (wpi::math::pi / 180.0))}; + m_rightVec = {std::cos(rightMotorAngle * (wpi::math::pi / 180.0)), + std::sin(rightMotorAngle * (wpi::math::pi / 180.0))}; + m_backVec = {std::cos(backMotorAngle * (wpi::math::pi / 180.0)), + std::sin(backMotorAngle * (wpi::math::pi / 180.0))}; AddChild(&m_leftMotor); AddChild(&m_rightMotor); AddChild(&m_backMotor); @@ -84,8 +83,9 @@ void KilloughDrive::DrivePolar(double magnitude, double angle, reported = true; } - DriveCartesian(magnitude * std::sin(angle * (kPi / 180.0)), - magnitude * std::cos(angle * (kPi / 180.0)), zRotation, 0.0); + DriveCartesian(magnitude * std::sin(angle * (wpi::math::pi / 180.0)), + magnitude * std::cos(angle * (wpi::math::pi / 180.0)), + zRotation, 0.0); } void KilloughDrive::StopMotor() { diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp index c74ba191e9..c62435499d 100644 --- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -11,6 +11,7 @@ #include #include +#include #include "frc/SpeedController.h" #include "frc/drive/Vector2d.h" @@ -18,8 +19,6 @@ using namespace frc; -constexpr double kPi = 3.14159265358979323846; - MecanumDrive::MecanumDrive(SpeedController& frontLeftMotor, SpeedController& rearLeftMotor, SpeedController& frontRightMotor, @@ -81,8 +80,9 @@ void MecanumDrive::DrivePolar(double magnitude, double angle, reported = true; } - DriveCartesian(magnitude * std::sin(angle * (kPi / 180.0)), - magnitude * std::cos(angle * (kPi / 180.0)), zRotation, 0.0); + DriveCartesian(magnitude * std::sin(angle * (wpi::math::pi / 180.0)), + magnitude * std::cos(angle * (wpi::math::pi / 180.0)), + zRotation, 0.0); } bool MecanumDrive::IsRightSideInverted() const { diff --git a/wpilibc/src/main/native/cpp/drive/Vector2d.cpp b/wpilibc/src/main/native/cpp/drive/Vector2d.cpp index 3d4b689a31..a6e68a6a5c 100644 --- a/wpilibc/src/main/native/cpp/drive/Vector2d.cpp +++ b/wpilibc/src/main/native/cpp/drive/Vector2d.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -9,9 +9,9 @@ #include -using namespace frc; +#include -constexpr double kPi = 3.14159265358979323846; +using namespace frc; Vector2d::Vector2d(double x, double y) { this->x = x; @@ -19,8 +19,8 @@ Vector2d::Vector2d(double x, double y) { } void Vector2d::Rotate(double angle) { - double cosA = std::cos(angle * (kPi / 180.0)); - double sinA = std::sin(angle * (kPi / 180.0)); + double cosA = std::cos(angle * (wpi::math::pi / 180.0)); + double sinA = std::sin(angle * (wpi::math::pi / 180.0)); double out[2]; out[0] = x * cosA - y * sinA; out[1] = x * sinA + y * cosA; diff --git a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h index 324cd83c94..5bcc8d2816 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h @@ -7,6 +7,8 @@ #pragma once +#include + namespace frc { /** @@ -46,11 +48,11 @@ class Rotation2d { static Rotation2d FromDegrees(double degrees); /** - * Adds two rotations together, with the result being bounded between -kPi and - * kPi. + * Adds two rotations together, with the result being bounded between -pi and + * pi. * * For example, Rotation2d.FromDegrees(30) + Rotation2d.FromDegrees(60) = - * Rotation2d{-kPi/2} + * Rotation2d{-pi/2} * * @param other The rotation to add. * @@ -75,7 +77,7 @@ class Rotation2d { * rotation. * * For example, Rotation2d.FromDegrees(10) - Rotation2d.FromDegrees(100) = - * Rotation2d{-kPi/2} + * Rotation2d{-pi/2} * * @param other The rotation to subtract. * @@ -157,16 +159,14 @@ class Rotation2d { double m_cos = 1; double m_sin = 0; - constexpr static double kPi = 3.14159265358979323846; - template static T Rad2Deg(const T& rad) { - return rad * 180.0 / kPi; + return rad * 180.0 / wpi::math::pi; } template static T Deg2Rad(const T& deg) { - return deg * kPi / 180.0; + return deg * wpi::math::pi / 180.0; } }; } // namespace frc diff --git a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp index 8ebf1f293a..1363b1cfeb 100644 --- a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp +++ b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp @@ -11,6 +11,8 @@ #include #include +#include + #include "gtest/gtest.h" // Filter constants @@ -36,8 +38,7 @@ std::ostream& operator<<(std::ostream& os, } static double GetData(double t) { - constexpr double kPi = 3.14159265358979323846; - return 100.0 * std::sin(2.0 * kPi * t); + return 100.0 * std::sin(2.0 * wpi::math::pi * t); } class LinearFilterNoiseTest diff --git a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp index a6e57a88e2..5e32784421 100644 --- a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp +++ b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp @@ -12,6 +12,8 @@ #include #include +#include + #include "gtest/gtest.h" // Filter constants @@ -52,8 +54,8 @@ std::ostream& operator<<(std::ostream& os, } static double GetData(double t) { - constexpr double kPi = 3.14159265358979323846; - return 100.0 * std::sin(2.0 * kPi * t) + 20.0 * std::cos(50.0 * kPi * t); + return 100.0 * std::sin(2.0 * wpi::math::pi * t) + + 20.0 * std::cos(50.0 * wpi::math::pi * t); } static double GetPulseData(double t) { diff --git a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp index 24a4caf6e3..c91800eed3 100644 --- a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -7,17 +7,18 @@ #include +#include + #include "frc/geometry/Rotation2d.h" #include "gtest/gtest.h" using namespace frc; static constexpr double kEpsilon = 1E-9; -static constexpr double kPi = 3.14159265358979323846; TEST(Rotation2dTest, RadiansToDegrees) { - const Rotation2d one{kPi / 3}; - const Rotation2d two{kPi / 4}; + const Rotation2d one{wpi::math::pi / 3}; + const Rotation2d two{wpi::math::pi / 4}; EXPECT_NEAR(one.Degrees(), 60.0, kEpsilon); EXPECT_NEAR(two.Degrees(), 45.0, kEpsilon); @@ -27,15 +28,15 @@ TEST(Rotation2dTest, DegreesToRadians) { const auto one = Rotation2d::FromDegrees(45.0); const auto two = Rotation2d::FromDegrees(30.0); - EXPECT_NEAR(one.Radians(), kPi / 4.0, kEpsilon); - EXPECT_NEAR(two.Radians(), kPi / 6.0, kEpsilon); + EXPECT_NEAR(one.Radians(), wpi::math::pi / 4.0, kEpsilon); + EXPECT_NEAR(two.Radians(), wpi::math::pi / 6.0, kEpsilon); } TEST(Rotation2dTest, RotateByFromZero) { const Rotation2d zero; auto sum = zero + Rotation2d::FromDegrees(90.0); - EXPECT_NEAR(sum.Radians(), kPi / 2.0, kEpsilon); + EXPECT_NEAR(sum.Radians(), wpi::math::pi / 2.0, kEpsilon); EXPECT_NEAR(sum.Degrees(), 90.0, kEpsilon); } diff --git a/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp index 78e1e96985..8e0a14acda 100644 --- a/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -10,8 +10,7 @@ #include #include #include - -constexpr double kPi = 3.14159265358979; +#include /** * This sample program shows how to control a motor using a joystick. In the @@ -39,7 +38,7 @@ class Robot : public frc::TimedRobot { void RobotInit() override { // Use SetDistancePerPulse to set the multiplier for GetDistance // This is set up assuming a 6 inch wheel with a 360 CPR encoder. - m_encoder.SetDistancePerPulse((kPi * 6) / 360.0); + m_encoder.SetDistancePerPulse((wpi::math::pi * 6) / 360.0); } private: diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java index 5bab29d232..bca5c9291a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java @@ -68,11 +68,11 @@ public class Rotation2d { } /** - * Adds two rotations together, with the result being bounded between -kPi and - * kPi. + * Adds two rotations together, with the result being bounded between -pi and + * pi. * *

For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) = - * Rotation2d{-kPi/2} + * Rotation2d{-pi/2} * * @param other The rotation to add. * @return The sum of the two rotations. @@ -86,7 +86,7 @@ public class Rotation2d { * rotation. * *

For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) = - * Rotation2d{-kPi/2} + * Rotation2d{-pi/2} * * @param other The rotation to subtract. * @return The difference between the two rotations. diff --git a/wpiutil/src/main/native/include/wpi/math b/wpiutil/src/main/native/include/wpi/math new file mode 100644 index 0000000000..d9c18c7f68 --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/math @@ -0,0 +1,67 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +namespace wpi::math { + +template >> +inline constexpr T e_v = 2.718281828459045235360287471352662498L; + +template >> +inline constexpr T log2e_v = 1.442695040888963407359924681001892137L; + +template >> +inline constexpr T log10e_v = 0.434294481903251827651128918916605082L; + +template >> +inline constexpr T pi_v = 3.141592653589793238462643383279502884L; + +template >> +inline constexpr T inv_pi_v = 0.318309886183790671537767526745028724L; + +template >> +inline constexpr T inv_sqrtpi_v = 0.564189583547756286948079451560772586L; + +template >> +inline constexpr T ln2_v = 0.693147180559945309417232121458176568L; + +template >> +inline constexpr T ln10_v = 2.302585092994045684017991454684364208L; + +template >> +inline constexpr T sqrt2_v = 1.414213562373095048801688724209698078L; + +template >> +inline constexpr T sqrt3_v = 1.732050807568877293527446341505872366L; + +template >> +inline constexpr T inv_sqrt3_v = 0.577350269189625764509148780501957456L; + +template >> +inline constexpr T egamma_v = 0.577215664901532860606512090082402431L; + +template >> +inline constexpr T phi_v = 1.618033988749894848204586834365638117L; + +inline constexpr double e = e_v; +inline constexpr double log2e = log2e_v; +inline constexpr double log10e = log10e_v; +inline constexpr double pi = pi_v; +inline constexpr double inv_pi = inv_pi_v; +inline constexpr double inv_sqrtpi = inv_sqrtpi_v; +inline constexpr double ln2 = ln2_v; +inline constexpr double ln10 = ln10_v; +inline constexpr double sqrt2 = sqrt2_v; +inline constexpr double sqrt3 = sqrt3_v; +inline constexpr double inv_sqrt3 = inv_sqrt3_v; +inline constexpr double egamma = egamma_v; +inline constexpr double phi = phi_v; + +} // namespace wpi::math