From 34dcc46c0d3867bcc6d69aa2cfdcf1d1981a3f95 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 22 Jun 2024 11:32:49 -0700
Subject: [PATCH] [wpimath] Report error when x and y components of Rotation2d
 are zero

Fixes #6766.
---
 .../java/edu/wpi/first/math/geometry/Rotation2d.java   |  3 +++
 .../main/native/include/frc/geometry/Rotation2d.inc    | 10 ++++++++++
 2 files changed, 13 insertions(+)

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..cbcc3b91555 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
@@ -10,6 +10,7 @@
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.proto.Rotation2dProto;
 import edu.wpi.first.math.geometry.struct.Rotation2dStruct;
@@ -118,6 +119,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..740aaf0090d 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 <string>
+#include <type_traits>
+
 #include <gcem.hpp>
+#include <wpi/StackTrace.h>
 
 #include "frc/geometry/Rotation2d.h"
 #include "units/angle.h"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 
@@ -24,6 +29,11 @@ constexpr Rotation2d::Rotation2d(double x, double y) {
   } else {
     m_sin = 0.0;
     m_cos = 1.0;
+    if (!std::is_constant_evaluated()) {
+      wpi::math::MathSharedStore::ReportError(
+          "x and y components of Rotation2d are zero\n{}",
+          wpi::GetStackTrace(1));
+    }
   }
   m_value = units::radian_t{gcem::atan2(m_sin, m_cos)};
 }