pbio/color: Add new hsv bicone distance function.

This new distance function estimates the similarity of two hsv
colors by calculating their euclidean distance when mapped into a
Hue-Chroma-Lightness bicone. This is much more robust for realistic
colors, especially when low saturation or low value is involved.

CHANGELOG.md

@@ -7,6 +7,12 @@
 - Fixed Technic (Extra) Large motors not working ([support#1131]) on all hubs.
 - Fixed Powered Up Light not working ([support#1131]) on all hubs.
 
+### Changed
+
+- New color distance function used by the color sensors that is more
+  consistent when distinguishing user-provided colors ([pybricks-micropython#104]).
+
+[pybricks-micropython#104]: https://github.com/pybricks/pybricks-micropython/pull/104
 [support#1064]: https://github.com/pybricks/support/issues/1131
 
 ## [3.3.0b7] - 2023-06-30

bricks/_common/sources.mk

@@ -191,6 +191,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/include/pbio/color.h

@@ -118,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/src/color/util.c

@@ -0,0 +1,65 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2018-2022 The Pybricks Authors
+
+#include <pbio/color.h>
+
+// parabola approximating the first 90 degrees of sine. (0,90) to (0, 10000)
+static int32_t sin_deg_branch0(int32_t x) {
+    return (201 - x) * x;
+}
+
+// integer sine approximation from degrees to (-10000, 10000)
+static int32_t sin_deg(int32_t x) {
+    x = x % 360;
+    if (x < 90) {
+        return sin_deg_branch0(x);
+    }
+    if (x < 180) {
+        return sin_deg_branch0(180 - x);
+    }
+    if (x < 270) {
+        return -sin_deg_branch0(x - 180);
+    }
+    return -sin_deg_branch0(360 - x);
+}
+
+static int32_t cos_deg(int32_t x) {
+    return sin_deg(x + 90);
+}
+
+/**
+ * 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) {
+
+    int32_t a_h = hsv_a->h;
+    int32_t a_s = hsv_a->s;
+    int32_t a_v = hsv_a->v;
+
+    int32_t b_h = hsv_b->h;
+    int32_t b_s = hsv_b->s;
+    int32_t b_v = hsv_b->v;
+
+    // chroma (= radial coordinate in bicone) of a and b (0-10000)
+    int32_t radius_a = a_v * a_s;
+    int32_t radius_b = b_v * b_s;
+
+    // lightness (= z-coordinate in bicone) of a and b (0-20000)
+    int32_t lightness_a = (200 * a_v - a_s * a_v);
+    int32_t lightness_b = (200 * b_v - b_s * b_v);
+
+    // x and y deltas of a and b in HSV bicone (-20000, 20000)
+    int32_t delx = (radius_b * cos_deg(b_h) - radius_a * cos_deg(a_h)) / 10000;
+    int32_t dely = (radius_b * sin_deg(b_h) - radius_a * sin_deg(a_h)) / 10000;
+    // z delta of a and b in HSV bicone (-20000, 20000)
+    int32_t delz = (lightness_b - lightness_a);
+
+    // Squared Euclidean distance (0, 400000000)
+    int32_t cdist = delx * delx + dely * dely + delz * delz;
+
+    return cdist;
+}

lib/pbio/test/src/test_color.c

@@ -357,11 +357,264 @@ static void test_color_hsv_compression(void *env) {
     tt_want_int_op(hsv.v, ==, expanded.v);
 }
 
+static void test_color_hsv_cost(void *env) {
+    pbio_color_hsv_t color_a;
+    pbio_color_hsv_t color_b;
+    int32_t dist;
+
+    // color compared to itself should give 0
+    color_a.h = 0;
+    color_a.s = 100;
+    color_a.v = 100;
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_a), ==, 0);
+
+    // blacks with different saturations/hues should be the same
+    color_a.h = 230;
+    color_a.s = 23;
+    color_a.v = 0;
+
+    color_b.h = 23;
+    color_b.s = 99;
+    color_b.v = 0;
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), ==, 0);
+
+    // colors with different hues should be different when value>0 and saturation>0
+    color_a.h = 230;
+    color_a.s = 99;
+    color_a.v = 100;
+
+    color_b.h = 23;
+    color_b.s = 99;
+    color_b.v = 100;
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0);
+
+    // grays with different hues should be the same
+    color_a.h = 230;
+    color_a.s = 0;
+    color_a.v = 50;
+
+    color_b.h = 23;
+    color_b.s = 0;
+    color_b.v = 50;
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), ==, 0);
+
+    // distance should be greater when saturation is greater
+    color_a.h = 30;
+    color_a.s = 20;
+    color_a.v = 70;
+
+    color_b.h = 60;
+    color_b.s = 20;
+    color_b.v = 70;
+
+    dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+
+    color_a.h = 30;
+    color_a.s = 40;
+    color_a.v = 70;
+
+    color_b.h = 60;
+    color_b.s = 40;
+    color_b.v = 70;
+
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, dist);
+
+    // resolve colors that are close
+    color_a.h = 30;
+    color_a.s = 20;
+    color_a.v = 70;
+
+    color_b.h = 35;
+    color_b.s = 20;
+    color_b.v = 70;
+
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0);
+
+    color_a.h = 30;
+    color_a.s = 20;
+    color_a.v = 70;
+
+    color_b.h = 30;
+    color_b.s = 25;
+    color_b.v = 70;
+
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0);
+
+    color_a.h = 30;
+    color_a.s = 20;
+    color_a.v = 70;
+
+    color_b.h = 30;
+    color_b.s = 20;
+    color_b.v = 75;
+
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, 0);
+
+    // hues 360 and 0 should be the same
+    color_a.h = 360;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 0;
+    color_b.s = 100;
+    color_b.v = 100;
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), ==, 0);
+
+    // distance between hues 359 and 1 should be smaller than hues 1 and 5
+    color_a.h = 359;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 1;
+    color_b.s = 100;
+    color_b.v = 100;
+    dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+
+    color_a.h = 1;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 5;
+    color_b.s = 100;
+    color_b.v = 100;
+
+    tt_want_int_op(pbio_color_get_bicone_squared_distance(&color_a, &color_b), >, dist);
+
+    // check distance is monotonous along several color paths. This should catch potential int overflows
+    int prev_dist = 0;
+    bool monotone = true;
+
+    // along saturation
+    color_a.h = 180;
+    color_a.s = 0;
+    color_a.v = 100;
+
+    color_b.h = 180;
+    color_b.s = 0;
+    color_b.v = 100;
+
+    while (color_a.s < 100) {
+        color_a.s += 5;
+        dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+
+        if (dist <= prev_dist) {
+            monotone = false;
+            break;
+        }
+        prev_dist = dist;
+    }
+    tt_want(monotone);
+
+    // along value
+
+    prev_dist = 0;
+    monotone = true;
+
+    color_a.h = 180;
+    color_a.s = 100;
+    color_a.v = 0;
+
+    color_b.h = 180;
+    color_b.s = 100;
+    color_b.v = 0;
+
+    while (color_a.v < 100) {
+        color_a.v += 5;
+        dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+
+        if (dist <= prev_dist) {
+            monotone = false;
+            break;
+        }
+        prev_dist = dist;
+    }
+    tt_want(monotone);
+
+    // along value, saturation 0
+
+    prev_dist = 0;
+    monotone = true;
+
+    color_a.h = 180;
+    color_a.s = 0;
+    color_a.v = 0;
+
+    color_b.h = 180;
+    color_b.s = 0;
+    color_b.v = 0;
+
+    while (color_a.v < 100) {
+        color_a.v += 5;
+        dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+
+        if (dist <= prev_dist) {
+            monotone = false;
+            break;
+        }
+        prev_dist = dist;
+    }
+    tt_want(monotone);
+
+    // along chroma
+
+    prev_dist = 0;
+    monotone = true;
+
+    color_a.h = 180;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 180;
+    color_b.s = 100;
+    color_b.v = 100;
+
+    for (int i = -19; i < 21; i++) {
+        color_a.s = i < 0 ? -i * 5 : i * 5;
+        color_a.h = i < 0 ? 180 : 0;
+        color_a.v = 10000 / (200 - color_a.s); // constant lightness
+
+        dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+
+        if (dist <= prev_dist) {
+            monotone = false;
+        }
+        prev_dist = dist;
+    }
+    tt_want(monotone);
+
+    // check max distances
+
+    color_a.h = 0;
+    color_a.s = 100;
+    color_a.v = 100;
+
+    color_b.h = 180;
+    color_b.s = 100;
+    color_b.v = 100;
+
+    dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+    tt_want_int_op(dist, >, 390000000);
+    tt_want_int_op(dist, <, 410000000);
+
+    color_a.h = 0;
+    color_a.s = 0;
+    color_a.v = 0;
+
+    color_b.h = 0;
+    color_b.s = 0;
+    color_b.v = 100;
+
+    dist = pbio_color_get_bicone_squared_distance(&color_a, &color_b);
+    tt_want_int_op(dist, >, 390000000);
+    tt_want_int_op(dist, <, 410000000);
+}
+
 struct testcase_t pbio_color_tests[] = {
     PBIO_TEST(test_rgb_to_hsv),
     PBIO_TEST(test_hsv_to_rgb),
     PBIO_TEST(test_color_to_hsv),
     PBIO_TEST(test_color_to_rgb),
     PBIO_TEST(test_color_hsv_compression),
+    PBIO_TEST(test_color_hsv_cost),
     END_OF_TESTCASES
 };