116 lines
4.1 KiB
C
116 lines
4.1 KiB
C
#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;
|
|
}
|