diff --git a/screens/handcut.cpp b/screens/handcut.cpp index a697683..a64218c 100644 --- a/screens/handcut.cpp +++ b/screens/handcut.cpp @@ -30,6 +30,9 @@ constexpr size_t kFileSize = kHeader + kMaxHands * kHand + kMaxCapsules * kCapsu constexpr int64_t kStaleNs = 300'000'000; // hands older than this are gone constexpr int64_t kHistoryNs = 1'000'000'000; constexpr double kNear = 0.12; // metres: nothing closer to an eye than this is cut +constexpr double kMaxSpeed = 2.5; // m/s: faster is a tracking jump, not a hand +constexpr double kStillSpeed = 0.05; // m/s: below this, a hand's velocity is noise +constexpr double kMaxAhead = 0.12; // s: never predict further than this int64_t MonoNs() { timespec ts; @@ -82,13 +85,61 @@ bool Hands::Read() { if (u64(16) != s1 || std::memcmp(copy, kMagic, 8) != 0) return false; seq_ = s1; uint64_t capture, publish; - uint32_t ncaps; + uint32_t nhands, ncaps; std::memcpy(&capture, copy + 24, 8); std::memcpy(&publish, copy + 32, 8); + std::memcpy(&nhands, copy + 40, 4); std::memcpy(&ncaps, copy + 44, 4); captureNs_ = int64_t(capture), publishNs_ = int64_t(publish); const Mat head = HeadAt(captureNs_); - world_.clear(); + + // each hand's palm in the room, and its velocity from the last time it was seen + ids_.clear(); + std::vector owners; // the hand each capsule belongs to, in file order + for (uint32_t k = 0; k < std::min(nhands, kMaxHands); ++k) { + const uint8_t *h = copy + kHeader + k * kHand; + uint32_t id, n; + float pts[21][3]; + std::memcpy(&id, h, 4); + std::memcpy(pts, h + 16, sizeof pts); + std::memcpy(&n, h + 268, 4); + const int idx = int(ids_.size()); + ids_.push_back(id); + owners.insert(owners.end(), std::min(n, kMaxCapsules), idx); + double palm[3] = {0, 0, 0}; + bool ok = true; + for (int j : {0, 5, 9, 13, 17}) { + float w[3]; + ok = ok && std::isfinite(pts[j][0]) && std::isfinite(pts[j][1]) && std::isfinite(pts[j][2]); + Apply(head, pts[j], w); + for (int i = 0; i < 3; ++i) palm[i] += w[i] / 5; + } + Motion &m = motion_[id]; + const double dt = (captureNs_ - m.ns) / 1e9; + if (!ok) { + m = Motion{}; + continue; + } + if (m.ns && dt > 0.005 && dt < 0.2) { + double speed = 0; + for (int i = 0; i < 3; ++i) { + m.v[i] += 0.5 * ((palm[i] - m.palm[i]) / dt - m.v[i]); + speed += m.v[i] * m.v[i]; + } + speed = std::sqrt(speed); + if (speed > kMaxSpeed) + for (double &v : m.v) v *= kMaxSpeed / speed; + } else { + m.v[0] = m.v[1] = m.v[2] = 0; + } + m.ns = captureNs_; + std::memcpy(m.palm, palm, sizeof palm); + } + for (auto it = motion_.begin(); it != motion_.end();) + it = captureNs_ - it->second.ns > kStaleNs ? motion_.erase(it) : std::next(it); + if (owners.size() != std::min(ncaps, kMaxCapsules)) owners.assign(std::min(ncaps, kMaxCapsules), -1); + + base_.clear(), owner_.clear(); for (uint32_t k = 0; k < std::min(ncaps, kMaxCapsules); ++k) { float f[8]; std::memcpy(f, copy + kHeader + kMaxHands * kHand + k * kCapsule, sizeof f); @@ -99,11 +150,17 @@ bool Hands::Read() { Apply(head, f, c.a); Apply(head, f + 3, c.b); c.ra = f[6], c.rb = f[7]; - world_.push_back(c); + base_.push_back(c); + owner_.push_back(owners[k]); } return true; } +void Hands::SetPrediction(bool on, double leadMs) { + predict_ = on; + leadNs_ = int64_t(std::clamp(leadMs, 0.0, 100.0) * 1e6); +} + Mat Hands::HeadAt(int64_t ns) const { const Past *best = nullptr; for (const Past &p : history_) @@ -115,7 +172,23 @@ bool Hands::Update(const Mat &head, int64_t nowNs) { history_.push_back({nowNs, head}); while (!history_.empty() && nowNs - history_.front().ns > kHistoryNs) history_.erase(history_.begin()); Read(); - if (nowNs - publishNs_ > kStaleNs) world_.clear(); + if (nowNs - publishNs_ > kStaleNs) base_.clear(), owner_.clear(); + // move each hand ahead to when this frame will be on the displays; a slow hand's + // velocity is mostly tracking noise, so it fades out below kStillSpeed + const double ahead = std::clamp((nowNs + leadNs_ - captureNs_) / 1e9, 0.0, kMaxAhead); + world_ = base_; + for (size_t k = 0; predict_ && k < world_.size(); ++k) { + if (owner_[k] < 0) continue; + const auto m = motion_.find(ids_[owner_[k]]); + if (m == motion_.end()) continue; + const double *v = m->second.v; + const double speed = std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); + const double gain = std::clamp((speed - kStillSpeed) / kStillSpeed, 0.0, 1.0); + for (int i = 0; i < 3; ++i) { + world_[k].a[i] += float(v[i] * gain * ahead); + world_[k].b[i] += float(v[i] * gain * ahead); + } + } return !world_.empty(); } diff --git a/screens/handcut.h b/screens/handcut.h index 3defcf2..15dc609 100644 --- a/screens/handcut.h +++ b/screens/handcut.h @@ -8,6 +8,10 @@ // sees them on a panel, and Renderer draws the panel's client buffer into a side-by-side // buffer (left eye | right eye) with those spots transparent. A panel shows that buffer, // with the overlay's SideBySide_Parallel flag, only while a hand is in front of it. +// +// The hands arrive some 30-60 ms after the cameras saw them, and show up on the displays +// later still, so Hands moves each hand ahead along its velocity in the room to where it +// will be when the frame reaches the eyes. #pragma once #include "vr.h" @@ -46,14 +50,27 @@ public: // Once per tick: the head pose now, CLOCK_MONOTONIC ns. Re-reads the hands file when // it changed. Returns true while fresh hands are known. bool Update(const Mat &head, int64_t nowNs); + // Where the hands will be `lead` after now (see SetPrediction). const std::vector &capsules() const { return world_; } + // Predict the hands' motion (on by default) to now + leadMs: about how long a frame + // takes from here to the displays. + void SetPrediction(bool on, double leadMs); + bool predicting() const { return predict_; } + double leadMs() const { return leadNs_ / 1e6; } private: bool Read(); Mat HeadAt(int64_t ns) const; struct Past { int64_t ns; Mat head; }; + struct Motion { int64_t ns = 0; double palm[3]{}, v[3]{}; }; // a hand's palm, in the room std::vector history_; // the last second of head poses - std::vector world_; + std::vector base_; // the capsules at capture time, in the room + std::vector owner_; // each capsule's hand (index into ids_), or -1 + std::vector ids_; // the hands in the file + std::map motion_; + std::vector world_; // base_, moved ahead + bool predict_ = true; + int64_t leadNs_ = 25'000'000; int fd_ = -1; const void *map_ = nullptr; uint64_t seq_ = 0; diff --git a/screens/vr.cpp b/screens/vr.cpp index 7592ba3..b146bf7 100644 --- a/screens/vr.cpp +++ b/screens/vr.cpp @@ -1415,7 +1415,9 @@ void ft_vr_poll(void (*handle)(const struct ft_event *, void *), void *data) { // state -> "ok // " // cutouts on|off|state hand cutouts (see handcut.h) -> "ok -// " +// ms, predict lead ms" +// cutouts predict on|off move the hands ahead along their velocity (on by default) +// cutouts lead ...to this long after now: about when the frame is on the displays // (size and key are handled in compositor.c.) Screens are // numbered from 1 here, like everywhere the user sees them. void ft_vr_command(const char *cmd, char *reply, int size) { @@ -1546,11 +1548,20 @@ void ft_vr_command(const char *cmd, char *reply, int size) { UpdateLasers(); std::snprintf(reply, size, "ok %s", LasersName()); } else if (std::sscanf(cmd, "cutouts %15s", word) == 1) { + char arg[16] = ""; + double ms = 0; if (!std::strcmp(word, "on")) g_cutouts = true; else if (!std::strcmp(word, "off")) g_cutouts = false; - else if (std::strcmp(word, "state") != 0) return (void)std::snprintf(reply, size, "error cutouts on|off|state"); - std::snprintf(reply, size, "ok %s %s %.2f ms", g_cutouts ? "on" : "off", - g_cutterState > 0 ? "ready" : g_cutterState < 0 ? "unavailable" : "idle", g_cutter.lastMs()); + else if (!std::strcmp(word, "predict") && std::sscanf(cmd, "cutouts predict %15s", arg) == 1 && + (!std::strcmp(arg, "on") || !std::strcmp(arg, "off"))) + g_hands.SetPrediction(!std::strcmp(arg, "on"), g_hands.leadMs()); + else if (!std::strcmp(word, "lead") && std::sscanf(cmd, "cutouts lead %lf", &ms) == 1) + g_hands.SetPrediction(g_hands.predicting(), ms); + else if (std::strcmp(word, "state") != 0) + return (void)std::snprintf(reply, size, "error cutouts on|off|state|predict on|off|lead "); + std::snprintf(reply, size, "ok %s %s %.2f ms, predict %s lead %.0f ms", g_cutouts ? "on" : "off", + g_cutterState > 0 ? "ready" : g_cutterState < 0 ? "unavailable" : "idle", g_cutter.lastMs(), + g_hands.predicting() ? "on" : "off", g_hands.leadMs()); } else if (std::strncmp(cmd, "state", 5) == 0) { std::snprintf(reply, size, "ok %s %d %.0f %s %.0f %s %d %s", ModeName(), g_manual ? 1 : 0, g_wristAngle, g_gestureHand.c_str(), g_gestureAngle, LasersName(), g_gameRunning ? 1 : 0,