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

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

@@ -2,64 +2,37 @@
 // 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);
-}
+#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.
+ * 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;
 }

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);
+}