added color utils

source/RobotAPI/interface/visualization/DebugDrawerInterface.ice

@@ -53,6 +53,13 @@ module armarx
          byte b;
     };
 
+    struct HsvColor
+    {
+        byte h;
+        byte s;
+        byte v;
+    };
+
 
     /*!
      * \brief Contains information about a layer. (name, visibility and number of elements)

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

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    ArmarX
+ * @author     Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date       2016
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#ifndef _ARMARX_COLORUTILS_H
+#define _ARMARX_COLORUTILS_H
+
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include "MathUtils.h"
+namespace armarx
+{
+    namespace colorutils
+    {
+
+
+
+
+        DrawColor24Bit HsvToRgb(const HsvColor& hsv)
+        {
+            DrawColor24Bit rgb;
+            unsigned char region, remainder, p, q, t;
+
+            if (hsv.s == 0)
+            {
+                rgb.r = hsv.v;
+                rgb.g = hsv.v;
+                rgb.b = hsv.v;
+                return rgb;
+            }
+            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)
+            {
+                case 0:
+                    rgb.r = hsv.v;
+                    rgb.g = t;
+                    rgb.b = p;
+                    break;
+                case 1:
+                    rgb.r = q;
+                    rgb.g = hsv.v;
+                    rgb.b = p;
+                    break;
+                case 2:
+                    rgb.r = p;
+                    rgb.g = hsv.v;
+                    rgb.b = t;
+                    break;
+                case 3:
+                    rgb.r = p;
+                    rgb.g = q;
+                    rgb.b = hsv.v;
+                    break;
+                case 4:
+                    rgb.r = t;
+                    rgb.g = p;
+                    rgb.b = hsv.v;
+                    break;
+                default:
+                    rgb.r = hsv.v;
+                    rgb.g = p;
+                    rgb.b = q;
+                    break;
+            }
+
+            return rgb;
+        }
+
+        HsvColor RgbToHsv(const DrawColor24Bit& rgb)
+        {
+            HsvColor hsv;
+            unsigned char rgbMin, rgbMax;
+
+            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);
+
+            hsv.v = rgbMax;
+            if (hsv.v == 0)
+            {
+                hsv.h = 0;
+                hsv.s = 0;
+                return hsv;
+            }
+
+            hsv.s = 255 * long(rgbMax - rgbMin) / hsv.v;
+            if (hsv.s == 0)
+            {
+                hsv.h = 0;
+                return hsv;
+            }
+
+            if (rgbMax == rgb.r)
+            {
+                hsv.h = 0 + 43 * (rgb.g - rgb.b) / (rgbMax - rgbMin);
+            }
+            else if (rgbMax == rgb.g)
+            {
+                hsv.h = 85 + 43 * (rgb.b - rgb.r) / (rgbMax - rgbMin);
+            }
+            else
+            {
+                hsv.h = 171 + 43 * (rgb.r - rgb.g) / (rgbMax - rgbMin);
+            }
+
+            return hsv;
+        }
+
+        /**
+         * @brief HeatMapColor calculates the color of a value between 0 and 1 on a heat map.
+         * @param percentage value between 0..1
+         * @return color on a heatmap corresponding to parameter. 0 -> blue, 1 -> red. Color has full (255) saturation and value.
+         */
+        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};
+        }
+
+        DrawColor24Bit HeatMapRGBColor(float percentage)
+        {
+            return HsvToRgb(HeatMapColor(percentage));
+        }
+
+        DrawColor HeatMapRGBAColor(float percentage)
+        {
+            auto color = HsvToRgb(HeatMapColor(percentage));
+            return DrawColor {0.0039215686f * color.r, //divide by 255
+                              0.0039215686f * color.g,
+                              0.0039215686f * color.b,
+                              1.0
+                             };
+        }
+
+    }
+
+}
+#endif