articulation_rviz_plugin: utils.cpp Source File

00001 /*
00002  * utils.cpp
00003  *
00004  *  Created on: Oct 1, 2009
00005  *      Author: sturm
00006  */
00007 
00008 #include "utils.h"
00009 
00010 namespace articulation_rviz_plugin {
00011 
00012 btVector3 HSV_to_RGB(btVector3 color) {
00013         double h = color.x(), s = color.y(), v = color.z();
00014         h = color.x();
00015         h -= floor(h);
00016         h *= 6;
00017         int i;
00018         float m, n, f;
00019 
00020         i = floor(h);
00021         f = h - i;
00022         if (!(i & 1))
00023                 f = 1 - f; // if i is even
00024         m = v * (1 - s);
00025         n = v * (1 - s * f);
00026         switch (i) {
00027         case 6:
00028         case 0:
00029                 return btVector3(v, n, m);
00030         case 1:
00031                 return btVector3(n, v, m);
00032         case 2:
00033                 return btVector3(m, v, n);
00034         case 3:
00035                 return btVector3(m, n, v);
00036         case 4:
00037                 return btVector3(n, m, v);
00038         case 5:
00039                 return btVector3(v, m, n);
00040         default:
00041                 return btVector3(1, 0.5, 0.5);
00042         }
00043 }
00044 
00045 btVector3 RGB_to_HSV( btVector3 color ) {
00046         double r = color.x(), g = color.y(), b = color.z();
00047         // RGB are from 0..1, H is from 0..360, SV from 0..1
00048         double maxC = b;
00049         if (maxC < g) maxC = g;
00050         if (maxC < r) maxC = r;
00051         double minC = b;
00052         if (minC > g) minC = g;
00053         if (minC > r) minC = r;
00054 
00055         double delta = maxC - minC;
00056 
00057         double V = maxC;
00058         double S = 0;
00059         double H = 0;
00060 
00061         if (delta == 0)
00062         {
00063                 H = 0;
00064                 S = 0;
00065         }
00066         else
00067         {
00068                 S = delta / maxC;
00069                 double dR = 60*(maxC - r)/delta + 180;
00070                 double dG = 60*(maxC - g)/delta + 180;
00071                 double dB = 60*(maxC - b)/delta + 180;
00072                 if (r == maxC)
00073                         H = dB - dG;
00074                 else if (g == maxC)
00075                         H = 120 + dR - dB;
00076                 else
00077                         H = 240 + dG - dR;
00078         }
00079 
00080         if (H<0)
00081                 H+=360;
00082         if (H>=360)
00083                 H-=360;
00084 
00085         return( btVector3(H/360.0, S, V) );
00086 }
00087 
00088 }