[wpilib] Fix precision issue in Color round-and-clamp (#6100)

wpilibsuite · Dec 26, 2023 · 795d4be · 795d4be
1 parent 7aa9ad4
commit 795d4be
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 22 deletions.
diff --git a/wpilibc/src/main/native/include/frc/util/Color.h b/wpilibc/src/main/native/include/frc/util/Color.h
@@ -10,6 +10,7 @@
 #include <string_view>
 
 #include <fmt/core.h>
+#include <gcem.hpp>
 #include <wpi/StringExtras.h>
 #include <wpi/ct_string.h>
 
@@ -868,12 +869,8 @@ class Color {
   double blue = 0.0;
 
  private:
-  static constexpr double kPrecision = 1.0 / (1 << 12);
-
   static constexpr double roundAndClamp(double value) {
-    const auto rounded =
-        (static_cast<int>(value / kPrecision) + 0.5) * kPrecision;
-    return std::clamp(rounded, 0.0, 1.0);
+    return std::clamp(gcem::ceil(value * (1 << 12)) / (1 << 12), 0.0, 1.0);
   }
 };
 

diff --git a/wpilibc/src/test/native/cpp/util/ColorTest.cpp b/wpilibc/src/test/native/cpp/util/ColorTest.cpp
@@ -17,11 +17,22 @@ TEST(ColorTest, ConstructDefault) {
 }
 
 TEST(ColorTest, ConstructFromDoubles) {
-  constexpr frc::Color color{1.0, 0.5, 0.25};
-
-  EXPECT_NEAR(1.0, color.red, 1e-2);
-  EXPECT_NEAR(0.5, color.green, 1e-2);
-  EXPECT_NEAR(0.25, color.blue, 1e-2);
+  {
+    constexpr frc::Color color{1.0, 0.5, 0.25};
+
+    EXPECT_NEAR(1.0, color.red, 1e-2);
+    EXPECT_NEAR(0.5, color.green, 1e-2);
+    EXPECT_NEAR(0.25, color.blue, 1e-2);
+  }
+
+  {
+    constexpr frc::Color color{1.0, 0.0, 0.0};
+
+    // Check for exact match to ensure round-and-clamp is correct
+    EXPECT_EQ(1.0, color.red);
+    EXPECT_EQ(0.0, color.green);
+    EXPECT_EQ(0.0, color.blue);
+  }
 }
 
 TEST(ColorTest, ConstructFromInts) {
@@ -52,9 +63,9 @@ TEST(ColorTest, ConstructFromHexString) {
 TEST(ColorTest, FromHSV) {
   constexpr frc::Color color = frc::Color::FromHSV(90, 128, 64);
 
-  EXPECT_DOUBLE_EQ(0.1256103515625, color.red);
-  EXPECT_DOUBLE_EQ(0.2510986328125, color.green);
-  EXPECT_DOUBLE_EQ(0.2510986328125, color.blue);
+  EXPECT_DOUBLE_EQ(0.125732421875, color.red);
+  EXPECT_DOUBLE_EQ(0.251220703125, color.green);
+  EXPECT_DOUBLE_EQ(0.251220703125, color.blue);
 }
 
 TEST(ColorTest, ToHexString) {

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
@@ -14,8 +14,6 @@
  */
 @SuppressWarnings("MemberName")
 public class Color {
-  private static final double kPrecision = Math.pow(2, -12);
-
   public final double red;
   public final double green;
   public final double blue;
@@ -178,8 +176,7 @@ public String toHexString() {
   }
 
   private static double roundAndClamp(double value) {
-    final var rounded = Math.round((value + kPrecision / 2) / kPrecision) * kPrecision;
-    return MathUtil.clamp(rounded, 0.0, 1.0);
+    return MathUtil.clamp(Math.ceil(value * (1 << 12)) / (1 << 12), 0.0, 1.0);
   }
 
   /*

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java
@@ -21,11 +21,22 @@ void testConstructDefault() {
 
   @Test
   void testConstructFromDoubles() {
-    var color = new Color(1.0, 0.5, 0.25);
-
-    assertEquals(1.0, color.red, 1e-2);
-    assertEquals(0.5, color.green, 1e-2);
-    assertEquals(0.25, color.blue, 1e-2);
+    {
+      var color = new Color(1.0, 0.5, 0.25);
+
+      assertEquals(1.0, color.red, 1e-2);
+      assertEquals(0.5, color.green, 1e-2);
+      assertEquals(0.25, color.blue, 1e-2);
+    }
+
+    {
+      var color = new Color(1.0, 0.0, 0.0);
+
+      // Check for exact match to ensure round-and-clamp is correct
+      assertEquals(1.0, color.red);
+      assertEquals(0.0, color.green);
+      assertEquals(0.0, color.blue);
+    }
   }
 
   @Test