From 7d400e51f9d81804c48a2d6738d0f822dd1061d2 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Wed, 5 Jul 2023 11:55:22 +0200 Subject: [PATCH] pbio/color/util: Move math to int_math. Also ensure we pick the proper sign for v in radial computations. --- lib/pbio/include/pbio/int_math.h | 2 + lib/pbio/src/color/util.c | 65 ++++++++++---------------------- lib/pbio/src/int_math.c | 43 +++++++++++++++++++++ 3 files changed, 64 insertions(+), 46 deletions(-) diff --git a/lib/pbio/include/pbio/int_math.h b/lib/pbio/include/pbio/int_math.h index c6d355003..430cbf300 100644 --- a/lib/pbio/include/pbio/int_math.h +++ b/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_ diff --git a/lib/pbio/src/color/util.c b/lib/pbio/src/color/util.c index 0d6b5756d..b2a2eb00a 100644 --- a/lib/pbio/src/color/util.c +++ b/lib/pbio/src/color/util.c @@ -2,64 +2,37 @@ // Copyright (c) 2018-2022 The Pybricks Authors #include - -// 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); -} +#include /** - * Gets squared Euclidean distance between HSV colors mapped into a chroma-lightness-bicone. - * The bicone is 20000 units tall and 20000 units in diameter. + * 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; + // 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; - int32_t b_h = hsv_b->h; - int32_t b_s = hsv_b->s; - int32_t b_v = hsv_b->v; + // 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; - // 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); + // 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 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); + 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) - int32_t cdist = delx * delx + dely * dely + delz * delz; - - return cdist; + return delta_x * delta_x + delta_y * delta_y + delta_z * delta_z; } diff --git a/lib/pbio/src/int_math.c b/lib/pbio/src/int_math.c index e525b187c..dc89112bf 100644 --- a/lib/pbio/src/int_math.c +++ b/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); +}