Make rgb565 conversion algorithms a bit better

src/framebuffer/common.rs

@@ -60,26 +60,26 @@ impl color {
     }
 
     pub fn to_rgb8(self) -> [u8; 3] {
-        // Components reversed because of the device
-        let components = self.as_native();
+        let rgb565 = u16::from_le_bytes(self.as_native());
 
-        let mut combined: u16 = u16::from(components[1]) << 8;
-        combined |= u16::from(components[0]);
+        let r5 = rgb565 >> 11 & 0b11111;
+        let g6 = rgb565 >> 5 & 0b111111;
+        let b5 = rgb565 & 0b11111;
 
-        let red = (((combined & 0b1111_1000_0000_0000) >> 11) << 3) as u8;
-        let green = (((combined & 0b0000_0111_1110_0000) >> 5) << 2) as u8;
-        let blue = ((combined & 0b0000_0000_0001_1111) << 3) as u8;
+        let r8 = (r5 * 255 / 0b11111) as u8;
+        let g8 = (g6 * 255 / 0b111111) as u8;
+        let b8 = (b5 * 255 / 0b11111) as u8;
 
-        [red, green, blue]
+        [r8, g8, b8]
     }
 
     #[inline]
     pub fn as_native(self) -> [u8; 2] {
         match self {
             color::BLACK => [0x00, 0x00],
-            color::RED => [0x07, 0xE0],
-            color::GREEN => [0x00, 0x1F],
-            color::BLUE => [0xF8, 0x00],
+            color::RED => [0x00, 0xF8],
+            color::GREEN => [0xE0, 0x07],
+            color::BLUE => [0x1F, 0x00],
             color::WHITE => [0xFF, 0xFF],
             color::GRAY(level) => color::rgb_to_native(255 - level, 255 - level, 255 - level),
             color::NATIVE_COMPONENTS(c1, c2) => [c1, c2],
@@ -97,14 +97,37 @@ impl color {
         //    green   : offset = 5,   length =6,      msb_right = 0
         //    blue    : offset = 0,   length =5,      msb_right = 0
         //
-        let r5 = (u16::from(r8) >> 3) as u8;
-        let g6 = (u16::from(g8) >> 2) as u8;
-        let b5 = (u16::from(b8) >> 3) as u8;
-
-        [
-            (((g6 & 0b00_0111) << 5) | b5),
-            ((r5 << 3) | ((g6 & 0b11_1000) >> 3)),
-        ]
+        let r5 = (r8 as u16 + 1) * 0b11111 / 255;
+        let g6 = (g8 as u16 + 1) * 0b111111 / 255;
+        let b5 = (b8 as u16 + 1) * 0b11111 / 255;
+
+        let rgb565 = r5 << 11 | g6 << 5 | b5;
+
+        rgb565.to_le_bytes()
+    }
+}
+
+#[test]
+fn rgb565_conversions() {
+    // Ensure that min and max values are transformed faithfully
+    assert_eq!(color::RGB(0, 0, 0).to_rgb565(), [0, 0]);
+    assert_eq!(color::RGB(255, 255, 255).to_rgb565(), [255, 255]);
+    assert_eq!(color::GRAY(0).to_rgb565(), [255, 255]);
+    assert_eq!(color::GRAY(255).to_rgb565(), [0, 0]);
+
+    assert_eq!(color::from_native([0, 0]).to_rgb8(), [0, 0, 0]);
+    assert_eq!(color::from_native([255, 255]).to_rgb8(), [255, 255, 255]);
+    assert_eq!(color::BLUE.to_rgb8(), [0, 0, 255]);
+    assert_eq!(color::GREEN.to_rgb8(), [0, 255, 0]);
+    assert_eq!(color::RED.to_rgb8(), [255, 0, 0]);
+    assert_eq!(color::RGB(255, 127, 0).to_rgb8(), [255, 125, 0]);
+
+    // Ensure that every single RGB565 value can be transformed to RGB8 and back losslessly
+    for native in 0..u16::MAX {
+        let [lo, hi] = native.to_le_bytes();
+        let [r, g, b] = color::NATIVE_COMPONENTS(lo, hi).to_rgb8();
+
+        assert_eq!(color::RGB(r, g, b).to_rgb565(), [lo, hi]);
     }
 }