| Brian Silverman | 890a32a | 2018-03-11 15:41:56 -0700 | [diff] [blame^] | 1 | #ifndef CTR_EXCLUDE_WPILIB_CLASSES |
| 2 | |
| 3 | #include "ctre/phoenix/HsvToRgb.h" |
| 4 | #include <math.h> |
| 5 | |
| 6 | namespace ctre { |
| 7 | namespace phoenix { |
| 8 | /** |
| 9 | * Convert hue/saturation/and value into RGB values |
| 10 | * |
| 11 | * @param hDegrees Hue in degrees |
| 12 | * @param S Saturation with range of 0 to 1 |
| 13 | * @param V Value with range of 0 to 1 |
| 14 | * @param r Calculated Red value of RGB |
| 15 | * @param g Calculated Green value of RGB |
| 16 | * @param b Calculated Blue value of RGB |
| 17 | */ |
| 18 | void HsvToRgb::Convert(double hDegrees, double S, double V, float* r, float* g, |
| 19 | float* b) { |
| 20 | double R, G, B; |
| 21 | double H = hDegrees; |
| 22 | |
| 23 | //Handles wrap-around |
| 24 | if (H < 0) { |
| 25 | H += 360; |
| 26 | }; |
| 27 | if (H >= 360) { |
| 28 | H -= 360; |
| 29 | }; |
| 30 | |
| 31 | if (V <= 0) |
| 32 | R = G = B = 0; |
| 33 | else if (S <= 0) |
| 34 | R = G = B = V; |
| 35 | else { |
| 36 | double hf = H / 60.0; |
| 37 | int i = (int) floor(hf); |
| 38 | double f = hf - i; |
| 39 | double pv = V * (1 - S); |
| 40 | double qv = V * (1 - S * f); |
| 41 | double tv = V * (1 - S * (1 - f)); |
| 42 | switch (i) { |
| 43 | //Red is dominant color |
| 44 | case 0: |
| 45 | R = V; |
| 46 | G = tv; |
| 47 | B = pv; |
| 48 | break; |
| 49 | |
| 50 | //Green is dominant color |
| 51 | case 1: |
| 52 | R = qv; |
| 53 | G = V; |
| 54 | B = pv; |
| 55 | break; |
| 56 | case 2: |
| 57 | R = pv; |
| 58 | G = V; |
| 59 | B = tv; |
| 60 | break; |
| 61 | |
| 62 | //Blue is dominant color |
| 63 | case 3: |
| 64 | R = pv; |
| 65 | G = qv; |
| 66 | B = V; |
| 67 | break; |
| 68 | case 4: |
| 69 | R = tv; |
| 70 | G = pv; |
| 71 | B = V; |
| 72 | break; |
| 73 | |
| 74 | //Red is dominant color |
| 75 | case 5: |
| 76 | R = V; |
| 77 | G = pv; |
| 78 | B = qv; |
| 79 | break; |
| 80 | |
| 81 | //Back-up case statements, in case our math is wrong |
| 82 | case 6: |
| 83 | R = V; |
| 84 | G = tv; |
| 85 | B = pv; |
| 86 | break; |
| 87 | case -1: |
| 88 | R = V; |
| 89 | G = pv; |
| 90 | B = qv; |
| 91 | break; |
| 92 | |
| 93 | //Color is not defined |
| 94 | default: |
| 95 | //pretend color is black and white |
| 96 | R = G = B = V; |
| 97 | break; |
| 98 | } |
| 99 | } |
| 100 | *r = (float) R; |
| 101 | *g = (float) G; |
| 102 | *b = (float) B; |
| 103 | } |
| 104 | } |
| 105 | } |
| 106 | #endif |