steam-frame/Frametop--frametop
1
0 Fork 0
mirror of https://github.com/DeeJanuz/frametop.git synced 2026-10-27 05:00:45 +01:00
Code Issues Packages Projects Releases 22 Wiki Activity
Files
7dfdfd24e1603ed65671b60274afac72ed00a211
Frametop--frametop/screens/handcut.cpp
T
DeeJanuzandClaude Opus 5.5 6c921b113c ft-screens: "grab that" brings a panel to arm's reach (HANDS_GRAB_THAT) 
Hold an open hand out to a screen, remote display or floating window,
palm to it, then pull the arm back making a fist: the panel comes over
250 ms to 45 cm ahead of the eyes, at the fist's height, facing you
(user, 2026-10-10). Screens wider than HANDS_GRAB_THAT_WIDTH (0.6 m)
come at that width. The same gesture at a panel that came, still where
it came to, sends it back; the reset button sends floating windows back
(the layout reset places screens and remote displays itself).

screens/grabthat.{h,cpp}: the detector (pure logic): open fingers, palm
ahead and the wrist 28 cm out for 0.2 s arm it; the fingers closing with
the palm still to the panel, then on one frame a fist, the wrist 8 cm
back within 0.5 s and the palm turned over fire it. The panel is the one
the eye-through-palm line meets, else the one the head points at. From
the closing (thumb clear of the index: not a pinch) to 0.4 s after the
fire the hand counts as near, so the helper doesn't take the fist as a
gaze grip. Hands frames carry the head pose at capture now
(Hands::captureHead).

Tests replay the user's flag (fires twice, right hand, at -11.35 and
-5.89 s; expected -11.4 and -5.9), and the other 12 flags and 15 minutes
of recordings fire nothing (1 of 143 pinches, 3 of 12 grips begin on a
claimed hand); synthetic waves, pushes, fists without the pull, slow
pulls, a hand turned over first, held-out pinches, a drop to the lap and
typing don't fire. handinput-test --grabthat FLAG_DIR... scans flags.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
2026-10-10 15:49:38 -06:00

911 lines
41 KiB
C++
Raw Blame History

