increase resolution of cone distance, add tests

pybricks · Mar 1, 2022 · 074063b · 074063b
1 parent 4c51690
commit 074063b
Show file tree

Hide file tree

Showing 2 changed files with 87 additions and 3 deletions.
diff --git a/lib/pbio/src/color/util.c b/lib/pbio/src/color/util.c
@@ -77,5 +77,5 @@ int32_t pbio_get_cone_cost(const pbio_color_hsv_t *a, const pbio_color_hsv_t *b)
             fix16_sq(delx),
             fix16_sq(dely)),
         fix16_sq(delz));
-    return fix16_to_int(cdist);
+    return fix16_to_int(fix16_mul(cdist, fix16_from_int(5000)));
 }
diff --git a/lib/pbio/test/src/color.c b/lib/pbio/test/src/color.c
@@ -351,6 +351,7 @@ static void test_color_hsv_compression(void *env) {
 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;
@@ -368,15 +369,98 @@ static void test_color_hsv_cost(void *env) {
     color_b.v = 0;
     tt_want_int_op(pbio_get_cone_cost(&color_a, &color_b), ==, 0);
 
-    // max value with different saturations/hues should be different
+    // colors with different hues should be different when value>0 and saturation>0
     color_a.h = 230;
-    color_a.s = 23;
+    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_get_cone_cost(&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_get_cone_cost(&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_get_cone_cost(&color_a, &color_b);
+    printf("\n\nhello world %d\n", dist);
+
+    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_get_cone_cost(&color_a, &color_b), >, dist);
+
+    // resolve colors that are very 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_get_cone_cost(&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_get_cone_cost(&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 = 71;
+
+    tt_want_int_op(pbio_get_cone_cost(&color_a, &color_b), >, 0);
+
+    // distance of opposite colors should be the same as double saturation to gray
+    color_a.h = 20;
+    color_a.s = 20;
+    color_a.v = 70;
+
+    color_b.h = 200;
+    color_b.s = 20;
+    color_b.v = 70;
+    dist = pbio_get_cone_cost(&color_a, &color_b);
+
+    color_a.h = 20;
+    color_a.s = 40;
+    color_a.v = 70;
+
+    color_b.h = 0;
+    color_b.s = 0;
+    color_b.v = 70;
+
+    tt_want_int_op(pbio_get_cone_cost(&color_a, &color_b), ==, dist);
 }
 
 struct testcase_t pbio_color_tests[] = {