diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 83366e576e4..aa0da86a388 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -118,6 +118,8 @@ public Rotation2d(double x, double y) { } else { m_sin = 0.0; m_cos = 1.0; + MathSharedStore.reportError( + "x and y components of Rotation2d are zero\n", Thread.currentThread().getStackTrace()); } m_value = Math.atan2(m_sin, m_cos); } diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc index 104b66b1605..fe8f320dab8 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc @@ -4,10 +4,15 @@ #pragma once +#include +#include + #include +#include #include "frc/geometry/Rotation2d.h" #include "units/angle.h" +#include "wpimath/MathShared.h" namespace frc { @@ -24,6 +29,12 @@ constexpr Rotation2d::Rotation2d(double x, double y) { } else { m_sin = 0.0; m_cos = 1.0; + if (!std::is_constant_evaluated()) { + std::string msg = + fmt::format("x and y components of Rotation2d are zero\n{}", + wpi::GetStackTrace(1)); + wpi::math::MathSharedStore::ReportError(msg); + } } m_value = units::radian_t{gcem::atan2(m_sin, m_cos)}; }