util_pb/pb_color_map: use HSV cone distance as error

.vscode/c_cpp_properties.json

@@ -54,7 +54,9 @@
                 "STM32_H=<stm32f4xx.h>",
                 "STM32_HAL_H=<stm32f4xx_hal.h>",
                 "USE_FULL_LL_DRIVER",
-                "FIXMATH_NO_CTYPE"
+                "FIXMATH_NO_CTYPE",
+                "FIXMATH_NO_CACHE",
+                "FIXMATH_FAST_SIN"
             ],
             "compilerPath": "${gcc10ArmNoneEabi}",
             "compilerArgs": [
@@ -97,7 +99,9 @@
                 "STM32_H=<stm32f0xx.h>",
                 "STM32_HAL_H=<stm32f0xx_hal.h>",
                 "USE_FULL_LL_DRIVER",
-                "FIXMATH_NO_CTYPE"
+                "FIXMATH_NO_CTYPE",
+                "FIXMATH_NO_CACHE",
+                "FIXMATH_FAST_SIN"
             ],
             "compilerPath": "${gcc10ArmNoneEabi}",
             "compilerArgs": [
@@ -138,7 +142,9 @@
                 "STM32_H=<stm32f0xx.h>",
                 "STM32_HAL_H=<stm32f0xx_hal.h>",
                 "USE_FULL_LL_DRIVER",
-                "FIXMATH_NO_CTYPE"
+                "FIXMATH_NO_CTYPE",
+                "FIXMATH_NO_CACHE",
+                "FIXMATH_FAST_SIN"
             ],
             "compilerPath": "${gcc10ArmNoneEabi}",
             "compilerArgs": [
@@ -183,7 +189,9 @@
                 "STM32_H=<stm32l4xx.h>",
                 "STM32_HAL_H=<stm32l4xx_hal.h>",
                 "USE_FULL_LL_DRIVER",
-                "FIXMATH_NO_CTYPE"
+                "FIXMATH_NO_CTYPE",
+                "FIXMATH_NO_CACHE",
+                "FIXMATH_FAST_SIN"
             ],
             "compilerPath": "${gcc10ArmNoneEabi}",
             "compilerArgs": [
@@ -234,7 +242,9 @@
                 "STM32_H=<stm32f4xx.h>",
                 "STM32_HAL_H=<stm32f4xx_hal.h>",
                 "USE_FULL_LL_DRIVER",
-                "FIXMATH_NO_CTYPE"
+                "FIXMATH_NO_CTYPE",
+                "FIXMATH_NO_CACHE",
+                "FIXMATH_FAST_SIN"
             ],
             "compilerPath": "${gcc10ArmNoneEabi}",
             "compilerArgs": [
@@ -271,7 +281,9 @@
             ],
             "defines": [
                 "MICROPY_ROM_TEXT_COMPRESSION",
-                "FIXMATH_NO_CTYPE"
+                "FIXMATH_NO_CTYPE",
+                "FIXMATH_NO_CACHE",
+                "FIXMATH_FAST_SIN"
             ],
             "compilerPath": "${gcc10ArmNoneEabi}",
             "compilerArgs": [
@@ -305,7 +317,9 @@
                 "STM32F070xB",
                 "STM32_H=<stm32f4xx.h>",
                 "STM32_HAL_H=<stm32f4xx_hal.h>",
-                "FIXMATH_NO_CTYPE"
+                "FIXMATH_NO_CTYPE",
+                "FIXMATH_NO_CACHE",
+                "FIXMATH_FAST_SIN"
             ],
             "compilerPath": "${gcc10ArmNoneEabi}",
             "compilerArgs": [

bricks/ev3dev/Makefile

@@ -307,6 +307,7 @@ PYBRICKS_LIB_SRC_C = $(addprefix lib/,\
 	ev3dev/src/ev3dev_stretch/sysfs.c \
 	libfixmath/libfixmath/fix16_sqrt.c \
 	libfixmath/libfixmath/fix16_str.c \
+	libfixmath/libfixmath/fix16_trig.c \
 	libfixmath/libfixmath/fix16.c \
 	libfixmath/libfixmath/uint32.c \
 	pbio/drv/battery/battery_linux_ev3.c \
@@ -320,6 +321,7 @@ PYBRICKS_LIB_SRC_C = $(addprefix lib/,\
 	pbio/platform/motors/settings.c \
 	pbio/src/battery.c \
 	pbio/src/color/conversion.c \
+	pbio/src/color/util.c \
 	pbio/src/control.c \
 	pbio/src/dcmotor.c \
 	pbio/src/drivebase.c \

bricks/nxt/Makefile

@@ -65,7 +65,7 @@ INC += -I../../lib/pbio
 INC += -I../..
 INC += -I$(BUILD)
 
-COPT += -DFIXMATH_NO_CTYPE
+COPT += -DFIXMATH_NO_CTYPE -DFIXMATH_NO_CACHE -DFIXMATH_FAST_SIN
 
 CFLAGS_MCU = -mthumb -mtune=arm7tdmi -mcpu=arm7tdmi -msoft-float
 CFLAGS = $(INC) -Wall -Werror -std=c99 -nostdlib -fshort-enums $(CFLAGS_MCU) $(COPT) $(CFLAGS_EXTRA)
@@ -188,6 +188,7 @@ CONTIKI_SRC_C = $(addprefix lib/contiki-core/,\
 LIBFIXMATH_SRC_C = $(addprefix lib/libfixmath/libfixmath/,\
 	fix16_sqrt.c \
 	fix16_str.c \
+	fix16_trig.c \
 	fix16.c \
 	uint32.c \
 	)
@@ -206,6 +207,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\
 	platform/$(PBIO_PLATFORM)/sys.c \
 	src/battery.c \
 	src/color/conversion.c \
+	src/color/util.c \
 	src/control.c \
 	src/dcmotor.c \
 	src/drivebase.c \

bricks/stm32/stm32.mk

@@ -406,11 +406,12 @@ LWRB_SRC_C = lib/lwrb/src/lwrb/lwrb.c
 
 # libfixmath
 
-COPT += -DFIXMATH_NO_CTYPE
+COPT += -DFIXMATH_NO_CTYPE -DFIXMATH_NO_CACHE -DFIXMATH_FAST_SIN
 
 LIBFIXMATH_SRC_C = $(addprefix lib/libfixmath/libfixmath/,\
 	fix16_sqrt.c \
 	fix16_str.c \
+	fix16_trig.c \
 	fix16.c \
 	uint32.c \
 	)
@@ -463,6 +464,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\
 	platform/$(PBIO_PLATFORM)/sys.c \
 	src/battery.c \
 	src/color/conversion.c \
+	src/color/util.c \
 	src/control.c \
 	src/dcmotor.c \
 	src/drivebase.c \

lib/pbio/include/pbio/color.h

@@ -13,6 +13,8 @@
 
 #include <stdint.h>
 
+#include <fixmath.h>
+
 /** @cond INTERNAL */
 
 /**
@@ -106,12 +108,27 @@ typedef struct __attribute__((__packed__)) {
     uint8_t v;
 } pbio_color_compressed_hsv_t;
 
+/** HSV color with fix16 components */
+typedef struct {
+    /** The hue component. radians. */
+    fix16_t h;
+    /** The saturation component. 0 to 1. */
+    fix16_t s;
+    /** The value component. 0 to 1. */
+    fix16_t v;
+} pbio_color_hsv_fix16_t;
+
 void pbio_color_rgb_to_hsv(const pbio_color_rgb_t *rgb, pbio_color_hsv_t *hsv);
 void pbio_color_hsv_to_rgb(const pbio_color_hsv_t *hsv, pbio_color_rgb_t *rgb);
 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);
+void pbio_color_hsv_to_fix16(const pbio_color_hsv_t *hsv, pbio_color_hsv_fix16_t *hsv_fix16);
+
+int32_t pbio_get_hsv_cost(const pbio_color_hsv_t *x, const pbio_color_hsv_t *c);
+int32_t pbio_get_cone_cost(const pbio_color_hsv_t *a, const pbio_color_hsv_t *b, const int32_t chroma_weight);
+int32_t pbio_get_bicone_cost(const pbio_color_hsv_t *a, const pbio_color_hsv_t *b, const int32_t chroma_weight);
 
 #endif // _PBIO_COLOR_H_
 

lib/pbio/src/color/conversion.c

@@ -2,6 +2,8 @@
 // Copyright (c) 2020 The Pybricks Authors
 // Copyright (c) 2013 FastLED
 
+#include <fixmath.h>
+
 #include <pbio/color.h>
 
 /**
@@ -233,3 +235,18 @@ void pbio_color_hsv_expand(const pbio_color_compressed_hsv_t *compressed, pbio_c
     hsv->s = compressed->s;
     hsv->v = compressed->v;
 }
+
+/**
+ * Converts HSV to normalized HSV for fixed point operations.
+ *
+ * @param [in]  hsv         The source HSV color value.
+ * @param [out] hsv_fix16   The destination HSV_fix16 color value.
+ */
+void pbio_color_hsv_to_fix16(const pbio_color_hsv_t *hsv, pbio_color_hsv_fix16_t *hsv_fix16) {
+    const fix16_t by100 = F16C(0,0100);
+
+    // normalize h to radians, s/v to (0,1)
+    hsv_fix16->h = fix16_deg_to_rad(fix16_from_int(hsv->h));
+    hsv_fix16->s = fix16_mul(fix16_from_int(hsv->s), by100);
+    hsv_fix16->v = fix16_mul(fix16_from_int(hsv->v), by100);
+}

lib/pbio/src/color/util.c

@@ -0,0 +1,115 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2018-2022 The Pybricks Authors
+
+#include <fixmath.h>
+
+#include <pbio/color.h>
+
+// Cost function between two colors a and b. The lower, the closer they are.
+int32_t pbio_get_hsv_cost(const pbio_color_hsv_t *x, const pbio_color_hsv_t *c) {
+
+    // Calculate the hue error
+    int32_t hue_error;
+
+    if (c->s <= 5 || x->s <= 5) {
+        // When comparing against unsaturated colors,
+        // the hue error is not so relevant.
+        hue_error = 0;
+    } else {
+        hue_error = c->h > x->h ? c->h - x->h : x->h - c->h;
+        if (hue_error > 180) {
+            hue_error = 360 - hue_error;
+        }
+    }
+
+    // Calculate the value error:
+    int32_t value_error = x->v > c->v ? x->v - c->v : c->v - x->v;
+
+    // Calculate the saturation error, with extra penalty for low saturation
+    int32_t saturation_error = x->s > c->s ? x->s - c->s : c->s - x->s;
+    saturation_error += (100 - c->s) / 2;
+
+    // Total error
+    return hue_error * hue_error + 5 * saturation_error * saturation_error + 2 * value_error * value_error;
+}
+
+// gets squared cartesian distance between hsv colors mapped into a chroma-value-cone
+int32_t pbio_get_cone_cost(const pbio_color_hsv_t *hsv_a, const pbio_color_hsv_t *hsv_b, const int32_t chroma_weight) {
+
+    pbio_color_hsv_fix16_t a, b;
+    pbio_color_hsv_to_fix16(hsv_a, &a);
+    pbio_color_hsv_to_fix16(hsv_b, &b);
+
+    // radial coordinates of a and b
+    fix16_t radius_a = fix16_mul(fix16_sub(fix16_one, fix16_sq(fix16_sub(fix16_one, a.v))), a.s);
+    fix16_t radius_b = fix16_mul(fix16_sub(fix16_one, fix16_sq(fix16_sub(fix16_one, b.v))), b.s);
+
+    // x, y and z deltas between cartesian coordinates of a and b in HSV cone
+    // delx = radius_b*cos(b_h) - radius_a*cos(a_h)
+    fix16_t delx = fix16_sub(
+        fix16_mul(radius_b, fix16_cos(b.h)),
+        fix16_mul(radius_a, fix16_cos(a.h)));
+
+    // dely = radius_b*sin(b_h) - radius_a*sin(a_h)
+    fix16_t dely = fix16_sub(
+        fix16_mul(radius_b, fix16_sin(b.h)),
+        fix16_mul(radius_a, fix16_sin(a.h)));
+
+    // delz = cone_height * (b_v - a_v)
+    fix16_t delz = fix16_sub(b.v, a.v);
+
+    // cdist = chroma_weight*(delx*delx + dely*dely) + (100-chroma_weight)*delz*delz
+    fix16_t cdist = fix16_add(
+        fix16_mul(
+            fix16_from_int(100 * chroma_weight),
+            fix16_add(
+                fix16_sq(delx),
+                fix16_sq(dely))),
+        fix16_mul(
+            fix16_from_int(10000 - 100 * chroma_weight),
+            fix16_sq(delz)));
+
+    return fix16_to_int(cdist);
+}
+
+// gets squared cartesian distance between hsv colors mapped into a chroma-lighness-bicone
+int32_t pbio_get_bicone_cost(const pbio_color_hsv_t *hsv_a, const pbio_color_hsv_t *hsv_b, const int32_t chroma_weight) {
+
+    pbio_color_hsv_fix16_t a, b;
+    pbio_color_hsv_to_fix16(hsv_a, &a);
+    pbio_color_hsv_to_fix16(hsv_b, &b);
+
+    // radial coordinates of a and b
+    fix16_t radius_a = fix16_mul(fix16_mul(a.v, fix16_sub(F16C(2,0), a.v)), fix16_mul(a.s, fix16_sub(F16C(2,0), a.s)));
+    fix16_t radius_b = fix16_mul(fix16_mul(b.v, fix16_sub(F16C(2,0), b.v)), fix16_mul(b.s, fix16_sub(F16C(2,0), b.s)));
+
+    // x, y and z deltas between cartesian coordinates of a and b in HSV bicone
+    // delx = radius_b*cos(b_h) - radius_a*cos(a_h)
+    fix16_t delx = fix16_sub(
+        fix16_mul(radius_b, fix16_cos(b.h)),
+        fix16_mul(radius_a, fix16_cos(a.h)));
+
+    // dely = radius_b*sin(b_h) - radius_a*sin(a_h)
+    fix16_t dely = fix16_sub(
+        fix16_mul(radius_b, fix16_sin(b.h)),
+        fix16_mul(radius_a, fix16_sin(a.h)));
+
+    // delz = cone_height * ((b_v-b_s/2) - (a_v-a_s/2))
+    fix16_t delz = fix16_sub(
+        fix16_sub(b.v, fix16_mul(fix16_mul(F16C(0,5), b.s), b.v)),
+        fix16_sub(a.v, fix16_mul(fix16_mul(F16C(0,5), a.s), a.v)));
+
+    // cdist = chroma_weight*(delx*delx + dely*dely) + (100-chroma_weight)*delz*delz
+    fix16_t cdist = fix16_add(
+        fix16_mul(
+            fix16_from_int(100 * chroma_weight),
+            fix16_add(
+                fix16_sq(delx),
+                fix16_sq(dely))),
+        fix16_mul(
+            fix16_from_int(10000 - 100 * chroma_weight),
+            fix16_sq(delz)));
+
+    // multiply by 100 to increase resolution when converting to int
+    return fix16_to_int(cdist);
+}