HCL bicone color distance function (without libfixmath)

CHANGELOG.md

@@ -8,11 +8,14 @@
 - The `use_gyro` method is added to the normal `DriveBase` class instead of
   having a separate `GyroDriveBase` class. Since the latter was only released
   in beta versions, this is not a breaking change ([support#1054]).
+- New color distance function used by the color sensors that is more
+  consistent when distinguishing user-provided colors ([pybricks-micropython#104]).
 
 ### Fixed
 - Improved external device detection speed ([support#1140]).
 - Fixed Powered Up Tilt Sensor not working  ([support#1189]).
 
+[pybricks-micropython#104]: https://github.com/pybricks/pybricks-micropython/pull/104
 [support#1054]: https://github.com/pybricks/support/issues/1054
 [support#1140]: https://github.com/pybricks/support/issues/1140
 [support#1189]: https://github.com/pybricks/support/issues/1189

bricks/_common/sources.mk

@@ -190,6 +190,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\
 	src/angle.c \
 	src/battery.c \
 	src/color/conversion.c \
+	src/color/util.c \
 	src/control.c \
 	src/control_settings.c \
 	src/dcmotor.c \

lib/pbio/drv/led/led_pwm.c

@@ -48,7 +48,7 @@ static pbio_error_t pbdrv_led_pwm_set_hsv(pbdrv_led_dev_t *dev, const pbio_color
     uint32_t Y = ((color->r_brightness * r + color->g_brightness * g + color->b_brightness * b) >> 12) + 1;
 
     // Reapply V from HSV for brightness. V is squared for gamma correction.
-    uint32_t scale_factor = hsv->v * hsv->v * pdata->scale_factor / 128;
+    uint32_t scale_factor = pbio_color_hsv_get_v(hsv) * pbio_color_hsv_get_v(hsv) * pdata->scale_factor / 128;
 
     r = r * scale_factor / Y;
     g = g * scale_factor / Y;

lib/pbio/drv/led/led_virtual.c

@@ -26,7 +26,7 @@
 static pbio_error_t pbdrv_led_virtual_set_hsv(pbdrv_led_dev_t *dev, const pbio_color_hsv_t *hsv) {
     uint8_t id = (intptr_t)dev->pdata;
 
-    return pbdrv_virtual_call_method("led", id, "on_set_hsv", "IBBB", pbdrv_clock_get_us(), hsv->h, hsv->s, hsv->v);
+    return pbdrv_virtual_call_method("led", id, "on_set_hsv", "IBBB", pbdrv_clock_get_us(), hsv->h, hsv->s, pbio_color_hsv_get_v(hsv));
 }
 
 static const pbdrv_led_funcs_t pbdrv_led_virtual_funcs = {

lib/pbio/include/pbio/color.h

@@ -92,18 +92,24 @@ typedef struct {
     uint16_t h;
     /** The saturation component. 0 to 100 percent. */
     uint8_t s;
-    /** The value component. 0 to 100 percent. */
-    uint8_t v;
+    /** The value component. Normally 0 to 100 percent but allowed to be
+     * negative to provide higher contrast in color scanning applications. */
+    int8_t v;
 } pbio_color_hsv_t;
 
+static inline uint8_t pbio_color_hsv_get_v(const pbio_color_hsv_t *hsv) {
+    return hsv->v < 0 ? 0 : hsv->v;
+}
+
 /** Compressed HSV color. Stores data in 24 bytes instead of 32. */
 typedef struct __attribute__((__packed__)) {
     /** The hue component. 0 to 359 degrees. */
     uint16_t h : 9;
     /** The saturation component. 0 to 100 percent. */
     uint8_t s : 7;
-    /** The value component. 0 to 100 percent. */
-    uint8_t v;
+    /** The value component. Normally 0 to 100 percent but allowed to be
+     * negative to provide higher contrast in color scanning applications. */
+    int8_t v;
 } pbio_color_compressed_hsv_t;
 
 void pbio_color_rgb_to_hsv(const pbio_color_rgb_t *rgb, pbio_color_hsv_t *hsv);
@@ -112,6 +118,7 @@ void pbio_color_to_hsv(pbio_color_t color, pbio_color_hsv_t *hsv);
 void pbio_color_to_rgb(pbio_color_t color, pbio_color_rgb_t *rgb);
 void pbio_color_hsv_compress(const pbio_color_hsv_t *hsv, pbio_color_compressed_hsv_t *compressed);
 void pbio_color_hsv_expand(const pbio_color_compressed_hsv_t *compressed, pbio_color_hsv_t *hsv);
+int32_t pbio_color_get_bicone_squared_distance(const pbio_color_hsv_t *hsv_a, const pbio_color_hsv_t *hsv_b);
 
 #endif // _PBIO_COLOR_H_
 

lib/pbio/include/pbio/int_math.h

@@ -35,6 +35,8 @@ int32_t pbio_int_math_sign(int32_t a);
 int32_t pbio_int_math_atan2(int32_t y, int32_t x);
 int32_t pbio_int_math_mult_then_div(int32_t a, int32_t b, int32_t c);
 int32_t pbio_int_math_sqrt(int32_t n);
+int32_t pbio_int_math_sin_deg(int32_t x);
+int32_t pbio_int_math_cos_deg(int32_t x);
 
 #endif // _PBIO_INT_MATH_H_
 

lib/pbio/include/pbio/light.h

@@ -29,7 +29,8 @@ typedef struct _pbio_color_light_t pbio_color_light_t;
     { .h = (hue), .s = (saturation), .v = (value) }
 
 /** Sentinel value for a color light animation array. */
-#define PBIO_COLOR_LIGHT_ANIMATION_END { .v = UINT8_MAX }
+#define PBIO_COLOR_LIGHT_ANIMATION_END_V (INT8_MAX)
+#define PBIO_COLOR_LIGHT_ANIMATION_END_HSV { .v = PBIO_COLOR_LIGHT_ANIMATION_END_V }
 
 #if PBIO_CONFIG_LIGHT
 

lib/pbio/src/color/conversion.c

@@ -105,7 +105,7 @@ void pbio_color_hsv_to_rgb(const pbio_color_hsv_t *hsv, pbio_color_rgb_t *rgb) {
     // the output more visually linear.
 
     // Scale 0..100 percent to 0..255
-    uint8_t value = 327 * hsv->v / 128;
+    uint8_t value = 327 * pbio_color_hsv_get_v(hsv) / 128;
     uint8_t saturation = 327 * hsv->s / 128;
 
     // The brightness floor is minimum number that all of

lib/pbio/src/color/util.c

@@ -0,0 +1,38 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2018-2022 The Pybricks Authors
+
+#include <pbio/color.h>
+#include <pbio/int_math.h>
+
+/**
+ * Gets squared Euclidean distance between HSV colors mapped into a
+ * chroma-lightness-bicone. The bicone is 20000 units tall and 20000 units in
+ * diameter.
+ *
+ * @param [in]  hsv_a    The first HSV color.
+ * @param [in]  hsv_b    The second HSV color.
+ * @returns              Squared distance (0 to 400000000).
+ */
+int32_t pbio_color_get_bicone_squared_distance(const pbio_color_hsv_t *hsv_a, const pbio_color_hsv_t *hsv_b) {
+
+    // Chroma (= radial coordinate in bicone) of a and b (0-10000).
+    int32_t radius_a = pbio_color_hsv_get_v(hsv_a) * hsv_a->s;
+    int32_t radius_b = pbio_color_hsv_get_v(hsv_b) * hsv_b->s;
+
+    // Lightness (= z-coordinate in bicone) of a and b (0-20000).
+    // v is allowed to be negative, resulting in negative lightness.
+    // This can be used to create a higher contrast between "none-color" and
+    // normal colors.
+    int32_t lightness_a = (200 - hsv_a->s) * hsv_a->v;
+    int32_t lightness_b = (200 - hsv_b->s) * hsv_b->v;
+
+    // z delta of a and b in HSV bicone (-20000, 20000).
+    int32_t delta_z = (lightness_b - lightness_a);
+
+    // x and y deltas of a and b in HSV bicone (-20000, 20000)
+    int32_t delta_x = (radius_b * pbio_int_math_cos_deg(hsv_b->h) - radius_a * pbio_int_math_cos_deg(hsv_a->h)) / 10000;
+    int32_t delta_y = (radius_b * pbio_int_math_sin_deg(hsv_b->h) - radius_a * pbio_int_math_sin_deg(hsv_a->h)) / 10000;
+
+    // Squared Euclidean distance (0, 400000000)
+    return delta_x * delta_x + delta_y * delta_y + delta_z * delta_z;
+}

lib/pbio/src/int_math.c

@@ -294,3 +294,46 @@ int32_t pbio_int_math_mult_then_div(int32_t a, int32_t b, int32_t c) {
     assert(result == (int64_t)a * (int64_t)b / (int64_t)c);
     return result;
 }
+
+/**
+ * Approximates first 90-degree segment of a sine in degrees, output
+ * upscaled by 10000.
+ *
+ * @param [in]  x        Angle in degrees (0-90).
+ * @returns              Approximately sin(x) * 10000.
+ */
+static int32_t pbio_int_math_sin_deg_branch0(int32_t x) {
+    return (201 - x) * x;
+}
+
+// integer sine approximation from degrees to (-10000, 10000)
+
+/**
+ * Approximates sine of an angle in degrees, output upscaled by 10000.
+ *
+ * @param [in]  x        Angle in degrees.
+ * @returns              Approximately sin(x) * 10000.
+ */
+int32_t pbio_int_math_sin_deg(int32_t x) {
+    x = x % 360;
+    if (x < 90) {
+        return pbio_int_math_sin_deg_branch0(x);
+    }
+    if (x < 180) {
+        return pbio_int_math_sin_deg_branch0(180 - x);
+    }
+    if (x < 270) {
+        return -pbio_int_math_sin_deg_branch0(x - 180);
+    }
+    return -pbio_int_math_sin_deg_branch0(360 - x);
+}
+
+/**
+ * Approximates cosine of an angle in degrees, output upscaled by 10000.
+ *
+ * @param [in]  x        Angle in degrees.
+ * @returns              Approximately cos(x) * 10000.
+ */
+int32_t pbio_int_math_cos_deg(int32_t x) {
+    return pbio_int_math_sin_deg(x + 90);
+}

lib/pbio/src/light/color_light.c

@@ -147,7 +147,7 @@ static uint32_t pbio_color_light_animate_next(pbio_light_animation_t *animation)
     const pbio_color_compressed_hsv_t *cell = &cells[light->current_cell++];
 
     // if we have reached the array terminator, start back at the beginning
-    if (cell->v == UINT8_MAX) {
+    if (cell->v == PBIO_COLOR_LIGHT_ANIMATION_END_V) {
         cell = &cells[0];
         light->current_cell = 1;
     }
@@ -169,7 +169,7 @@ static uint32_t pbio_color_light_animate_next(pbio_light_animation_t *animation)
  *
  * @param [in]  light       The light instance
  * @param [in]  interval    The the time intervale between animation cells in milliseconds
- * @param [in]  cells       Array of up to 65536 animation cells ending with ::PBIO_COLOR_LIGHT_ANIMATION_END
+ * @param [in]  cells       Array of up to 65536 animation cells ending with ::PBIO_COLOR_LIGHT_ANIMATION_END_HSV
  */
 void pbio_color_light_start_animation(pbio_color_light_t *light, uint16_t interval, const pbio_color_compressed_hsv_t *cells) {
     pbio_color_light_stop_animation(light);