// Hand cutouts, the CPU side (see handcut.h): the hands and gestures files, the matte and
// where the eyes see it on a panel. No GPU or SteamVR calls here (handrender.cpp has those).
#include "handcut.h"
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <unistd.h>
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <fstream>
#include <sstream>
#include <string>
namespace handcut {
namespace {
// The hands file ft-hands publishes (hands/include/fh_hands.h).
constexpr uint32_t kMaxHands = FH_HANDS_MAX_HANDS, kMaxCapsules = FH_HANDS_MAX_CAPSULES;
constexpr int64_t kStaleNs = 300'000'000; // hands older than this are gone
constexpr int64_t kReopenNs = 3'000'000'000; // a file this stale is opened again (ft-hands may have made a new one)
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
constexpr double kPalmLen = 0.0815; // wrist to middle knuckle of the skeleton's average hand
int64_t MonoNs() {
timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
return int64_t(ts.tv_sec) * 1'000'000'000 + ts.tv_nsec;
}
void Apply(const Mat &m, const float p[3], float out[3]) {
float r[3];
for (int i = 0; i < 3; ++i) r[i] = m.m[i][0] * p[0] + m.m[i][1] * p[1] + m.m[i][2] * p[2] + m.m[i][3];
std::memcpy(out, r, sizeof r);
}
// Room -> panel-local: R^T (p - t).
void ToLocal(const Mat &m, const double p[3], double out[3]) {
const double d[3] = {p[0] - m.m[0][3], p[1] - m.m[1][3], p[2] - m.m[2][3]};
for (int i = 0; i < 3; ++i) out[i] = m.m[0][i] * d[0] + m.m[1][i] * d[1] + m.m[2][i] * d[2];
}
double Dist(const float a[3], const float b[3]) {
return std::sqrt(double(a[0] - b[0]) * (a[0] - b[0]) + double(a[1] - b[1]) * (a[1] - b[1]) +
double(a[2] - b[2]) * (a[2] - b[2]));
}
void Mid(const float a[3], const float b[3], float out[3]) {
for (int i = 0; i < 3; ++i) out[i] = (a[i] + b[i]) / 2;
}
bool Finite(const float *v, int n) {
for (int i = 0; i < n; ++i)
if (!std::isfinite(v[i]) || std::fabs(v[i]) > 10) return false;
return true;
}
// The skeleton's rough radii (hands/track/skeleton.cpp) at the average hand's size, for version 1.
float DefaultRadius(int i) { return i == 0 ? 0.020f : i == 1 || i % 4 == 1 ? 0.012f : i <= 4 ? 0.0095f : 0.0085f; }
// Turns p by `rad` about the unit axis through `at` (Rodrigues).
void Turn(float p[3], const double at[3], const double axis[3], double rad) {
const double v[3] = {p[0] - at[0], p[1] - at[1], p[2] - at[2]};
const double c = std::cos(rad), s = std::sin(rad), d = axis[0] * v[0] + axis[1] * v[1] + axis[2] * v[2];
const double x[3] = {axis[1] * v[2] - axis[2] * v[1], axis[2] * v[0] - axis[0] * v[2], axis[0] * v[1] - axis[1] * v[0]};
for (int i = 0; i < 3; ++i) p[i] = float(at[i] + v[i] * c + x[i] * s + axis[i] * d * (1 - c));
}
} // namespace
double Hand::speed() const { return std::sqrt(double(v[0]) * v[0] + double(v[1]) * v[1] + double(v[2]) * v[2]); }
void Hand::Palm(double out[3]) const {
out[0] = out[1] = out[2] = 0;
for (int j : {0, 5, 9, 13, 17})
for (int i = 0; i < 3; ++i) out[i] += p[j][i] / 5.0;
}
// ---------------------------------------------------------------------------- settings
std::string Setting(const char *name, const char *def) {
const std::string env = std::string("FT_") + name;
if (const char *v = std::getenv(env.c_str())) return v;
const char *home = std::getenv("HOME");
if (home) {
std::ifstream f(std::string(home) + "/.config/frametop.conf");
std::string line, found;
bool have = false;
const std::string key = std::string(name) + "=";
while (std::getline(f, line)) {
if (line.rfind(key, 0) != 0) continue;
std::string v = line.substr(key.size());
const size_t hash = v.find('#');
if (hash != std::string::npos) v.resize(hash);
while (!v.empty() && std::isspace(static_cast<unsigned char>(v.back()))) v.pop_back();
if (v.size() >= 2 && (v[0] == '"' || v[0] == '\'') && v.back() == v[0]) v = v.substr(1, v.size() - 2);
found = v, have = true; // the last one counts, as when the shell reads it
}
if (have) return found;
}
return def;
}
bool ParseShift(const std::string &text, std::vector<Shift> &out) {
std::vector<Shift> s;
std::istringstream in(text);
std::string item;
while (in >> item) {
Shift k;
if (std::sscanf(item.c_str(), "%lf:%lf,%lf", &k.dist, &k.yawDeg, &k.pitchDeg) != 3 || !(k.dist > 0) ||
std::fabs(k.yawDeg) > 10 || std::fabs(k.pitchDeg) > 10)
return false;
s.push_back(k);
}
std::sort(s.begin(), s.end(), [](const Shift &a, const Shift &b) { return a.dist < b.dist; });
out = s;
return true;
}
const char *ShapeName(MatteShape s) {
return s == MatteShape::Circle ? "circle" : s == MatteShape::Mesh ? "mesh" : "capsules";
}
bool ParseShape(const std::string &name, MatteShape *out) {
for (MatteShape s : {MatteShape::Circle, MatteShape::Mesh, MatteShape::Capsules})
if (name == ShapeName(s)) return *out = s, true;
return false;
}
std::string ReadMatteSettings(MatteParams &p) {
std::string log;
auto num = [&](const char *name, double &v, double lo, double hi) {
const std::string s = Setting(name, "");
if (s.empty()) return;
char *end;
const double x = std::strtod(s.c_str(), &end);
if (end != s.c_str() && !*end && x >= lo && x <= hi) v = x, log += std::string(name) + "=" + s + " ";
else log += std::string(name) + "=" + s + " (ignored) ";
};
const std::string m = Setting("HANDS_MATTE", "");
if (!m.empty()) log += "HANDS_MATTE=" + m + (ParseShape(m, &p.shape) ? " " : " (ignored) ");
num("HANDS_MATTE_MARGIN", p.marginDeg, 0, 10);
num("HANDS_MATTE_MOTION", p.motionMs, 0, 200);
num("HANDS_MATTE_MOTION_MAX", p.motionMaxDeg, 0, 10);
const std::string sh = Setting("HANDS_MATTE_SHIFT", "");
if (!sh.empty()) log += "HANDS_MATTE_SHIFT=" + sh + (ParseShift(sh, p.shift) ? " " : " (ignored) ");
return log;
}
double MatteParams::Margin(double dist, double speed) const {
const double still = dist * std::tan(marginDeg * M_PI / 180);
const double moving = std::min(speed * motionMs / 1000, dist * std::tan(motionMaxDeg * M_PI / 180));
return std::max(minMarginM, still + moving);
}
// ------------------------------------------------------------------------------- hands
Hands::~Hands() {
if (map_) munmap(const_cast<void *>(map_), mapLen_);
if (gmap_) munmap(const_cast<void *>(gmap_), gmapLen_);
if (fd_ >= 0) close(fd_);
if (gfd_ >= 0) close(gfd_);
}
namespace {
// Maps up to `want` bytes of a file in the hands folder, if it's ours and at least `least` long.
const void *MapFile(const char *name, size_t least, size_t want, int *fd, size_t *len) {
const std::string path = "/run/user/" + std::to_string(getuid()) + "/frametop-hands/" + name;
*fd = open(path.c_str(), O_RDONLY | O_CLOEXEC | O_NOFOLLOW);
if (*fd < 0) return nullptr;
struct stat st;
if (fstat(*fd, &st) < 0 || st.st_uid != getuid() || size_t(st.st_size) < least) {
close(*fd), *fd = -1;
return nullptr;
}
*len = std::min(size_t(st.st_size), want);
void *m = mmap(nullptr, *len, PROT_READ, MAP_SHARED, *fd, 0);
if (m == MAP_FAILED) {
close(*fd), *fd = -1;
return nullptr;
}
return m;
}
// A copy of a seqlocked file (both files start with magic, version, size, seq). False if it
// was being written, or hasn't changed since `last`.
bool CopySeq(const void *map, size_t len, uint64_t *last, void *out) {
const auto *seq = reinterpret_cast<const uint64_t *>(static_cast<const char *>(map) + 16);
const uint64_t s1 = __atomic_load_n(seq, __ATOMIC_ACQUIRE);
if ((s1 & 1) || s1 == *last) return false;
std::memcpy(out, map, len);
__atomic_thread_fence(__ATOMIC_ACQUIRE);
if (__atomic_load_n(seq, __ATOMIC_RELAXED) != s1) return false;
*last = s1;
return true;
}
} // namespace
bool Hands::ReadFiles() {
const int64_t now = MonoNs();
bool got = false;
// a long-stale file: ft-hands may have stopped and made a new one since
if (map_ && now - publishNs_ > kReopenNs && now - lastOpenTry_ > kReopenNs) {
munmap(const_cast<void *>(map_), mapLen_), close(fd_);
map_ = nullptr, fd_ = -1, seq_ = 0;
}
if (!map_ && now - lastOpenTry_ >= 1'000'000'000) {
lastOpenTry_ = now;
map_ = MapFile("hands", sizeof(fh_hands_t), sizeof(fh_hands_v2_t), &fd_, &mapLen_);
}
static fh_hands_v2_t copy;
if (map_ && CopySeq(map_, mapLen_, &seq_, &copy)) {
if (std::memcmp(copy.v1.magic, FH_HANDS_MAGIC, 8) == 0) {
if (mapLen_ < sizeof copy) std::memset(reinterpret_cast<char *>(&copy) + mapLen_, 0, sizeof copy - mapLen_);
const bool v2 = copy.v1.version >= 2 && mapLen_ >= sizeof copy && copy.v1.size >= sizeof copy;
// A version 2 file grown under a version 1 mapping: map it again next time.
if (copy.v1.version >= 2 && mapLen_ < sizeof copy) {
munmap(const_cast<void *>(map_), mapLen_), close(fd_);
map_ = nullptr, fd_ = -1, seq_ = 0, lastOpenTry_ = 0;
}
version_ = int(copy.v1.version);
Ingest(copy.v1, v2 ? copy.fits : nullptr, nullptr);
got = true;
}
}
// the gestures file, written right after each hands update
if (gmap_ && now - publishNs_ > kReopenNs && now - lastGOpenTry_ > kReopenNs) {
munmap(const_cast<void *>(gmap_), gmapLen_), close(gfd_);
gmap_ = nullptr, gfd_ = -1, gseq_ = 0;
}
if (!gmap_ && now - lastGOpenTry_ >= 1'000'000'000) {
lastGOpenTry_ = now;
// version 2 (the grips) or later; a later one's extra slots are left out
gmap_ = MapFile("gestures", offsetof(fh_gestures_t, grip) + sizeof(fh_pinch_t) * 2, sizeof(fh_gestures_t), &gfd_,
&gmapLen_);
}
static fh_gestures_t g;
if (gmap_ && CopySeq(gmap_, gmapLen_, &gseq_, &g) && std::memcmp(g.magic, FH_GESTURES_MAGIC, 8) == 0 && g.version >= 2) {
if (gmapLen_ < sizeof g) std::memset(reinterpret_cast<char *>(&g) + gmapLen_, 0, sizeof g - gmapLen_);
Ingest(copy.v1, nullptr, &g);
}
return got;
}
void Hands::Ingest(const fh_hands_t &v1, const fh_hand_fit_t *fits, const fh_gestures_t *g) {
if (g) { // gestures only
const Mat head = HeadAt(int64_t(g->capture_ns));
for (int k = 0; k < 4; ++k) {
const fh_pinch_t &in = k < 2 ? g->pinch[k] : g->grip[k - 2];
Gesture &o = k < 2 ? pinch_[k] : grip_[k - 2];
o.tracked = in.flags & FH_PINCH_TRACKED, o.down = in.flags & FH_PINCH_DOWN, o.lost = in.flags & FH_PINCH_LOST;
o.handId = in.hand_id, o.begins = in.begins, o.ends = in.ends;
o.beginNs = int64_t(in.begin_ns), o.endNs = int64_t(in.end_ns);
o.distance = in.distance, o.strength = in.strength;
if (Finite(in.point, 3)) Apply(head, in.point, o.point);
if (Finite(in.begin_point, 3)) Apply(HeadAt(o.beginNs), in.begin_point, o.beginPoint);
}
return;
}
++frames_;
const uint32_t nhands = std::min<uint32_t>(v1.nhands, kMaxHands), ncaps = std::min<uint32_t>(v1.ncapsules, kMaxCapsules);
captureNs_ = int64_t(v1.capture_ns), publishNs_ = int64_t(v1.publish_ns);
const Mat head = captureHead_ = HeadAt(captureNs_);
seen_.clear();
std::vector<int> owners; // the hand each capsule belongs to, in file order (-1: none kept)
for (uint32_t k = 0; k < nhands; ++k) {
const fh_hand_t &in = v1.hands[k];
const uint32_t nc = std::min<uint32_t>(in.ncapsules, kMaxCapsules);
Hand h;
h.id = in.id;
h.right = in.flags & FH_HAND_RIGHT;
h.stereo = in.flags & FH_HAND_STEREO;
h.confidence = in.confidence;
h.captureNs = captureNs_;
const fh_hand_fit_t *f = fits && (fits[k].flags & FH_FIT_VALID) && fits[k].id == in.id ? &fits[k] : nullptr;
bool ok = true;
if (f) {
h.fit = true;
for (int j = 0; j < 21; ++j) {
ok = ok && Finite(f->joints[j].p, 3);
Apply(head, f->joints[j].p, h.p[j]);
h.radius[j] = std::isfinite(f->joints[j].radius) && f->joints[j].radius > 0 && f->joints[j].radius < 0.05
? f->joints[j].radius : DefaultRadius(j);
}
for (int b = 0; b < 20; ++b) h.bone[b] = f->bone_len[b];
ok = ok && Finite(f->elbow, 3) && Finite(f->pinch_point, 3) && Finite(f->contact, 3);
Apply(head, f->elbow, h.elbow);
Apply(head, f->pinch_point, h.pinchPoint);
Apply(head, f->contact, h.contact);
h.elbowConf = f->elbow_conf, h.hasElbow = true;
h.pinchD[0] = f->pinch_d[0], h.pinchD[1] = f->pinch_d[1];
if (f->tracked_ns) h.captureNs = int64_t(f->tracked_ns);
} else {
for (int j = 0; j < 21; ++j) {
ok = ok && Finite(in.pts[j], 3);
Apply(head, in.pts[j], h.p[j]);
h.radius[j] = DefaultRadius(j);
}
for (int b = 0; b < 20; ++b) {
const int a = b % 4 == 0 ? 0 : b; // bones 0, 4, 8, 12, 16 start at the wrist
h.bone[b] = float(Dist(h.p[a], h.p[b + 1]));
}
Mid(h.p[5], h.p[9], h.pinchPoint);
Mid(h.p[4], h.p[8], h.contact);
h.pinchD[0] = float(Dist(h.p[4], h.p[8])), h.pinchD[1] = float(Dist(h.p[4], h.p[12]));
// no elbow: the forearm carries on along the hand's axis (as ft-hands' capsule)
const double n = Dist(h.p[0], h.p[9]) + 1e-9;
for (int i = 0; i < 3; ++i) h.elbow[i] = float(h.p[0][i] + (h.p[0][i] - h.p[9][i]) / n * 0.25);
}
if (!ok) {
owners.insert(owners.end(), nc, -1);
motion_.erase(h.id);
continue;
}
// its palm's velocity in the room, from the last time it was seen
double palm[3];
h.Palm(palm);
Motion &m = motion_[h.id];
const double dt = (captureNs_ - m.ns) / 1e9;
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 if (!m.ns || dt >= 0.2) {
m.v[0] = m.v[1] = m.v[2] = 0;
}
m.ns = captureNs_;
std::memcpy(m.palm, palm, sizeof palm);
for (int i = 0; i < 3; ++i) h.v[i] = float(m.v[i]);
owners.insert(owners.end(), nc, int(seen_.size()));
seen_.push_back(h);
}
for (auto it = motion_.begin(); it != motion_.end();)
it = captureNs_ - it->second.ns > kStaleNs ? motion_.erase(it) : std::next(it);
if (owners.size() != ncaps) owners.assign(ncaps, -1);
base_.clear(), owner_.clear();
for (uint32_t k = 0; k < ncaps; ++k) {
const fh_capsule_t &f = v1.capsules[k];
bool ok = Finite(f.a, 3) && Finite(f.b, 3) && std::isfinite(f.ra) && std::isfinite(f.rb);
if (!ok || f.ra <= 0 || f.rb <= 0 || f.ra > 1 || f.rb > 1) continue;
Capsule c;
Apply(head, f.a, c.a);
Apply(head, f.b, c.b);
c.ra = f.ra, c.rb = f.rb;
base_.push_back(c);
owner_.push_back(owners[k]);
}
}
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_)
if (!best || std::llabs(p.ns - ns) < std::llabs(best->ns - ns)) best = &p;
return best ? best->head : Mat{};
}
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());
ReadFiles();
Advance(nowNs);
return !world_.empty() || !shown_.empty();
}
bool Hands::Feed(const void *hands, size_t size, const fh_gestures_t *gestures, const Mat &head, int64_t nowNs) {
history_.push_back({nowNs, head});
while (!history_.empty() && nowNs - history_.front().ns > kHistoryNs) history_.erase(history_.begin());
static fh_hands_v2_t copy;
std::memset(&copy, 0, sizeof copy);
if (hands && size >= sizeof(fh_hands_t)) {
std::memcpy(&copy, hands, std::min(size, sizeof copy));
if (std::memcmp(copy.v1.magic, FH_HANDS_MAGIC, 8) == 0) {
const bool v2 = copy.v1.version >= 2 && size >= sizeof copy;
version_ = int(copy.v1.version);
Ingest(copy.v1, v2 ? copy.fits : nullptr, nullptr);
}
}
if (gestures && std::memcmp(gestures->magic, FH_GESTURES_MAGIC, 8) == 0 && gestures->version >= 2)
Ingest(copy.v1, nullptr, gestures);
Advance(nowNs);
return !shown_.empty();
}
void Hands::Advance(int64_t nowNs) {
nowNs_ = nowNs;
fresh_ = nowNs - publishNs_ <= kStaleNs;
if (!fresh_) {
base_.clear(), owner_.clear(), seen_.clear();
for (Gesture *g : {&pinch_[0], &pinch_[1], &grip_[0], &grip_[1]}) g->down = g->tracked = false; // ft-hands stopped
}
// 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);
auto gain = [](const float v[3]) {
const double speed = std::sqrt(double(v[0]) * v[0] + double(v[1]) * v[1] + double(v[2]) * v[2]);
return std::clamp((speed - kStillSpeed) / kStillSpeed, 0.0, 1.0);
};
shown_ = seen_;
world_ = base_;
if (predict_) {
for (Hand &h : shown_) {
const double k = gain(h.v) * ahead;
auto move = [&](float p[3]) {
for (int i = 0; i < 3; ++i) p[i] += float(h.v[i] * k);
};
for (auto &p : h.p) move(p);
move(h.elbow), move(h.pinchPoint), move(h.contact);
}
for (size_t k = 0; k < world_.size(); ++k) {
if (owner_[k] < 0) continue;
const Hand &h = seen_[owner_[k]];
const double a = gain(h.v) * ahead;
for (int i = 0; i < 3; ++i) world_[k].a[i] += float(h.v[i] * a), world_[k].b[i] += float(h.v[i] * a);
}
}
// the correction hook: Room View's picture of the hand, off by distance (off by default)
if (!shift_.empty() && !history_.empty()) {
const Mat &head = history_.back().head;
const double at[3] = {head.m[0][3], head.m[1][3], head.m[2][3]};
const double up[3] = {head.m[0][1], head.m[1][1], head.m[2][1]}, right[3] = {head.m[0][0], head.m[1][0], head.m[2][0]};
for (size_t k = 0; k < shown_.size(); ++k) {
Hand &h = shown_[k];
double palm[3];
h.Palm(palm);
const double d = std::sqrt((palm[0] - at[0]) * (palm[0] - at[0]) + (palm[1] - at[1]) * (palm[1] - at[1]) +
(palm[2] - at[2]) * (palm[2] - at[2]));
double yaw = shift_.front().yawDeg, pitch = shift_.front().pitchDeg;
for (size_t i = 1; i < shift_.size(); ++i) {
if (d <= shift_[i - 1].dist) break;
const Shift &a = shift_[i - 1], &b = shift_[i];
const double t = std::clamp((d - a.dist) / (b.dist - a.dist), 0.0, 1.0);
yaw = a.yawDeg + (b.yawDeg - a.yawDeg) * t, pitch = a.pitchDeg + (b.pitchDeg - a.pitchDeg) * t;
}
auto turn = [&](float p[3]) {
Turn(p, at, up, -yaw * M_PI / 180); // positive yaw: to the right
Turn(p, at, right, pitch * M_PI / 180);
};
for (auto &p : h.p) turn(p);
turn(h.elbow), turn(h.pinchPoint), turn(h.contact);
for (size_t c = 0; c < world_.size(); ++c)
if (owner_[c] == int(k)) turn(world_[c].a), turn(world_[c].b);
}
}
}
void Hands::Points(int64_t nowNs, bool predict, double leadMs, std::vector<HandPoints> &out) const {
out.clear();
const double ahead = std::clamp((nowNs + leadMs * 1e6 - captureNs_) / 1e9, 0.0, kMaxAhead);
for (const Hand &h : seen_) {
HandPoints hp{h.id, h.right, {}};
std::memcpy(hp.p, h.p, sizeof hp.p);
const double speed = h.speed();
const double gain = predict ? std::clamp((speed - kStillSpeed) / kStillSpeed, 0.0, 1.0) : 0.0; // as Advance
for (auto &p : hp.p)
for (int i = 0; i < 3; ++i) p[i] += float(h.v[i] * gain * ahead);
out.push_back(hp);
}
}
// ------------------------------------------------------------------------------- matte
namespace {
// A finger's half-width at each joint, metres, at the average hand's size: real hands
// (thumb and index about 20 mm across at the base, 15 at the tip), not ft-hands' capsules
// (which have a margin in them). The margin is added on top (MatteParams::Margin).
float JointRadius(int j) {
static const float r[21] = {0, 0.0125f, 0.0110f, 0.0095f, 0.0085f, // wrist, thumb
0.0105f, 0.0095f, 0.0085f, 0.0075f, // index
0.0108f, 0.0098f, 0.0088f, 0.0078f, // middle
0.0102f, 0.0092f, 0.0083f, 0.0074f, // ring
0.0092f, 0.0083f, 0.0076f, 0.0068f}; // little
return r[j];
}
struct V {
double x, y, z;
V(double a = 0, double b = 0, double c = 0) : x(a), y(b), z(c) {}
explicit V(const float p[3]) : x(p[0]), y(p[1]), z(p[2]) {}
V operator+(const V &o) const { return {x + o.x, y + o.y, z + o.z}; }
V operator-(const V &o) const { return {x - o.x, y - o.y, z - o.z}; }
V operator*(double k) const { return {x * k, y * k, z * k}; }
double dot(const V &o) const { return x * o.x + y * o.y + z * o.z; }
V cross(const V &o) const { return {y * o.z - z * o.y, z * o.x - x * o.z, x * o.y - y * o.x}; }
double norm() const { return std::sqrt(dot(*this)); }
V unit() const {
const double n = norm();
return n > 1e-9 ? *this * (1 / n) : V{};
}
};
std::array<float, 3> F3(const V &v) { return {float(v.x), float(v.y), float(v.z)}; }
} // namespace
void BuildMatte(const std::vector<Hand> &hands, const std::vector<Capsule> &fileCapsules, const double headPos[3],
const MatteParams &p, Matte &out) {
out.clear();
const V head(headPos[0], headPos[1], headPos[2]);
if (p.shape == MatteShape::Capsules) {
// ft-hands' capsules have a margin of 4 mm in them already; the angular margin is
// the mesh's (the capsule matte stays as it was, for comparison)
out.caps = fileCapsules;
return;
}
for (const Hand &h : hands) {
V P[21];
for (int j = 0; j < 21; ++j) P[j] = V(h.p[j]);
double palm[3];
h.Palm(palm);
const double dist = (V(palm[0], palm[1], palm[2]) - head).norm();
const double m = p.Margin(dist, h.speed());
const double k = std::clamp((P[9] - P[0]).norm() / kPalmLen, 0.7, 1.4); // this hand's size
if (p.shape == MatteShape::Circle) {
// the middle of what the head sees of the hand: the box around its joints (each
// with its half-width; the wrist's is the hand's) in a plane facing the head...
V w;
V u[21];
double rr[21], d[21];
for (int j = 0; j < 21; ++j) {
d[j] = std::max(1e-3, (P[j] - head).norm());
u[j] = (P[j] - head) * (1 / d[j]), w = w + u[j];
rr[j] = (j ? JointRadius(j) : 0.025) * k;
}
w = w.unit();
const V a = (std::fabs(w.y) < 0.9 ? V(0, 1, 0) : V(1, 0, 0)).cross(w).unit(), b = w.cross(a);
double lo[2] = {1e9, 1e9}, hi[2] = {-1e9, -1e9};
for (int j = 0; j < 21; ++j) {
const double f = u[j].dot(w), t = rr[j] / d[j];
if (f < 0.1) continue; // beside the head: not seen
const double x = u[j].dot(a) / f, y = u[j].dot(b) / f;
lo[0] = std::min(lo[0], x - t), hi[0] = std::max(hi[0], x + t);
lo[1] = std::min(lo[1], y - t), hi[1] = std::max(hi[1], y + t);
}
if (lo[0] > hi[0]) continue;
const V c = (w + a * ((lo[0] + hi[0]) / 2) + b * ((lo[1] + hi[1]) / 2)).unit();
// ...and the widest angle from there to the hand's edge, at the palm's distance
double reach = 0;
for (int j = 0; j < 21; ++j)
reach = std::max(reach, std::acos(std::clamp(u[j].dot(c), -1.0, 1.0)) + std::atan(rr[j] / d[j]));
const double r = dist * std::tan(std::min(reach, 1.2)) + m;
const V at = head + c * dist;
out.circles.push_back({{float(at.x), float(at.y), float(at.z)}, float(r)});
continue;
}
// fingers: a tapered capsule per bone; the tip's end pulled back (MediaPipe's tip
// point is the very end of the finger, a capsule's round end reaches past it)
static const int kBones[][2] = {{1, 2}, {2, 3}, {3, 4}, {5, 6}, {6, 7}, {7, 8}, {9, 10}, {10, 11},
{11, 12}, {13, 14}, {14, 15}, {15, 16}, {17, 18}, {18, 19}, {19, 20}};
for (const auto &b : kBones) {
const double ra = JointRadius(b[0]) * k, rb = JointRadius(b[1]) * k;
V a = P[b[0]], e = P[b[1]];
if (b[1] % 4 == 0) {
const V d = e - a;
const double n = d.norm();
if (n > 1e-6) e = e - d * (std::min(0.5 * rb, 0.4 * n) / n);
}
out.caps.push_back({{float(a.x), float(a.y), float(a.z)}, {float(e.x), float(e.y), float(e.z)}, float(ra + m),
float(rb + m)});
}
// the palm: a slab through the wrist and the knuckles, widened to the hand's edges
const V across = (P[17] - P[5]).unit(); // from the index side to the little finger's
const V fwd = (P[9] - P[0]).unit();
const V normal = across.cross(fwd).unit();
Hull palmHull;
const double half = 0.012 * k;
const V edge[] = {P[0] + across * (0.026 * k), P[0] - across * (0.020 * k), P[1],
P[5] - across * (0.008 * k), P[9], P[13],
P[17] + across * (0.009 * k), (P[0] + P[17]) * 0.5 + across * (0.014 * k)};
for (const V &q : edge)
for (double s : {-half, half}) palmHull.v.push_back(F3(q + normal * s));
out.hulls.push_back(palmHull), out.hullMargin.push_back(float(m));
// the thumb's web, between the thumb's base and the index knuckle
Hull web;
const double wh = 0.007 * k;
for (const V &q : {P[1], P[2], P[5] + (P[6] - P[5]) * 0.15, P[5]})
for (double s : {-wh, wh}) web.v.push_back(F3(q + normal * s));
out.hulls.push_back(web), out.hullMargin.push_back(float(m));
// the forearm: a stub of a tapered tube from the wrist toward the elbow (flatter than
// round). A stub, as the capsules' (16 cm): the elbow is a guess (13-15 degrees off in
// the forearm probe), and a longer piece costs a bigger redraw for little.
V dir = V(h.elbow) - P[0];
double len = dir.norm();
if (len < 0.05) continue;
dir = dir * (1 / len);
len = std::min(len, 0.18 * k);
V a = across - dir * across.dot(dir);
if (a.norm() < 1e-6) a = normal.cross(dir);
a = a.unit();
const V b = dir.cross(a).unit();
Hull arm;
const double rw[2] = {0.027 * k, 0.019 * k}, re[2] = {0.032 * k, 0.025 * k};
for (int ring = 0; ring < 2; ++ring) {
const V c = P[0] + dir * (ring ? len : 0.0);
const double *r = ring ? re : rw;
for (int i = 0; i < 8; ++i) {
const double t = i * M_PI / 4;
arm.v.push_back(F3(c + a * (r[0] * std::cos(t)) + b * (r[1] * std::sin(t))));
}
}
out.hulls.push_back(arm), out.hullMargin.push_back(float(m));
}
}
// ----------------------------------------------------------------------------- project
namespace {
// Where the line from eye e through point q (both panel-local) meets the panel, as texture
// pixels, and how much a size at q grows there. False if q isn't between the eye and it.
bool OnPanel(const Panel &p, const double e[3], const double q[3], double *x, double *y, double *grow) {
const double d[3] = {q[0] - e[0], q[1] - e[1], q[2] - e[2]};
double s, u, v;
if (p.curve <= 0) {
if (e[2] <= q[2] || q[2] <= 0) return false;
s = e[2] / (e[2] - q[2]);
u = e[0] + s * d[0];
v = e[1] + s * d[1];
} else {
// OpenVR bends a curved panel into a cylinder around (0, *, r), toward its front.
const double r = p.curve, ez = e[2] - r;
const double A = d[0] * d[0] + d[2] * d[2], B = 2 * (e[0] * d[0] + ez * d[2]), C = e[0] * e[0] + ez * ez - r * r;
const double disc = B * B - 4 * A * C;
if (A < 1e-12 || disc < 0) return false;
s = (-B + std::sqrt(disc)) / (2 * A); // the far side: the panel, seen from inside
const double px = e[0] + s * d[0], pz = e[2] + s * d[2];
if (pz > r) return false;
u = r * std::atan2(px, r - pz);
v = e[1] + s * d[1];
}
if (s <= 1) return false; // the hand is behind the panel
*x = (u / p.width + 0.5) * p.pxWidth;
*y = (0.5 - v / p.height) * p.pxHeight;
*grow = s;
return true;
}
constexpr double kEps = 0.002; // a hand pushed through the panel: only the part in front is cut
bool ProjectCapsule(const Panel &p, const Capsule &c, const double eye[3], Capsule2D *out) {
const double pxPerM = p.pxWidth / p.width;
double a[3], b[3];
const double wa[3] = {c.a[0], c.a[1], c.a[2]}, wb[3] = {c.b[0], c.b[1], c.b[2]};
ToLocal(p.pose, wa, a);
ToLocal(p.pose, wb, b);
// a hand pushed through the panel: keep the part in front
if (a[2] < kEps && b[2] < kEps) return false;
if (a[2] < kEps || b[2] < kEps) {
double *in = a[2] < kEps ? b : a, *out3 = a[2] < kEps ? a : b;
const double t = (in[2] - kEps) / (in[2] - out3[2]);
for (int i = 0; i < 3; ++i) out3[i] = in[i] + t * (out3[i] - in[i]);
}
// and the part near the eye's plane: it would land far across the panel with a
// huge radius, so one bad hand estimate there tears a hole through the screen
const double zmax = eye[2] - kNear;
if (a[2] > zmax && b[2] > zmax) return false;
if (a[2] > zmax || b[2] > zmax) {
double *in = a[2] > zmax ? b : a, *out3 = a[2] > zmax ? a : b;
const double t = (zmax - in[2]) / (out3[2] - in[2]);
for (int i = 0; i < 3; ++i) out3[i] = in[i] + t * (out3[i] - in[i]);
}
double ax, ay, ga, bx, by, gb;
if (!OnPanel(p, eye, a, &ax, &ay, &ga) || !OnPanel(p, eye, b, &bx, &by, &gb)) return false;
const float ra = float(c.ra * ga * pxPerM), rb = float(c.rb * gb * pxPerM);
const float r = std::max(ra, rb);
if (std::max(ax, bx) + r < 0 || std::min(ax, bx) - r > p.pxWidth || std::max(ay, by) + r < 0 ||
std::min(ay, by) - r > p.pxHeight)
return false;
*out = {float(ax), float(ay), float(bx), float(by), ra, rb};
return true;
}
// A disk facing the eye, as the eye sees it on the panel: an ellipse there (a circle on a
// panel square to the eye), drawn as the stadium around it (a capsule as long as the
// ellipse, as wide as it is narrow), with the capsules' soft edge.
bool ProjectCircle(const Panel &p, const Circle &k, const double eye[3], Capsule2D *out) {
double c[3];
const double w[3] = {k.c[0], k.c[1], k.c[2]};
ToLocal(p.pose, w, c);
if (c[2] < kEps || c[2] > eye[2] - kNear) return false; // behind the panel, or at the eye
double cx, cy, g;
if (!OnPanel(p, eye, c, &cx, &cy, &g)) return false;
const V e(eye[0], eye[1], eye[2]), at(c[0], c[1], c[2]), d = (at - e).unit();
const V a = (std::fabs(d.y) < 0.9 ? V(0, 1, 0) : V(1, 0, 0)).cross(d).unit(), b = d.cross(a);
// the rim, along the rays from the eye through it (a point near the eye on each: the
// rim may reach past the panel's plane, the ray still meets the panel)
constexpr int kRim = 16;
double rim[kRim][2];
int n = 0;
for (int i = 0; i < kRim; ++i) {
const double t = i * 2 * M_PI / kRim;
const V q = at + (a * std::cos(t) + b * std::sin(t)) * k.r;
if ((q - e).dot(d) <= 0) continue;
const V near = e + (q - e) * 0.1;
const double nq[3] = {near.x, near.y, near.z};
double x, y, gr;
if (OnPanel(p, eye, nq, &x, &y, &gr)) rim[n][0] = x - cx, rim[n][1] = y - cy, ++n;
}
if (n < kRim / 2) return false;
// the long axis: toward the farthest rim point
int far = 0;
for (int i = 1; i < n; ++i)
if (std::hypot(rim[i][0], rim[i][1]) > std::hypot(rim[far][0], rim[far][1])) far = i;
const double len = std::hypot(rim[far][0], rim[far][1]);
if (!(len > 0.5)) return false;
const double ux = rim[far][0] / len, uy = rim[far][1] / len;
double lo[2] = {1e9, 1e9}, hi[2] = {-1e9, -1e9};
for (int i = 0; i < n; ++i) {
const double s = rim[i][0] * ux + rim[i][1] * uy, t = -rim[i][0] * uy + rim[i][1] * ux;
lo[0] = std::min(lo[0], s), hi[0] = std::max(hi[0], s), lo[1] = std::min(lo[1], t), hi[1] = std::max(hi[1], t);
}
// the rim's points are 360/16 degrees apart: the ellipse reaches up to 2% past them
const double grow = 1 / std::cos(M_PI / kRim);
const double mx = cx + ux * (lo[0] + hi[0]) / 2 - uy * (lo[1] + hi[1]) / 2,
my = cy + uy * (lo[0] + hi[0]) / 2 + ux * (lo[1] + hi[1]) / 2;
const double A = (hi[0] - lo[0]) / 2 * grow, B = std::min(A, (hi[1] - lo[1]) / 2 * grow);
const double half = A - B;
if (mx + A < 0 || mx - A > p.pxWidth || my + A < 0 || my - A > p.pxHeight) return false;
*out = {float(mx - ux * half), float(my - uy * half), float(mx + ux * half), float(my + uy * half), float(B), float(B)};
return true;
}
// Keeps the part of a convex point set with lo <= z <= hi: the points inside, and where the
// segments between an inside and an outside point cross the plane (the clipped hull's corners
// are among them).
void ClipZ(std::vector<std::array<double, 3>> &pts, double plane, bool keepAbove) {
std::vector<std::array<double, 3>> out;
auto in = [&](const std::array<double, 3> &q) { return keepAbove ? q[2] >= plane : q[2] <= plane; };
for (size_t i = 0; i < pts.size(); ++i) {
if (in(pts[i])) {
out.push_back(pts[i]);
continue;
}
for (size_t j = 0; j < pts.size(); ++j) {
if (!in(pts[j])) continue;
const double t = (plane - pts[i][2]) / (pts[j][2] - pts[i][2]);
out.push_back({pts[i][0] + t * (pts[j][0] - pts[i][0]), pts[i][1] + t * (pts[j][1] - pts[i][1]), plane});
}
}
pts.swap(out);
}
double Cross2(const std::array<double, 2> &o, const std::array<double, 2> &a, const std::array<double, 2> &b) {
return (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0]);
}
// The convex hull (counterclockwise in x right, y down... either way: normals are set
// pointing away from the centroid), at most kMaxHullEdges corners.
std::vector<std::array<double, 2>> Hull2(std::vector<std::array<double, 2>> p) {
std::sort(p.begin(), p.end());
p.erase(std::unique(p.begin(), p.end(), [](auto &a, auto &b) { return std::fabs(a[0] - b[0]) < 1e-3 && std::fabs(a[1] - b[1]) < 1e-3; }),
p.end());
if (p.size() < 3) return p;
std::vector<std::array<double, 2>> h(2 * p.size());
size_t k = 0;
for (size_t i = 0; i < p.size(); ++i) {
while (k >= 2 && Cross2(h[k - 2], h[k - 1], p[i]) <= 0) --k;
h[k++] = p[i];
}
for (size_t i = p.size() - 1, t = k + 1; i > 0; --i) {
while (k >= t && Cross2(h[k - 2], h[k - 1], p[i - 1]) <= 0) --k;
h[k++] = p[i - 1];
}
h.resize(k - 1);
// too many corners: drop the ones that take the least area away
while (h.size() > size_t(kMaxHullEdges)) {
size_t best = 0;
double least = 1e300;
for (size_t i = 0; i < h.size(); ++i) {
const double a = std::fabs(Cross2(h[(i + h.size() - 1) % h.size()], h[i], h[(i + 1) % h.size()]));
if (a < least) least = a, best = i;
}
h.erase(h.begin() + long(best));
}
return h;
}
constexpr float kHullFeather = 2.0f; // pixels either side of a hull's edge
bool ProjectHull(const Panel &p, const Hull &hull, float margin, const double eye[3], Hull2D *out) {
const double pxPerM = p.pxWidth / p.width;
std::vector<std::array<double, 3>> pts;
for (const auto &q : hull.v) {
const double w[3] = {q[0], q[1], q[2]};
double l[3];
ToLocal(p.pose, w, l);
pts.push_back({l[0], l[1], l[2]});
}
ClipZ(pts, kEps, true); // the part in front of the panel
if (pts.size() < 3) return false;
ClipZ(pts, eye[2] - kNear, false); // ...and not near the eye's plane (see ProjectCapsule)
if (pts.size() < 3) return false;
std::vector<std::array<double, 2>> flat;
double grow = 0;
for (const auto &q : pts) {
double x, y, g;
if (!OnPanel(p, eye, q.data(), &x, &y, &g)) continue;
flat.push_back({x, y});
grow = std::max(grow, g);
}
const auto h = Hull2(flat);
if (h.size() < 3) return false;
double cx = 0, cy = 0;
for (const auto &q : h) cx += q[0] / h.size(), cy += q[1] / h.size();
Hull2D o;
o.r = float(margin * grow * pxPerM);
// the edges, in order around it (edge i from corner i to i + 1), normals pointing out
std::vector<std::array<double, 2>> corner;
for (size_t i = 0; i < h.size(); ++i) {
const auto &a = h[i], &b = h[(i + 1) % h.size()];
double nx = b[1] - a[1], ny = -(b[0] - a[0]);
const double n = std::hypot(nx, ny);
if (n < 1e-6) continue;
nx /= n, ny /= n;
double c = nx * a[0] + ny * a[1];
if (nx * cx + ny * cy - c > 0) nx = -nx, ny = -ny, c = -c; // point away from the middle
o.e[o.n][0] = float(nx), o.e[o.n][1] = float(ny), o.e[o.n][2] = float(c);
corner.push_back(a);
++o.n;
}
if (o.n < 3) return false;
// what's drawn: each corner pushed out along its bisector so the edges move out by `reach`
// (the margin and the feather), at most 3 times that at a sharp corner
const double reach = o.r + kHullFeather + 1;
o.box[0] = o.box[1] = 1e9f, o.box[2] = o.box[3] = -1e9f;
for (int i = 0; i < o.n; ++i) {
const float *n1 = o.e[(i + o.n - 1) % o.n], *n2 = o.e[i];
double bx = n1[0] + n2[0], by = n1[1] + n2[1];
const double bl = std::hypot(bx, by);
double k = reach;
if (bl > 1e-6) {
bx /= bl, by /= bl;
k = reach / std::max(1.0 / 3, bx * n2[0] + by * n2[1]);
} else {
bx = n2[0], by = n2[1];
}
o.fan[i][0] = float(corner[i][0] + bx * k), o.fan[i][1] = float(corner[i][1] + by * k);
o.box[0] = std::min(o.box[0], o.fan[i][0]), o.box[1] = std::min(o.box[1], o.fan[i][1]);
o.box[2] = std::max(o.box[2], o.fan[i][0]), o.box[3] = std::max(o.box[3], o.fan[i][1]);
}
if (o.box[2] < 0 || o.box[0] > p.pxWidth || o.box[3] < 0 || o.box[1] > p.pxHeight) return false;
*out = o;
return true;
}
} // namespace
bool Project(const Panel &p, const std::vector<Capsule> &caps, const double eyes[2][3], std::vector<Capsule2D> out[2]) {
bool any = false;
for (int e = 0; e < 2; ++e) {
out[e].clear();
double eye[3];
ToLocal(p.pose, eyes[e], eye);
if (eye[2] <= 0.01) continue; // behind the panel
for (const Capsule &c : caps) {
Capsule2D d;
if (ProjectCapsule(p, c, eye, &d)) out[e].push_back(d), any = true;
}
}
return any;
}
bool Project(const Panel &p, const Matte &m, const double eyes[2][3], Spots out[2]) {
bool any = false;
for (int e = 0; e < 2; ++e) {
out[e].clear();
double eye[3];
ToLocal(p.pose, eyes[e], eye);
if (eye[2] <= 0.01) continue; // behind the panel
for (const Circle &c : m.circles) {
Capsule2D d;
if (ProjectCircle(p, c, eye, &d)) out[e].caps.push_back(d), any = true;
}
for (const Capsule &c : m.caps) {
Capsule2D d;
if (ProjectCapsule(p, c, eye, &d)) out[e].caps.push_back(d), any = true;
}
for (size_t k = 0; k < m.hulls.size(); ++k) {
Hull2D d;
if (ProjectHull(p, m.hulls[k], k < m.hullMargin.size() ? m.hullMargin[k] : 0, eye, &d))
out[e].hulls.push_back(d), any = true;
}
}
return any;
}
} // namespace handcut
Reference in new issue View Git Blame Copy Permalink