Probe Move tracking through PSMoveAPI

crates/muninn-daemon/src/main.rs

@@ -5631,15 +5631,6 @@ fn tick_move_light_commands(
     Ok(())
 }
 
-const DEFAULT_MOVE_LIGHT_COLORS: &[(u8, u8, u8)] = &[
-    (255, 48, 64),
-    (32, 160, 255),
-    (80, 255, 120),
-    (255, 208, 48),
-    (200, 80, 255),
-    (255, 128, 32),
-];
-
 fn start_default_move_light_worker(
     options: &Options,
     suppressed_paths: Arc<Mutex<HashSet<String>>>,
@@ -5735,8 +5726,28 @@ fn push_unique_light_target(
 }
 
 fn default_move_color_for_identity(identity: &str) -> (u8, u8, u8) {
-    let hash = stable_u64_hash(identity);
-    DEFAULT_MOVE_LIGHT_COLORS[hash as usize % DEFAULT_MOVE_LIGHT_COLORS.len()]
+    let hue = (stable_u64_hash(identity) % 360) as f64;
+    hsv_to_rgb(hue, 0.82, 1.0)
+}
+
+fn hsv_to_rgb(hue_degrees: f64, saturation: f64, value: f64) -> (u8, u8, u8) {
+    let chroma = value * saturation;
+    let hue_sector = (hue_degrees.rem_euclid(360.0)) / 60.0;
+    let secondary = chroma * (1.0 - ((hue_sector % 2.0) - 1.0).abs());
+    let (red, green, blue) = match hue_sector.floor() as u8 {
+        0 => (chroma, secondary, 0.0),
+        1 => (secondary, chroma, 0.0),
+        2 => (0.0, chroma, secondary),
+        3 => (0.0, secondary, chroma),
+        4 => (secondary, 0.0, chroma),
+        _ => (chroma, 0.0, secondary),
+    };
+    let floor = value - chroma;
+    (
+        ((red + floor) * 255.0).round() as u8,
+        ((green + floor) * 255.0).round() as u8,
+        ((blue + floor) * 255.0).round() as u8,
+    )
 }
 
 fn stable_u64_hash(value: &str) -> u64 {
@@ -10641,6 +10652,15 @@ mod tests {
             default_move_color_for_identity("move-0006f523e2d1"),
             default_move_color_for_identity("move-000704a6be5f")
         );
+        let roster = [
+            "move-0006f523e2d1",
+            "move-000704a39772",
+            "move-000704a6be5f",
+            "move-000704a800d0",
+        ];
+        let colors = roster.map(default_move_color_for_identity);
+        assert_eq!(colors.into_iter().collect::<HashSet<_>>().len(), roster.len());
+        assert!(colors.into_iter().all(|color| color.0.max(color.1).max(color.2) == 255));
     }
 
     #[test]

native/probes/psmoveapi_tracker_probe.c

@@ -0,0 +1,116 @@
+#include <psmoveapi/psmove.h>
+#include <psmoveapi/psmove_tracker.h>
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+
+static uint64_t stable_hash(const char *value)
+{
+    uint64_t hash = UINT64_C(14695981039346656037);
+    while (*value) {
+        hash ^= (unsigned char)*value++;
+        hash *= UINT64_C(1099511628211);
+    }
+    return hash ? hash : 1;
+}
+
+static void stable_color(const char *serial, unsigned char color[3])
+{
+    const double hue_sector = (double)(stable_hash(serial) % 360) / 60.0;
+    const double chroma = 0.82;
+    double sector_mod = hue_sector;
+    while (sector_mod >= 2.0) sector_mod -= 2.0;
+    const double secondary = chroma * (1.0 - (sector_mod > 1.0 ? sector_mod - 1.0 : 1.0 - sector_mod));
+    double red = 0.0, green = 0.0, blue = 0.0;
+    switch ((int)hue_sector) {
+    case 0: red = chroma; green = secondary; break;
+    case 1: red = secondary; green = chroma; break;
+    case 2: green = chroma; blue = secondary; break;
+    case 3: green = secondary; blue = chroma; break;
+    case 4: red = secondary; blue = chroma; break;
+    default: red = chroma; blue = secondary; break;
+    }
+    const double floor = 1.0 - chroma;
+    color[0] = (unsigned char)((red + floor) * 255.0 + 0.5);
+    color[1] = (unsigned char)((green + floor) * 255.0 + 0.5);
+    color[2] = (unsigned char)((blue + floor) * 255.0 + 0.5);
+}
+
+int main(int argc, char **argv)
+{
+    const int camera = argc > 1 ? atoi(argv[1]) : 0;
+    const int frames = argc > 2 ? atoi(argv[2]) : 300;
+    const float exposure = argc > 3 ? strtof(argv[3], NULL) : 0.12f;
+    const int count = psmove_count_connected();
+    if (count <= 0) {
+        fprintf(stderr, "no PS Move controllers connected\n");
+        return 2;
+    }
+
+    PSMoveTracker *tracker = psmove_tracker_new_with_camera(camera);
+    if (!tracker) {
+        fprintf(stderr, "camera %d could not create PSMoveTracker\n", camera);
+        return 3;
+    }
+    psmove_tracker_set_exposure(tracker, exposure);
+
+    PSMove **moves = calloc((size_t)count, sizeof(*moves));
+    if (!moves) {
+        return 4;
+    }
+    int enabled = 0;
+    for (int index = 0; index < count; index++) {
+        PSMove *move = psmove_connect_by_id(index);
+        if (!move) {
+            continue;
+        }
+        const char *serial = psmove_get_serial(move);
+        unsigned char color[3];
+        stable_color(serial, color);
+        const enum PSMoveTracker_Status status = psmove_tracker_enable_with_color(
+            tracker, move, color[0], color[1], color[2]);
+        fprintf(stderr, "controller=%s color=%u,%u,%u calibration=%d\n",
+                serial, color[0], color[1], color[2], status);
+        if (status == Tracker_CALIBRATED) {
+            moves[enabled++] = move;
+        } else {
+            psmove_disconnect(move);
+        }
+    }
+
+    const struct PSMoveCameraInfo *info = psmove_tracker_get_camera_info(tracker);
+    fprintf(stderr, "camera=%d name=%s api=%s size=%dx%d exposure=%.3f controllers=%d\n",
+            camera,
+            info && info->camera_name ? info->camera_name : "unknown",
+            info && info->camera_api ? info->camera_api : "unknown",
+            info ? info->width : 0,
+            info ? info->height : 0,
+            psmove_tracker_get_exposure(tracker),
+            enabled);
+
+    for (int frame = 0; frame < frames; frame++) {
+        psmove_tracker_update_image(tracker);
+        for (int index = 0; index < enabled; index++) {
+            psmove_tracker_update(tracker, moves[index]);
+            float x = 0.f, y = 0.f, radius = 0.f;
+            const int age_ms = psmove_tracker_get_position(
+                tracker, moves[index], &x, &y, &radius);
+            if (age_ms >= 0 && radius > 0.f) {
+                printf("frame=%d controller=%s x=%.3f y=%.3f radius=%.3f age_ms=%d\n",
+                       frame, psmove_get_serial(moves[index]), x, y, radius, age_ms);
+                fflush(stdout);
+            }
+        }
+    }
+
+    for (int index = 0; index < enabled; index++) {
+        psmove_tracker_disable(tracker, moves[index]);
+        psmove_disconnect(moves[index]);
+    }
+    psmove_tracker_free(tracker);
+    free(moves);
+    return enabled > 0 ? 0 : 5;
+}