steam-frame/Frametop--frametop
1
0 Fork 0
mirror of https://github.com/DeeJanuz/frametop.git synced 2026-10-26 21:01:13 +01:00
Code Issues Packages Projects Releases 22 Wiki Activity
Files
7dfdfd24e1603ed65671b60274afac72ed00a211
Frametop--frametop/screens/grabthat.cpp
T
DeeJanuzandClaude Opus 5.5 e4bc1fbc6e hands: grab that and its glow aim where the open palm faces 
The line from the eyes through the palm lit panels off to the side of a palm
that wasn't facing them. The line is now the palm's own, along its normal,
with the margins seen from the palm. HANDS_GRAB_THAT_AIM=eyes brings the old
line back.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
2026-10-13 16:36:01 -06:00

458 lines
20 KiB
C++
Raw Blame History

// "Grab that": an open hand held out to a window, then a fist pulled back (see grabthat.h).
#include "grabthat.h"
#include "palmmenu.h"
#include <algorithm>
#include <cmath>
#include <cstdio>
namespace grabthat {
namespace {
using handcut::Hand;
double Dot(const double a[3], const double b[3]) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; }
double Dist3(const float a[3], const float b[3]) {
const double d[3] = {double(a[0]) - b[0], double(a[1]) - b[1], double(a[2]) - b[2]};
return std::sqrt(Dot(d, d));
}
// A finger's extension: its tip's distance from the wrist over its knuckle's (`mcp`: 5 index,
// 9 middle, 13 ring, 17 little), as handinput's.
double Extension(const Hand &h, int mcp) { return Dist3(h.p[mcp + 3], h.p[0]) / std::max(1e-6, Dist3(h.p[mcp], h.p[0])); }
void Forward(const Mat &head, double out[3]) { out[0] = -head.m[0][2], out[1] = -head.m[1][2], out[2] = -head.m[2][2]; }
// A pose at c facing the point `at` (upright), as handinput's.
Mat Facing(const double c[3], const double at[3]) {
double z[3] = {at[0] - c[0], at[1] - c[1], at[2] - c[2]};
const double zl = std::sqrt(Dot(z, z));
if (zl < 1e-6) z[0] = 0, z[1] = 0, z[2] = 1;
else
for (int i = 0; i < 3; ++i) z[i] /= zl;
double x[3] = {z[2], 0, -z[0]};
const double xl = std::hypot(x[0], x[2]);
if (xl < 1e-6) x[0] = 1, x[2] = 0;
else x[0] /= xl, x[2] /= xl;
const double y[3] = {z[1] * x[2] - z[2] * x[1], z[2] * x[0] - z[0] * x[2], z[0] * x[1] - z[1] * x[0]};
Mat m{};
for (int i = 0; i < 3; ++i) m.m[i][0] = float(x[i]), m.m[i][1] = float(y[i]), m.m[i][2] = float(z[i]), m.m[i][3] = float(c[i]);
return m;
}
struct Quat {
double w, x, y, z;
};
Quat ToQuat(const Mat &m) {
const double t = m.m[0][0] + m.m[1][1] + m.m[2][2];
Quat q;
if (t > 0) {
const double s = std::sqrt(t + 1) * 2;
q = {s / 4, (m.m[2][1] - m.m[1][2]) / s, (m.m[0][2] - m.m[2][0]) / s, (m.m[1][0] - m.m[0][1]) / s};
} else if (m.m[0][0] > m.m[1][1] && m.m[0][0] > m.m[2][2]) {
const double s = std::sqrt(1 + m.m[0][0] - m.m[1][1] - m.m[2][2]) * 2;
q = {(m.m[2][1] - m.m[1][2]) / s, s / 4, (m.m[0][1] + m.m[1][0]) / s, (m.m[0][2] + m.m[2][0]) / s};
} else if (m.m[1][1] > m.m[2][2]) {
const double s = std::sqrt(1 + m.m[1][1] - m.m[0][0] - m.m[2][2]) * 2;
q = {(m.m[0][2] - m.m[2][0]) / s, (m.m[0][1] + m.m[1][0]) / s, s / 4, (m.m[1][2] + m.m[2][1]) / s};
} else {
const double s = std::sqrt(1 + m.m[2][2] - m.m[0][0] - m.m[1][1]) * 2;
q = {(m.m[1][0] - m.m[0][1]) / s, (m.m[0][2] + m.m[2][0]) / s, (m.m[1][2] + m.m[2][1]) / s, s / 4};
}
const double l = std::sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
return {q.w / l, q.x / l, q.y / l, q.z / l};
}
void FromQuat(const Quat &q, Mat *m) {
const double w = q.w, x = q.x, y = q.y, z = q.z;
const double r[3][3] = {{1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)},
{2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)},
{2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)}};
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j) m->m[i][j] = float(r[i][j]);
}
} // namespace
Features Measure(const Hand &h, const Mat &head) {
Features f;
double e[4];
for (int k = 0; k < 4; ++k) e[k] = Extension(h, 5 + 4 * k);
f.open = (e[0] + e[1] + e[2] + e[3]) / 4, f.curl3 = (e[1] + e[2] + e[3]) / 3;
double fw[3], n[3];
Forward(head, fw);
palmmenu::PalmNormal(h, n);
f.palmFwd = Dot(n, fw);
const double w[3] = {h.p[0][0] - head.m[0][3], h.p[0][1] - head.m[1][3], h.p[0][2] - head.m[2][3]};
f.fwd = Dot(w, fw);
double palm[3];
h.Palm(palm);
double a[3] = {palm[0] - head.m[0][3], palm[1] - head.m[1][3], palm[2] - head.m[2][3]};
const double al = std::sqrt(Dot(a, a));
f.aim = al > 1e-6 ? Dot(n, a) / al : 0;
return f;
}
void Detector::Reset() {
for (Side &sd : side_) sd = Side{};
fired_.clear();
}
void Detector::Quiet(int64_t until) {
for (Side &sd : side_) {
sd.quietUntil = std::max(sd.quietUntil, until);
if (sd.state != State::Idle) sd.state = State::Idle, sd.recent.clear();
}
}
void Detector::Open(Side &sd, int64_t t, const Hand &h, const Mat &head) {
sd.lastOpen = t;
for (int i = 0; i < 3; ++i) sd.eye[i] = head.m[i][3];
h.Palm(sd.palm);
palmmenu::PalmNormal(h, sd.normal);
}
void Detector::Frame(int64_t t, const std::vector<Hand> &hands, const Mat &head, const bool allowed[2]) {
fired_.clear();
for (int s = 0; s < 2; ++s) {
Side &sd = side_[s];
if (!p_.on || !allowed[s]) {
sd.state = State::Idle, sd.recent.clear(), sd.claimUntil = 0;
continue;
}
// the hand it follows, else that side's most confident one
const Hand *h = nullptr;
if (sd.state != State::Idle)
for (const Hand &c : hands)
if (c.id == sd.handId) h = &c;
if (!h)
for (const Hand &c : hands)
if (c.right == (s == 1) && (!h || c.confidence > h->confidence)) h = &c;
if (!h || !h->stereo) { // one camera's depth is 2-6 cm off: it neither arms nor fires
if (sd.state != State::Idle && t - sd.lastSeen > int64_t(p_.lostMs * 1e6)) sd.state = State::Idle, sd.recent.clear();
continue;
}
if (h->id != sd.handId) sd.recent.clear();
sd.lastSeen = t, sd.handId = h->id, sd.head = head;
const Features f = Measure(*h, head);
sd.thumb = Dist3(h->p[4], h->p[8]);
sd.recent.push_back({t, f.fwd, f.palmFwd});
while (!sd.recent.empty() && t - sd.recent.front().t > int64_t(p_.pullMs * 1e6)) sd.recent.erase(sd.recent.begin());
double palmNow[3];
h->Palm(palmNow);
const double facingDeg = std::acos(std::clamp(f.aim, -1.0, 1.0)) * 180 / M_PI;
const bool enter = f.open >= p_.openArm && f.palmFwd >= p_.palmArm && f.fwd >= p_.fwdArm;
const bool keep = f.open >= p_.openKeep && f.palmFwd >= p_.palmKeep && f.fwd >= p_.fwdKeep;
const bool closing = f.curl3 < p_.claimCurl;
const bool stale = t - sd.lastOpen > int64_t(p_.closeMs * 1e6);
// the arming hold: from here, its facing summed and where the palm started
auto hold = [&] {
sd.since = t, sd.facingSum = 0, sd.facingN = 0;
std::copy(palmNow, palmNow + 3, sd.holdFrom);
};
switch (sd.state) {
case State::Idle:
if (enter && t >= sd.quietUntil) {
sd.state = State::Opening, hold(), Open(sd, t, *h, head);
sd.facingSum += facingDeg, ++sd.facingN;
}
break;
case State::Opening: {
if (!keep || t < sd.quietUntil) {
sd.state = State::Idle;
break;
}
Open(sd, t, *h, head);
sd.facingSum += facingDeg, ++sd.facingN;
if (t - sd.since < int64_t(p_.armMs * 1e6)) break;
// held armMs: facing along the aim on average (Params::aimFacing), and still (the palm's
// way over it, Params::holdSpeed); else the hold starts again from here
const double secs = (t - sd.since) / 1e9;
const double d[3] = {palmNow[0] - sd.holdFrom[0], palmNow[1] - sd.holdFrom[1], palmNow[2] - sd.holdFrom[2]};
const double facing = sd.facingSum / std::max(1, sd.facingN), moved = std::sqrt(Dot(d, d)) / std::max(1e-3, secs);
if ((p_.aimFacing > 0 && facing > p_.aimFacing) || (p_.holdSpeed > 0 && moved > p_.holdSpeed)) {
++stats.unsteady;
hold();
break;
}
sd.facing = facing, sd.speed = moved;
sd.state = State::Armed, ++stats.arms;
break;
}
case State::Armed:
case State::Closing:
if (sd.state == State::Armed) {
if (keep && !closing) {
Open(sd, t, *h, head);
break;
}
if (!closing) { // turned away or lowered, still open: armed a little longer
if (stale) sd.state = State::Idle;
break;
}
double palm = f.palmFwd; // closing on the window: the palm toward it as the fingers began
for (const Side::Sample &q : sd.recent)
if (t - q.t <= int64_t(p_.closePalmMs * 1e6)) palm = std::max(palm, q.palm);
if (palm < p_.closePalm) {
sd.state = State::Idle;
break;
}
sd.state = State::Closing, ++stats.closings;
} else if (enter && !closing) { // opened out again
sd.state = State::Armed;
Open(sd, t, *h, head);
break;
}
if (stale) {
sd.state = State::Idle, ++stats.timeouts;
break;
}
double farthest = f.fwd;
for (const Side::Sample &q : sd.recent) farthest = std::max(farthest, q.fwd);
if (f.open <= p_.fist && farthest - f.fwd >= p_.pull && f.palmFwd <= p_.palmFlip) {
Fire x;
x.side = s, x.t = t, x.head = head, x.facing = sd.facing, x.speed = sd.speed;
std::copy(sd.eye, sd.eye + 3, x.eye), std::copy(sd.palm, sd.palm + 3, x.palm);
std::copy(sd.normal, sd.normal + 3, x.normal);
h->Palm(x.fist);
fired_.push_back(x);
++stats.fires;
sd.state = State::Idle, sd.recent.clear();
sd.quietUntil = t + int64_t(p_.againMs * 1e6), sd.claimUntil = t + int64_t(p_.claimMs * 1e6);
}
break;
}
}
}
bool Detector::Aim(int side, Fire *out, double *progress) const {
const Side &sd = side_[side];
if (!p_.on || sd.state == State::Idle) return false;
out->side = side, out->t = sd.lastSeen, out->head = sd.head;
std::copy(sd.eye, sd.eye + 3, out->eye), std::copy(sd.palm, sd.palm + 3, out->palm);
std::copy(sd.normal, sd.normal + 3, out->normal);
for (int i = 0; i < 3; ++i) out->fist[i] = sd.palm[i];
*progress = sd.state == State::Opening ? std::clamp((sd.lastSeen - sd.since) / (p_.armMs * 1e6), 0.0, 1.0) : 1.0;
return true;
}
bool Detector::claims(int side, int64_t t) const {
const Side &sd = side_[side];
return p_.on && ((sd.state == State::Closing && sd.thumb >= p_.claimThumb) || t < sd.claimUntil);
}
std::string Detector::Describe() const {
static const char *names[] = {"idle", "opening", "armed", "closing"};
return std::string("left ") + names[int(side_[0].state)] + ", right " + names[int(side_[1].state)];
}
bool RayHit(const Target &p, const double from[3], const double dir[3], double *u, double *v, double *t) {
// the ray in the panel's frame
const double d0[3] = {from[0] - p.pose.m[0][3], from[1] - p.pose.m[1][3], from[2] - p.pose.m[2][3]};
double o[3], d[3];
for (int i = 0; i < 3; ++i) {
o[i] = p.pose.m[0][i] * d0[0] + p.pose.m[1][i] * d0[1] + p.pose.m[2][i] * d0[2];
d[i] = p.pose.m[0][i] * dir[0] + p.pose.m[1][i] * dir[1] + p.pose.m[2][i] * dir[2];
}
if (p.curve <= 0) {
if (std::fabs(d[2]) < 1e-9) return false;
*t = -o[2] / d[2];
if (*t <= 0) return false;
*u = o[0] + *t * d[0], *v = o[1] + *t * d[1];
return true;
}
// the cylinder's axis at (0, *, r), the panel the part of it nearest the origin (Locate's)
const double r = p.curve, a = d[0] * d[0] + d[2] * d[2], b = 2 * (o[0] * d[0] + (o[2] - r) * d[2]),
c = o[0] * o[0] + (o[2] - r) * (o[2] - r) - r * r, disc = b * b - 4 * a * c;
if (a < 1e-12 || disc < 0) return false;
const double q = std::sqrt(disc), roots[2] = {(-b - q) / (2 * a), (-b + q) / (2 * a)};
for (double k : roots) {
const double x = o[0] + k * d[0], z = o[2] + k * d[2];
if (k <= 0 || r - z <= 0) continue;
*t = k, *u = r * std::atan2(x, r - z), *v = o[1] + k * d[1];
return true;
}
return false;
}
double OffDeg(const Target &p, const double from[3], const double dir[3]) {
// the outline, sampled (n steps an edge): seen from outside it, the nearest point is on it
const int n = 48;
double best = 180;
for (int e = 0; e < 4; ++e)
for (int i = 0; i <= n; ++i) {
const double s = double(i) / n - 0.5;
const double u = e < 2 ? s * p.width : (e == 2 ? -0.5 : 0.5) * p.width;
const double v = e < 2 ? (e == 0 ? -0.5 : 0.5) * p.height : s * p.height;
double x = u, z = 0; // (on a curved one along its curve, as RayHit)
if (p.curve > 0) x = p.curve * std::sin(u / p.curve), z = p.curve * (1 - std::cos(u / p.curve));
double d[3];
for (int r = 0; r < 3; ++r) d[r] = p.pose.m[r][0] * x + p.pose.m[r][1] * v + p.pose.m[r][2] * z + p.pose.m[r][3] - from[r];
const double l = std::sqrt(Dot(d, d));
if (l < 1e-6) continue;
best = std::min(best, std::acos(std::clamp(Dot(d, dir) / l, -1.0, 1.0)) * 180 / M_PI);
}
return best;
}
int Pick(const std::vector<Target> &panels, const Fire &f, const Params &k, Closest *closest) {
// the line: from the palm along its normal (Params::aimPalm), or from the eyes through the palm
const double *from = k.aimPalm ? f.palm : f.eye;
double dir[3];
for (int i = 0; i < 3; ++i) dir[i] = k.aimPalm ? f.normal[i] : f.palm[i] - f.eye[i];
const double l = std::sqrt(Dot(dir, dir));
int best = -1;
double bestT = 1e9;
// (not one near the eyes: Params::nearest)
auto near = [&](const Target &p) {
return k.nearest > 0 && std::hypot(p.pose.m[0][3] - f.eye[0], p.pose.m[1][3] - f.eye[1], p.pose.m[2][3] - f.eye[2]) < k.nearest;
};
if (l > 1e-6) {
for (double &x : dir) x /= l;
// one the line meets (within aimMargin), the nearest
for (const Target &p : panels) {
double u, v, t;
if (near(p) || !RayHit(p, from, dir, &u, &v, &t)) continue;
if (std::fabs(u) <= p.width / 2 + k.aimMargin && std::fabs(v) <= p.height / 2 + k.aimMargin && t < bestT) {
bestT = t, best = p.index;
if (closest) closest->index = p.index, closest->deg = 0;
}
}
// else the one it passes nearest, within aimMarginDeg
// (a panel that looks small, e.g. a little window pushed far out, gets a wider margin: up to
// smallDeg more for one that spans nothing, less as it spans more; user, 2026-10-11)
if (best < 0) {
double bestScore = 1e9;
for (const Target &p : panels) {
if (near(p)) continue;
const double off = OffDeg(p, from, dir);
if (closest && off < closest->deg) closest->index = p.index, closest->deg = off;
const double dist = std::max(0.1, std::hypot(p.pose.m[0][3] - from[0], p.pose.m[1][3] - from[1],
p.pose.m[2][3] - from[2]));
const double half = std::atan(std::max(p.width, p.height) / 2 / dist) * 180 / M_PI;
const double extra = std::max(0.0, k.smallDeg - half);
if (off <= k.aimMarginDeg + extra && off - extra < bestScore) bestScore = off - extra, best = p.index;
}
}
}
if (best >= 0 || k.headCone <= 0) return best;
// where the head points
double fw[3];
Forward(f.head, fw);
double bestA = k.headCone;
for (const Target &p : panels) {
if (near(p)) continue;
double to[3] = {p.pose.m[0][3] - f.head.m[0][3], p.pose.m[1][3] - f.head.m[1][3], p.pose.m[2][3] - f.head.m[2][3]};
const double tl = std::sqrt(Dot(to, to));
if (tl < 1e-6) continue;
const double a = std::acos(std::clamp(Dot(to, fw) / tl, -1.0, 1.0)) * 180 / M_PI;
if (a <= bestA) bestA = a, best = p.index;
}
return best;
}
Mat Arrival(const Fire &f, const Params &k) {
double fw[3];
Forward(f.head, fw);
double hx = fw[0], hz = fw[2];
const double hl = std::hypot(hx, hz);
if (hl < 1e-6) hx = 0, hz = -1;
else hx /= hl, hz /= hl;
const double eye[3] = {f.head.m[0][3], f.head.m[1][3], f.head.m[2][3]};
const double c[3] = {eye[0] + hx * k.distance, std::clamp(f.fist[1], eye[1] - k.below, eye[1] + k.above),
eye[2] + hz * k.distance};
return Facing(c, eye);
}
Outcome Decide(bool came, const Mat &now, const Mat &head, const Params &k) {
const double d = std::hypot(now.m[0][3] - head.m[0][3], now.m[1][3] - head.m[1][3], now.m[2][3] - head.m[2][3]);
const bool near = d <= k.distance + k.reachSlack;
return near ? (came ? Outcome::Back : Outcome::Centre) : Outcome::Bring;
}
const char *OutcomeName(Outcome o) {
switch (o) {
case Outcome::Back: return "goes back";
case Outcome::Centre: return "is near already: centred at arm's reach";
default: return "comes to arm's reach";
}
}
Mat Blend(const Mat &a, const Mat &b, double k) {
Quat qa = ToQuat(a), qb = ToQuat(b);
double c = qa.w * qb.w + qa.x * qb.x + qa.y * qb.y + qa.z * qb.z;
if (c < 0) qb = {-qb.w, -qb.x, -qb.y, -qb.z}, c = -c;
double wa = 1 - k, wb = k;
if (c < 0.9995) {
const double th = std::acos(c), s = std::sin(th);
wa = std::sin((1 - k) * th) / s, wb = std::sin(k * th) / s;
}
Quat q{wa * qa.w + wb * qb.w, wa * qa.x + wb * qb.x, wa * qa.y + wb * qb.y, wa * qa.z + wb * qb.z};
const double l = std::sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
q = {q.w / l, q.x / l, q.y / l, q.z / l};
Mat m{};
FromQuat(q, &m);
for (int i = 0; i < 3; ++i) m.m[i][3] = float(a.m[i][3] + (b.m[i][3] - a.m[i][3]) * k);
return m;
}
double Ease(double t) {
t = std::clamp(t, 0.0, 1.0);
return t * t * (3 - 2 * t);
}
Mat InFront(const Mat &head, double distance, double drop) {
double fw[3];
Forward(head, fw);
double hx = fw[0], hz = fw[2];
const double hl = std::hypot(hx, hz);
if (hl < 1e-6) hx = 0, hz = -1;
else hx /= hl, hz /= hl;
const double eye[3] = {head.m[0][3], head.m[1][3], head.m[2][3]};
const double c[3] = {eye[0] + hx * distance, eye[1] - drop, eye[2] + hz * distance};
return Facing(c, eye);
}
double ViewDistance(double width, double maxDeg, double nearest, double farthest, double *fit) {
const double t = std::tan(std::clamp(maxDeg, 1.0, 170.0) * M_PI / 360);
const double d = std::clamp(width / (2 * t), nearest, std::max(nearest, farthest));
if (fit) *fit = std::min(width, 2 * d * t);
return d;
}
Readable ReadableSize(double pxW, double pxH, double distance, const ReadParams &k) {
Readable r;
if (pxW < 1 || pxH < 1 || distance <= 0) return r;
const double ppd = std::max(1.0, k.ppd);
double w = pxW / ppd, h = pxH / ppd;
const double maxW = pxW / pxH >= kWideAspect ? std::max(k.maxW, k.maxWideW) : k.maxW;
const double shrink = std::min({1.0, maxW / w, k.maxH / h});
w *= shrink, h *= shrink;
if (w < k.minW) h *= k.minW / w, w = k.minW;
r.degW = w, r.degH = h, r.ppd = pxW / w;
if (w <= k.flatDeg) {
r.width = 2 * distance * std::tan(w * M_PI / 360);
} else {
r.width = distance * w * M_PI / 180; // round the eyes: the arc
r.curve = distance;
}
return r;
}
int SusanPick(const std::vector<Bearing> &panels, int current, double from, bool next, double aheadDeg) {
int best = -1;
double bestA = 1e9;
for (const Bearing &b : panels) {
if (b.index == current) continue;
if (current < 0 && std::fabs(b.deg) <= aheadDeg) continue; // (in front already)
double a = (next ? 1 : -1) * (b.deg - from);
while (a <= 1e-6) a += 360; // round past where it started
while (a > 360) a -= 360;
if (a < bestA) bestA = a, best = b.index;
}
return best;
}
} // namespace grabthat
Reference in new issue View Git Blame Copy Permalink