refactored HSV color utils

source/RobotAPI/interface/visualization/DebugDrawerInterface.ice

@@ -52,11 +52,15 @@ module armarx
          byte b;
     };
 
+
     struct HsvColor
     {
-        byte h;
-        byte s;
-        byte v;
+        //! 0-360
+        float h;
+        //! 0 - 1
+        float s;
+        //! 0 - 1
+        float v;
     };
 
 

source/RobotAPI/libraries/core/math/ColorUtils.h

@@ -30,103 +30,126 @@ namespace armarx
     namespace colorutils
     {
 
-
-
-
-        DrawColor24Bit HsvToRgb(const HsvColor& hsv)
+        DrawColor24Bit HsvToRgb(const HsvColor& in)
         {
-            DrawColor24Bit rgb;
-            unsigned char region, remainder, p, q, t;
-
-            if (hsv.s == 0)
+            double      hh, p, q, t, ff;
+            long        i;
+            double r, g, b;
+            if (in.s <= 0.0)        // < is bogus, just shuts up warnings
             {
-                rgb.r = hsv.v;
-                rgb.g = hsv.v;
-                rgb.b = hsv.v;
-                return rgb;
+                r = in.v;
+                g = in.v;
+                b = in.v;
+                return DrawColor24Bit {(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
             }
-            constexpr float by43 = 1.0f / 43.0f;
-            region = hsv.h * by43;
-            remainder = (hsv.h - (region * 43)) * 6;
-
-            p = (hsv.v * (255 - hsv.s)) >> 8;
-            q = (hsv.v * (255 - ((hsv.s * remainder) >> 8))) >> 8;
-            t = (hsv.v * (255 - ((hsv.s * (255 - remainder)) >> 8))) >> 8;
-
-            switch (region)
+            hh = in.h;
+            if (hh >= 360.0)
+            {
+                hh = 0.0;
+            }
+            hh /= 60.0;
+            i = (long)hh;
+            ff = hh - i;
+            p = in.v * (1.0 - in.s);
+            q = in.v * (1.0 - (in.s * ff));
+            t = in.v * (1.0 - (in.s * (1.0 - ff)));
+
+            switch (i)
             {
                 case 0:
-                    rgb.r = hsv.v;
-                    rgb.g = t;
-                    rgb.b = p;
+                    r = in.v;
+                    g = t;
+                    b = p;
                     break;
                 case 1:
-                    rgb.r = q;
-                    rgb.g = hsv.v;
-                    rgb.b = p;
+                    r = q;
+                    g = in.v;
+                    b = p;
                     break;
                 case 2:
-                    rgb.r = p;
-                    rgb.g = hsv.v;
-                    rgb.b = t;
+                    r = p;
+                    g = in.v;
+                    b = t;
                     break;
+
                 case 3:
-                    rgb.r = p;
-                    rgb.g = q;
-                    rgb.b = hsv.v;
+                    r = p;
+                    g = q;
+                    b = in.v;
                     break;
                 case 4:
-                    rgb.r = t;
-                    rgb.g = p;
-                    rgb.b = hsv.v;
+                    r = t;
+                    g = p;
+                    b = in.v;
                     break;
+                case 5:
                 default:
-                    rgb.r = hsv.v;
-                    rgb.g = p;
-                    rgb.b = q;
+                    r = in.v;
+                    g = p;
+                    b = q;
                     break;
             }
 
-            return rgb;
+            return DrawColor24Bit {(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
         }
 
-        HsvColor RgbToHsv(const DrawColor24Bit& rgb)
+        HsvColor RgbToHsv(const DrawColor24Bit& in)
         {
-            HsvColor hsv;
-            unsigned char rgbMin, rgbMax;
+            double r = in.r * 0.00392156862;
+            double g = in.g * 0.00392156862;
+            double b = in.b * 0.00392156862;
+            HsvColor         out;
+            double      min, max, delta;
 
-            rgbMin = rgb.r < rgb.g ? (rgb.r < rgb.b ? rgb.r : rgb.b) : (rgb.g < rgb.b ? rgb.g : rgb.b);
-            rgbMax = rgb.r > rgb.g ? (rgb.r > rgb.b ? rgb.r : rgb.b) : (rgb.g > rgb.b ? rgb.g : rgb.b);
+            min = r < g ? r : g;
+            min = min  < b ? min  : b;
 
-            hsv.v = rgbMax;
-            if (hsv.v == 0)
+            max = r > g ? r : g;
+            max = max  > b ? max  : b;
+
+            out.v = max;                                // v
+            delta = max - min;
+            if (delta < 0.00001)
             {
-                hsv.h = 0;
-                hsv.s = 0;
-                return hsv;
+                out.s = 0;
+                out.h = 0; // undefined, maybe nan?
+                return out;
             }
-
-            hsv.s = 255 * long(rgbMax - rgbMin) / hsv.v;
-            if (hsv.s == 0)
+            if (max > 0.0)    // NOTE: if Max is == 0, this divide would cause a crash
             {
-                hsv.h = 0;
-                return hsv;
+                out.s = (delta / max);                  // s
             }
-
-            if (rgbMax == rgb.r)
+            else
+            {
+                // if max is 0, then r = g = b = 0
+                // s = 0, h is undefined
+                out.s = 0.0;
+                out.h = 0;                            // its now undefined
+                return out;
+            }
+            if (r >= max)                            // > is bogus, just keeps compilor happy
             {
-                hsv.h = 0 + 43 * (rgb.g - rgb.b) / (rgbMax - rgbMin);
+                out.h = (g - b) / delta;    // between yellow & magenta
             }
-            else if (rgbMax == rgb.g)
+            else if (g >= max)
             {
-                hsv.h = 85 + 43 * (rgb.b - rgb.r) / (rgbMax - rgbMin);
+                out.h = 2.0 + (b - r) / delta;    // between cyan & yellow
             }
             else
             {
-                hsv.h = 171 + 43 * (rgb.r - rgb.g) / (rgbMax - rgbMin);
+                out.h = 4.0 + (r - g) / delta;    // between magenta & cyan
             }
 
-            return hsv;
+            out.h *= 60.0;                              // degrees
+
+            if (out.h < 0.0)
+            {
+                out.h += 360.0;
+            }
+
+            return out;
+
+
         }
 
         /**
@@ -137,8 +160,8 @@ namespace armarx
         HsvColor HeatMapColor(float percentage)
         {
             percentage = math::MathUtils::LimitMinMax(0.0f, 1.0f, percentage);
-            constexpr float factor = 240.0 * (255.0 / 360.0);
-            return HsvColor {(unsigned char)((1.0 - percentage) * factor), 255, 255};
+
+            return HsvColor {((1.0f - percentage) * 240.f), 1.0f, 1.0f};
         }
 
         DrawColor24Bit HeatMapRGBColor(float percentage)

source/RobotAPI/libraries/core/test/MathUtilsTest.cpp

@@ -25,6 +25,7 @@
 #include <RobotAPI/Test.h>
 #include <ArmarXCore/core/test/IceTestHelper.h>
 #include "../math/MathUtils.h"
+#include "../math/ColorUtils.h"
 using namespace armarx;
 
 void check_close(float a, float b, float tolerance)
@@ -60,3 +61,16 @@ BOOST_AUTO_TEST_CASE(fmodTest)
     }
 
 }
+
+BOOST_AUTO_TEST_CASE(ColorUtilTest)
+{
+    auto testColor = [](DrawColor24Bit rgb)
+    {
+        auto hsv = colorutils::RgbToHsv(rgb);
+        ARMARX_INFO << VAROUT(rgb.r) << VAROUT(rgb.g) << VAROUT(rgb.b) << " --> " << VAROUT(hsv.h) << VAROUT(hsv.s) << VAROUT(hsv.v);
+    };
+    testColor({255, 0, 0});
+    testColor({0, 255, 0});
+    testColor({0, 0, 255});
+    testColor({0, 20, 0});
+}