From 7066b591ff58c39d8e4759a63d173e2c47b72b0a Mon Sep 17 00:00:00 2001 From: DeeJanuz <45082401+DeeJanuz@users.noreply.github.com> Date: Fri, 2 Oct 2026 11:44:34 -0600 Subject: [PATCH] Pointer helper: one function per main loop section Structure only, no behaviour change: every frame does the same work in the same order. main() now sets up and runs a short loop that calls the sections, each a Pointer member named after its comment: HeadsetOff, GazeAwake, RelayCommands, ReadFrame, ClaimPulse, PauseOverlayList, LaserMode, HandRole, LastUsedWins, Recenter, GazeMode, Hands, GazePrecisionButtons, KeyboardClicks, CalPanelOpened, HeadSteer, PinchesAndGrips, HandsStopped, MarkHeld, HeadFollow, SlowWork, Cursor, HeldBackPress, KeyboardClickHold, Click, PollControllerButtons, SteamVRQuit - The values the sections share within a frame (the poses, hmdRaw, tnow, eye, and handOk from PinchesAndGrips for HandsStopped) are in a Frame, read by ReadFrame at the same point as before. eye moved there from just after "last used wins"; it only depends on the poses, read once. Sections open with local aliases (hmd, hm, tnow, eye), so the bodies are the old code, dedented. - None of the loop's 43 continues skipped the rest of a frame: 25 were in the relay's recvfrom loop and 18 in the controller, press, and overlay scans. The recvfrom loop's body is now Command(), where they're returns. The quit event's "return 0" is SteamVRQuit() returning true. - The loop's lambdas are members too: HandAngles, EndHold, StartHold, Steer, BeginHold, HeadAngles (taking tnow or the HMD pose as a parameter), and Nearest with its Hit type. - The cursor section is Cursor(): Tilt() while tilting; otherwise FindCursor() (collision, drag lock, occlusion; returns a Spot), DrawDot() (SteamVR Settings, our dot), and SendRay() (the controller pose). Built with -Wextra -Wshadow too: no warnings. The three shadowing warnings the old main() had ("from" twice, "now") are gone with the separate scopes. Co-Authored-By: Claude Opus 5.5 --- pointer/helper/ft-pointer.cpp | 2294 ++++++++++++++++++--------------- 1 file changed, 1264 insertions(+), 1030 deletions(-) diff --git a/pointer/helper/ft-pointer.cpp b/pointer/helper/ft-pointer.cpp index ff740b0..e29d542 100644 --- a/pointer/helper/ft-pointer.cpp +++ b/pointer/helper/ft-pointer.cpp @@ -659,6 +659,17 @@ constexpr double kPlaceDegPerSec = 60; // tested: 40 deg/s is applied exact using Clock = std::chrono::steady_clock; +// One frame's poses and time: ReadFrame reads them after the relay's commands, and the sections +// after it share them. handOk: PinchesAndGrips read the hand gestures (for HandsStopped). +struct Frame { + vr::TrackedDevicePose_t all[vr::k_unMaxTrackedDeviceCount]; + vr::TrackedDevicePose_t hmdRaw; + Clock::time_point tnow; + Vec3 eye; + bool handOk = false; + const vr::TrackedDevicePose_t &Hmd() const { return all[0]; } +}; + // Everything the pointer keeps: its settings, what Init sets up, and the state the main loop // carries from one frame to the next. struct Pointer { @@ -813,7 +824,61 @@ struct Pointer { std::string FindPanel(const char *key, Panel &p, Vec3 &eye); void GrabProbe(const char *key); std::string Place(const char *key, Vec3 target, const Basis &bt, double grabBelow); - int Run(); + // Nearest's answer: the nearest overlay along a ray. + struct Hit { + double along = 1e9; + std::string key; + bool scene = false; + Vec3 point, normal; + }; + // Where the cursor is this frame (FindCursor), for DrawDot and SendRay. + struct Spot { + bool dragging = false; + Vec3 dir, point; + double best = 1e9, distance = 0; + bool onEdge = false, occluded = false, onScene = false, onPanel = false; + }; + + // The main loop's sections, in the order main calls them, and what they share. + void HeadsetOff(); + void GazeAwake(); + void RelayCommands(); + void Command(const char *buf, const sockaddr_un &sender, socklen_t senderLen); + void ReadFrame(Frame &frame); + void ClaimPulse(const Frame &frame); + void PauseOverlayList(); + void LaserMode(); + void HandRole(const Frame &frame); + void LastUsedWins(const Frame &frame); + void Recenter(const Frame &frame); + void GazeMode(const Frame &frame); + void Hands(const Frame &frame); + bool HandAngles(const float p[3], uint64_t t_ns, const Vec3 &from, double &hy, double &hp); + void EndHold(bool lost); + void StartHold(bool press, Clock::time_point tnow); + void Steer(double hy, double hp, double deadzone, double gain, Clock::time_point tnow); + void BeginHold(int side, bool grip, const fh_pinch_t &g, Clock::time_point tnow); + void GazePrecisionButtons(const Frame &frame); + bool HeadAngles(const vr::TrackedDevicePose_t &hmd, double &hy, double &hp); + void KeyboardClicks(const Frame &frame); + void CalPanelOpened(); + void HeadSteer(const Frame &frame); + void PinchesAndGrips(Frame &frame); + void HandsStopped(const Frame &frame); + void MarkHeld(const Frame &frame); + void HeadFollow(const Frame &frame); + void SlowWork(); + void Cursor(const Frame &frame); + void Tilt(const Frame &frame); + Hit Nearest(Vec3 from, Vec3 d); + Spot FindCursor(const Frame &frame); + void DrawDot(const Frame &frame, const Spot &spot); + void SendRay(const Frame &frame, const Spot &spot); + void HeldBackPress(const Frame &frame); + void KeyboardClickHold(const Frame &frame); + void Click(const Frame &frame); + void PollControllerButtons(); + bool SteamVRQuit(); }; // After cfg.Load: a changed POINTER_FOLLOW or POINTER_GAZE turns head follow or gaze mode on or off. @@ -1206,1039 +1271,1208 @@ std::string Pointer::Place(const char *key, Vec3 target, const Basis &bt, double } -int Pointer::Run() { - while (true) { - // The headset off: SteamVR drops the HMD's activity to idle as soon as it comes off. - // An awake pointer (a connected controller, SteamVR's laser mode forced on) kept the - // displays from sleeping, so it's released at once, and the mouse can't wake it until - // the headset is back on (then the first mouse input does). - const auto level = sys->GetTrackedDeviceActivityLevel(vr::k_unTrackedDeviceIndex_Hmd); - headsetOff = level == vr::k_EDeviceActivityLevel_Idle || level == vr::k_EDeviceActivityLevel_Standby || - level == vr::k_EDeviceActivityLevel_Idle_Timeout; - if (headsetOff && active) { - active = false; - claimPending = claimHeld = false; - overlay->HideOverlay(cursor); - overlay->HideOverlay(marker); - SendTo(out, "ft_pointer", "btn a 0"); - SendTo(out, "ft_pointer", "hide"); - std::printf("headset off: pointer released\n"); - std::fflush(stdout); - } - - // Outside games, gaze mode keeps the pointer (see the top); the relay needs to know. - { - const auto t = Clock::now(); - if (t - inGameAt > std::chrono::milliseconds(500)) { - inGameAt = t; - inGame = vr::VRApplications()->GetCurrentSceneProcessId() != 0; - } - // Hand gestures in the last two minutes keep it the same way. - const bool handsRecent = cfg.handsOn && handUsed != Clock::time_point{} && t - handUsed < std::chrono::minutes(2); - const bool awake = (gazeOn || handsRecent) && !inGame && !headsetOff; - if (awake != gazeAwake || t - gazeAwakeAt > std::chrono::seconds(5)) { - if (awake != gazeAwake) - std::printf("%s keeps the pointer: %s\n", gazeOn ? "gaze" : "hand use", - awake ? "yes" : "no (off, in a game, or headset off)"); - if (awake != gazeAwake) std::fflush(stdout); - gazeAwake = awake; - gazeAwakeAt = t; - SendTo(out, "frametop_relay", awake ? "gazeawake 1" : "gazeawake 0"); - } - } - - // Commands from the relay. - char buf[256]; - ssize_t n; - sockaddr_un from{}; - socklen_t fromLen = sizeof from; - while ((n = recvfrom(in, buf, sizeof buf - 1, 0, reinterpret_cast(&from), &fromLen)) > 0) { - buf[n] = 0; - // Reply to the sender (the layout tool binds an abstract address to get answers). - const sockaddr_un sender = from; - const socklen_t senderLen = fromLen; - fromLen = sizeof from; - auto reply = [&](const std::string &msg) { - if (senderLen > offsetof(sockaddr_un, sun_path)) - sendto(out, msg.data(), msg.size(), 0, reinterpret_cast(&sender), senderLen); - }; - // The gaze, from ft-gazed: not mouse input, it never wakes the pointer. - double g[4]; - if (std::sscanf(buf, "gz %lf %lf %lf %lf", &g[0], &g[1], &g[2], &g[3]) == 4) { - gz = {g[0], g[1], g[2], g[3], Clock::now()}; - continue; - } - // The gaze calibration panel (see the top): presses answer it instead of clicking. - { - int on; - if (std::sscanf(buf, "calpanel %d", &on) == 1) { - const bool was = Clock::now() < calPanelUntil; - calPanelUntil = on ? Clock::now() + std::chrono::seconds(3) : Clock::time_point{}; - if (on && !was) calOpened = true; - continue; - } - } - if (Clock::now() < calPanelUntil) { - const bool accept = !std::strncmp(buf, "btn trigger 1", 13) || !std::strncmp(buf, "gazekey left 1", 14); - const bool quit = !std::strncmp(buf, "btn b 1", 7) || !std::strncmp(buf, "gazekey right 1", 15); - if (accept || quit) { - SendTo(out, "ft_gazed", accept ? "calaccept" : "calquit"); - continue; - } - if (!std::strncmp(buf, "btn ", 4) || !std::strncmp(buf, "gazekey ", 8) || - !std::strncmp(buf, "precision ", 10) || !std::strncmp(buf, "gazedrag ", 9)) - continue; // their releases, and the other buttons: nothing to click now - } - { - char kind[16], source[16]; - int v; - if (std::sscanf(buf, "%15s %15s %d", kind, source, &v) == 3 && - (!std::strcmp(kind, "precision") || !std::strcmp(kind, "gazedrag"))) { - lastMouse = Clock::now(); - if (!active) Wake(Clock::now()); - devicePresses.push_back({source, !std::strcmp(kind, "gazedrag"), v != 0}); - continue; - } - if (std::sscanf(buf, "gazekey %15s %d", source, &v) == 2 && - (!std::strcmp(source, "left") || !std::strcmp(source, "right"))) { - lastMouse = Clock::now(); - if (!active) Wake(Clock::now()); - keyPresses.push_back({!std::strcmp(source, "right"), v != 0}); - continue; - } - } - if (std::strcmp(buf, "typing") == 0) { // not mouse input: it never wakes the pointer - lastTyping = Clock::now(); - continue; - } - if (std::strncmp(buf, "vrbind", 6) == 0) { - std::printf("controller buttons: %s\n", controllerButtons.Bind(buf + 6).c_str()); - std::fflush(stdout); - continue; - } - if (std::strncmp(buf, "vrglobal", 8) == 0) { - const char *arg = buf + 8; - while (*arg == ' ') ++arg; - ControllerButtons::SetGlobal(std::strncmp(arg, "off", 3) != 0); - reply(controllerButtons.Status()); - continue; - } - if (std::strncmp(buf, "vrstatus", 8) == 0) { - reply(controllerButtons.Status()); - continue; - } - if (std::strncmp(buf, "overlays", 8) == 0) { - overlays.Request(sender, senderLen); // answered from the list's thread - continue; - } - if (std::strncmp(buf, "gaze", 4) == 0) { - const char *arg = buf + 4; - while (*arg == ' ') ++arg; - if (*arg != '?') { // "gaze ?" only asks - gazeOn = std::strncmp(arg, "on", 2) == 0 ? true - : std::strncmp(arg, "off", 3) == 0 ? false - : !gazeOn; - gazeOwns = true, nudging = false; - std::printf("gaze mode %s\n", gazeOn ? "on" : "off"); - std::fflush(stdout); - } - reply(gazeOn ? "ok on" : "ok off"); - continue; - } - const bool mouseInput = std::strncmp(buf, "move", 4) == 0 || std::strncmp(buf, "btn", 3) == 0 || - std::strncmp(buf, "scroll", 6) == 0; - // A move held back (POINTER_GAZE_MOUSE_MOVE=held) only wakes the pointer: it isn't using - // the mouse, so a drifting mouse doesn't keep the gaze from taking the pointer back. - const bool moveHeldBack = std::strncmp(buf, "move", 4) == 0 && MouseMoveHeld(); - if (mouseInput && !moveHeldBack) lastMouse = Clock::now(); - // Any mouse input wakes the pointer (after a controller took over, or a helper restart). - if (!active && mouseInput) Wake(Clock::now()); - if (moveHeldBack) continue; - double a, b; - char key[128]; - double px, py, pz, pyaw, ppitch, proll = 0, pgrab = -1; - if (std::sscanf(buf, "grabprobe %127s", key) == 1) { - GrabProbe(key); - continue; - } - if (std::sscanf(buf, "place %127s %lf %lf %lf %lf %lf %lf %lf", key, &px, &py, &pz, &pyaw, &ppitch, &proll, - &pgrab) >= 6) { - reply(Place(key, {px, py, pz}, PanelBasis(pyaw, ppitch, proll), pgrab >= 0 ? pgrab : cfg.grabOffset)); - continue; - } - if (std::sscanf(buf, "measure %127s", key) == 1) { - Panel p; - Vec3 eye; - const std::string err = FindPanel(key, p, eye); - char msg[400] = ""; - if (err.empty()) - std::snprintf(msg, sizeof msg, "ok %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f", - p.center.x, p.center.y, p.center.z, p.width, p.height, p.basis.x.x, p.basis.x.y, - p.basis.x.z, p.basis.y.x, p.basis.y.y, p.basis.y.z, p.basis.z.x, p.basis.z.y, - p.basis.z.z); - reply(err.empty() ? msg : "error " + err); - continue; - } - if (std::strncmp(buf, "head", 4) == 0) { - vr::TrackedDevicePose_t h; - sys->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding, 0, &h, 1); - const Vec3 e = Position(h.mDeviceToAbsoluteTracking); - const Vec3 f = Rotate(h.mDeviceToAbsoluteTracking, {0, 0, -1}); - char msg[200]; - std::snprintf(msg, sizeof msg, "ok %.4f %.4f %.4f %.2f %.2f", e.x, e.y, e.z, - std::atan2(-f.x, -f.z) * 180 / M_PI, std::asin(std::clamp(f.y, -1.0, 1.0)) * 180 / M_PI); - reply(h.bPoseIsValid ? msg : "error no head pose (headset off?)"); - continue; - } - if (std::strncmp(buf, "debug", 5) == 0) { - debug = !debug; - std::printf("debug %s\n", debug ? "on" : "off"); - std::fflush(stdout); - continue; - } - if (std::strncmp(buf, "btn trigger 1", 13) == 0) { - LeftButton(true); - continue; - } else if (std::strncmp(buf, "btn trigger 0", 13) == 0) { - LeftButton(false); - continue; - } else if (std::strncmp(buf, "btn b 1", 7) == 0 && RightButton(true)) { - continue; - } else if (std::strncmp(buf, "btn b 0", 7) == 0 && RightButton(false)) { - continue; - } else if (std::strncmp(buf, "btn b 1", 7) == 0 && leftHeld) { - tilting = tiltStart = swallowedRight = true; // right press while dragging: tilt, no right-click - continue; - } else if (std::strncmp(buf, "btn b 0", 7) == 0 && swallowedRight) { - tilting = swallowedRight = false; - continue; - } - if (tilting && std::sscanf(buf, "move %lf %lf", &a, &b) == 2) { - tiltYaw += a; - tiltPitch = std::clamp(tiltPitch + b, -80.0, 80.0); - continue; - } - if (std::sscanf(buf, "move %lf %lf", &a, &b) == 2) { - lastMove = Clock::now(); // the dot shows while the mouse moves it (gaze mode) - if (gazeOn && gazeOwns) { - // The mouse takes the pointer from the gaze, from where the gaze left it. - gazeOwns = false; - nudging = haveHead && Clock::now() - gz.at < std::chrono::milliseconds(200); - nudgeRawHy = gz.rhy, nudgeRawHp = gz.rhp, nudgeHead = lastHead; - nudgeAt = Clock::now(), nudgeMoved = 0; - } - if (nudging || aimHeld) nudgeMoved += std::hypot(a, b); - if (!anchored) recenter = true; - yaw += a; - while (yaw > 180) yaw -= 360; - while (yaw < -180) yaw += 360; - pitch = std::clamp(pitch + b, -85.0, 85.0); - } else if (std::strncmp(buf, "recenter", 8) == 0) { - recenter = true; - } else if (std::strncmp(buf, "reload", 6) == 0) { - cfg.Load(ReadConfig()); - ApplyConfig(); - lastSlow = Clock::now() - std::chrono::seconds(10); // apply POINTER_IGNORE now - std::printf("reloaded: free distance %.2f m, dot %.2f deg, origin %.2f, head follow %s, leash %.0f deg, " - "controller pickup %.1fx, %zu ignored\n", - cfg.freeDistance, cfg.cursorDeg, cfg.originFraction, follow ? "on" : "off", cfg.leashDeg, cfg.pickupScale, - cfg.ignore.size()); - std::fflush(stdout); - } else if (std::strncmp(buf, "follow", 6) == 0) { - const char *arg = buf + 6; - while (*arg == ' ') ++arg; - const bool was = follow; - follow = std::strncmp(arg, "on", 2) == 0 ? true : std::strncmp(arg, "off", 3) == 0 ? false : !follow; - if (follow && !was) followReset = true; - std::printf("head follow %s (leash %.0f deg)\n", follow ? "on" : "off", cfg.leashDeg); - std::fflush(stdout); - } else if (std::strncmp(buf, "show", 4) == 0) { - if (!active) Wake(Clock::now()); - } else if (std::strncmp(buf, "hide", 4) == 0) { - active = false; - overlay->HideOverlay(cursor); - overlay->HideOverlay(marker); - SendTo(out, "ft_pointer", "hide"); - } else { - SendTo(out, "ft_pointer", buf); // btn, scroll - } - } - - vr::TrackedDevicePose_t all[vr::k_unMaxTrackedDeviceCount]; - sys->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding, 0.011f, all, vr::k_unMaxTrackedDeviceCount); - const vr::TrackedDevicePose_t &hmd = all[0]; - vr::TrackedDevicePose_t hmdRaw; - sys->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0.011f, &hmdRaw, 1); - const auto tnow = Clock::now(); - - // Claim pulse (switchlaserhand on the driver's "a" button, no click). - if (claimPending && tnow >= claimAt) { - SendTo(out, "ft_pointer", "btn a 1"); - claimPending = false; - claimHeld = true; - claimRelease = tnow + std::chrono::milliseconds(60); - } else if (claimHeld && tnow >= claimRelease) { - SendTo(out, "ft_pointer", "btn a 0"); - claimHeld = false; - } - - // The overlay list is only needed while the pointer is awake (see OverlayList). - overlays.SetPaused(!active || headsetOff); - - // Laser mode on while the pointer is awake. - if (active != laserModeShown) { - laserModeShown = active; - if (active) overlay->ShowOverlay(laserMode); - else overlay->HideOverlay(laserMode); - if (debug) std::printf("laser mode %s\n", active ? "forced on" : "released"); - if (debug) std::fflush(stdout); - } - - // Didn't get the hand role (a held controller keeps it): release, back off. - if (active && tnow - wokeAt > std::chrono::seconds(1) && ours != vr::k_unTrackedDeviceIndexInvalid && - sys->GetControllerRoleForTrackedDeviceIndex(ours) == vr::TrackedControllerRole_Invalid) { - active = false; - claimPending = claimHeld = false; - overlay->HideOverlay(cursor); - overlay->HideOverlay(marker); - SendTo(out, "ft_pointer", "btn a 0"); - SendTo(out, "ft_pointer", "hide"); - noWakeUntil = tnow + std::chrono::seconds(2); - std::printf("no hand role (a controller is in use): pointer released\n"); - std::fflush(stdout); - } - - // Last used wins: a real controller being moved releases the pointer (see the top), - // in gaze mode too. - if (active && hold.src == Src::None && tnow - lastMouse > std::chrono::milliseconds(500)) { - for (vr::TrackedDeviceIndex_t i = 1; i < vr::k_unMaxTrackedDeviceCount; ++i) { - if (i == ours || !all[i].bPoseIsValid || all[i].eTrackingResult != vr::TrackingResult_Running_OK || - sys->GetTrackedDeviceClass(i) != vr::TrackedDeviceClass_Controller) { - movingSince[i] = {}; - continue; - } - const auto &v = all[i].vVelocity.v, &w = all[i].vAngularVelocity.v; - const double speed = std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); - const double spin = std::sqrt(w[0] * w[0] + w[1] * w[1] + w[2] * w[2]); - if (speed <= 0.35 * cfg.pickupScale && spin <= 2.0 * cfg.pickupScale) { - movingSince[i] = {}; - continue; - } - if (movingSince[i] == Clock::time_point{}) movingSince[i] = tnow; - if (tnow - movingSince[i] >= std::chrono::milliseconds(100)) { - active = false; - claimPending = claimHeld = false; - overlay->HideOverlay(cursor); - overlay->HideOverlay(marker); - SendTo(out, "ft_pointer", "btn a 0"); - SendTo(out, "ft_pointer", "hide"); - std::printf("controller %u moved (%.2f m/s, %.1f rad/s): pointer released\n", i, speed, spin); - std::fflush(stdout); - break; - } - } - } else { - std::fill(std::begin(movingSince), std::end(movingSince), Clock::time_point{}); - } - const auto &hm = hmd.mDeviceToAbsoluteTracking.m; - const Vec3 eye{hm[0][3], hm[1][3], hm[2][3]}; - - if (recenter && hmd.bPoseIsValid) { - anchor = eye; - const Vec3 f{-hm[0][2], -hm[1][2], -hm[2][2]}; - yaw = std::atan2(-f.x, -f.z) * 180 / M_PI; - pitch = std::asin(std::clamp(f.y, -1.0, 1.0)) * 180 / M_PI; - anchored = true; - recenter = false; - followReset = true; - } - - // Gaze mode (see the top): the gaze has the pointer, or takes it back when you look - // well away from it. Not while a press holds the pointer, and only on fresh gaze. - if (hmd.bPoseIsValid) lastHead = hmd.mDeviceToAbsoluteTracking, haveHead = true; - if (gazeOn && active && hmd.bPoseIsValid && !tilting && !leftHeld && !aimHeld && !clickPress && !clickRelease && - tnow >= dropHoldUntil && - tnow - gz.at < std::chrono::milliseconds(150)) { - const Vec3 g = Rotate(hmd.mDeviceToAbsoluteTracking, Direction(gz.hy, gz.hp)); - if (!gazeOwns && havePoint) { - const double off = std::acos(std::clamp(Dot(g, Normalize(lastPoint - eye)), -1.0, 1.0)) * 180 / M_PI; - if (off > cfg.gazeRetake && tnow - lastMouse > std::chrono::milliseconds(300)) { - if (retakeSince == Clock::time_point{}) retakeSince = tnow; - if (tnow - retakeSince >= std::chrono::milliseconds(120)) gazeOwns = true, nudging = false; - } else { - retakeSince = {}; - } - } - if (gazeOwns) { - retakeSince = {}; - anchor = eye; - anchored = true; - yaw = std::atan2(-g.x, -g.z) * 180 / M_PI; - pitch = std::clamp(std::asin(std::clamp(g.y, -1.0, 1.0)) * 180 / M_PI, -85.0, 85.0); - } - } - - // Hands (see the top): pinches and grips from ft-hands. - { - vr::TrackedDevicePose_t h0; - sys->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding, 0, &h0, 1); - if (h0.bPoseIsValid) poses.Add(tnow, h0.mDeviceToAbsoluteTracking); - } - // Where a gesture's point is, seen from the hold's origin: yaw and pitch, degrees. - auto handAngles = [&](const float p[3], uint64_t t_ns, const Vec3 &from, double &hy, double &hp) { - vr::HmdMatrix34_t head; - if (!poses.At(t_ns, head)) return false; - const Vec3 d = Normalize(Position(head) + Rotate(head, Vec3{p[0], p[1], p[2]}) - from); - hy = std::atan2(-d.x, -d.z) * 180 / M_PI; - hp = std::asin(std::clamp(d.y, -1.0, 1.0)) * 180 / M_PI; - return true; - }; - auto endHold = [&](bool lost) { - if (hold.pressed) { - if (leftHeld) ReleaseLeft(); - } else if (aimHeld) { - aimHeld = aimHand = false; - if (lost) { - if (gazeOn) gazeOwns = true, nudging = false; // no click: the pointer goes back to the gaze - } else { - clickPress = true; // the click is on the release, where the pointer is now - pressRight = hold.right; - gazeBack = nudgeMoved < 0.2; - } - } - if (debug) - std::printf("hold %s %s%s\n", hold.src == Src::Hand ? (hold.grip ? "grip" : "pinch") - : hold.src == Src::Head ? (hold.right ? "key right" : "key left") - : hold.grip ? "gaze drag" : "gaze precision", - lost ? "lost" : "released", hold.pressed ? "" : lost ? " (no click)" : " (click)"); - if (debug) std::fflush(stdout); - hold = {}; - }; - // The press, once a hold's source is set: a real press (dragging until the release), or - // held back like gaze mode's mouse press (see the top): the click comes on the release. - auto startHold = [&](bool press) { - hold.startYaw = hold.lastYaw = yaw, hold.startPitch = hold.lastPitch = pitch; - hold.pressed = press; - if (press) { - gazeBack = gazeOn && gazeOwns; - PressLeft(); - } else { - gazeOwns = false; - nudging = gazeOn && haveHead && tnow - gz.at < std::chrono::milliseconds(200); - nudgeRawHy = gz.rhy, nudgeRawHp = gz.rhp, nudgeHead = lastHead; - nudgeAt = aimSince = tnow, nudgeMoved = 0; - aimHeld = aimHand = true; - pulseAt = tnow; - } - }; - // The hold's source moved to (hy, hp): past the dead zone, the pointer follows at the gain, - // from where it was when it got past. - auto steer = [&](double hy, double hp, double deadzone, double gain) { - const double dy = std::remainder(hy - hold.refYaw, 360.0), dp = hp - hold.refPitch; - if (!hold.engaged) { - if (std::hypot(dy, dp) <= deadzone) return; - hold.engaged = true; - hold.refYaw = hy, hold.refPitch = hp; - hold.startYaw = hold.lastYaw = yaw, hold.startPitch = hold.lastPitch = pitch; - return; - } - yaw = std::remainder(hold.startYaw + gain * dy, 360.0); - pitch = std::clamp(hold.startPitch + gain * dp, -85.0, 85.0); - // How far the nudge went: for a keyboard click, from where the dot was at the press - // (a head wobbles on the way); otherwise the path, as with the mouse. - if (!hold.pressed && hold.src == Src::Head) - nudgeMoved = std::hypot(std::remainder(yaw - hold.pressYaw, 360.0), pitch - hold.pressPitch); - else if (!hold.pressed) - nudgeMoved += std::hypot(std::remainder(yaw - hold.lastYaw, 360.0), pitch - hold.lastPitch); - hold.lastYaw = yaw, hold.lastPitch = pitch; - lastMove = tnow; // the dot shows while it moves (gaze mode) - }; - auto beginHold = [&](int side, bool grip, const fh_pinch_t &g) { - handUsed = tnow; - if (hold.src != Src::None) { - // A grip takes over a pinch on the way to it (closing the hand passes through - // one); otherwise one hold at a time. - if (hold.src != Src::Hand || !grip || hold.grip) return; - aimHeld = aimHand = false; - hold = {}; - } - if (leftHeld || aimHeld || clickPress || clickRelease) return; // the mouse's button is busy - if (!active) { - Wake(tnow); // the first gesture only wakes the pointer, like the first mouse move - return; - } - vr::HmdMatrix34_t head; - if (!poses.At(g.begin_ns, head)) return; - // No pinch just after a key (typing touches thumb to index), and a grip only with - // the hand held up (hands on a desk curl like a loose fist; looking down at them - // puts them straight ahead in the head's frame, where ft-hands can't tell). - const Vec3 at = Position(head) + Rotate(head, Vec3{g.begin_point[0], g.begin_point[1], g.begin_point[2]}); - if (!grip && tnow - lastTyping < std::chrono::duration(cfg.typingHold)) { - if (debug) std::printf("hand pinch ignored: typing\n"); - return; - } - if (grip && Position(head).y - at.y > cfg.handBelow) { - if (debug) std::printf("hand grip ignored: %.2f m below the eyes\n", Position(head).y - at.y); - return; - } - Hold h; - h.src = Src::Hand, h.side = side, h.grip = grip, h.origin = Position(head); - if (!handAngles(g.begin_point, g.begin_ns, h.origin, h.refYaw, h.refPitch)) return; - hold = h; - // A grip, or a pinch without gaze mode: a real press where the pointer is (where - // you look, in gaze mode), dragging with the hand until it opens. - startHold(grip || !gazeOn); - if (debug) std::printf("hand %s %s began\n", side ? "right" : "left", grip ? "grip" : "pinch"); - if (debug) std::fflush(stdout); - }; - // Gaze precision and gaze drag buttons (see the top): the mouse steers. - for (const DevicePress &p : devicePresses) { - if (p.source == "left" || p.source == "right") continue; // no controllers in gaze mode - if (!p.down) { - if (hold.src == Src::Mouse) endHold(false); - continue; - } - if (hold.src != Src::None || leftHeld || aimHeld || clickPress || clickRelease) continue; - Hold h; - h.src = Src::Mouse, h.grip = p.drag; - hold = h; - startHold(p.drag); - if (debug) std::printf("hold gaze %s began (%s)\n", p.drag ? "drag" : "precision", p.source.c_str()); - if (debug) std::fflush(stdout); - } - devicePresses.clear(); - // Keyboard clicks (see the top): held back at the gaze, steered by the head. - auto headAngles = [&](double &hy, double &hp) { - if (!hmd.bPoseIsValid) return false; - const Vec3 f = Normalize({-hm[0][2], -hm[1][2], -hm[2][2]}); - hy = std::atan2(-f.x, -f.z) * 180 / M_PI; - hp = std::asin(std::clamp(f.y, -1.0, 1.0)) * 180 / M_PI; - return true; - }; - for (const KeyPress &k : keyPresses) { - (k.right ? keyRightDown : keyLeftDown) = k.down; - if (!k.down) { - if (keyTilting && k.right) { // the tilt ends; the head drags again from here - keyTilting = tilting = false; - hold.engaged = false; - if (debug) std::printf("hold key left: tilt ended\n"); - if (debug) std::fflush(stdout); - } - // The keys holding it: a gaze_left then gaze_right drag, either; otherwise its own. - const bool held = hold.keyDrag ? keyLeftDown || keyRightDown : hold.right ? keyRightDown : keyLeftDown; - if (hold.src == Src::Head && !held) { - if (!hold.pressed && aimHeld && tnow - aimSince < std::chrono::duration(cfg.keyTap)) { - // A quick tap: a click where the dot was at the press, and the gaze was right. - yaw = hold.pressYaw, pitch = hold.pressPitch; - nudgeMoved = 0; - confirmLesson = true; - } - endHold(false); - keyTilting = false; - } - continue; - } - if (k.right && hold.src == Src::Head && !hold.right && hold.pressed && leftHeld) { - // gaze_right during a gaze_left drag: tilt while it's held (see the top). - tilting = tiltStart = keyTilting = true; - headAngles(keyTiltYaw, keyTiltPitch); - if (debug) std::printf("hold key left: tilt began\n"); - if (debug) std::fflush(stdout); - continue; - } - if (k.right && hold.src == Src::Head && !hold.right && !hold.pressed && aimHeld) { - // gaze_right while gaze_left aims: press the left button where the dot is now - // (the correction is a lesson), then the head drags. - aimHeld = aimHand = false; - hold.pressed = hold.grip = hold.keyDrag = true, hold.promote = false, hold.engaged = false; - headAngles(hold.refYaw, hold.refPitch); - gazeBack = gazeOn; - pressRight = false; - PressLeft(); - if (debug) std::printf("hold key left: pressed (right key)\n"); - if (debug) std::fflush(stdout); - continue; - } - if (hold.src != Src::None || leftHeld || aimHeld || clickPress || clickRelease) continue; - Hold h; - h.src = Src::Head, h.promote = true, h.right = k.right, h.pressYaw = yaw, h.pressPitch = pitch; - if (!headAngles(h.refYaw, h.refPitch)) continue; - hold = h; - startHold(false); - if (debug) std::printf("hold key %s began\n", k.right ? "right" : "left"); - if (debug) std::fflush(stdout); - } - keyPresses.clear(); - if (calOpened) { // the calibration panel came up: a press in progress ends, no click - calOpened = false; - if (hold.src != Src::None) endHold(true); - if (leftHeld) ReleaseLeft(); - aimHeld = aimHand = clickPress = false; - } - if (hold.src == Src::Head) { - double hy, hp; - if (headAngles(hy, hp) && keyTilting) { - // The head turns the panel, as the mouse does in a tilt; the pointer stays put. - tiltYaw += std::remainder(hy - keyTiltYaw, 360.0); - tiltPitch = std::clamp(tiltPitch + hp - keyTiltPitch, -80.0, 80.0); - keyTiltYaw = hy, keyTiltPitch = hp; - } else if (headAngles(hy, hp)) { - steer(hy, hp, hold.pressed ? 0.3 : cfg.headDeadzone, 1.0); - } - } - fh_gestures_t hg; - const bool handOk = cfg.handsOn && handFile.Read(hg); - if (handOk && (hg.seq != handSeq || handFile.opens != handOpens)) { - handSeq = hg.seq; - handPublished = hg.publish_ns; - const fh_pinch_t *slots[2] = {hg.pinch, hg.grip}; - // A new file, or a tracker whose counters went back: start counting from here. - bool rebase = !handBaseline || handFile.opens != handOpens; - for (int k = 0; k < 2; ++k) - for (int s = 0; s < 2; ++s) - rebase = rebase || slots[k][s].begins < seenBegins[k][s] || slots[k][s].ends < seenEnds[k][s]; - if (rebase) { - if (hold.src == Src::Hand) endHold(true); - for (int k = 0; k < 2; ++k) - for (int s = 0; s < 2; ++s) seenBegins[k][s] = slots[k][s].begins, seenEnds[k][s] = slots[k][s].ends; - handBaseline = true, handOpens = handFile.opens; - } - // Not over a VR game (unless the dashboard is up), and not with the headset off. - const bool allowed = !headsetOff && (!inGame || overlay->IsDashboardVisible()); - for (int k = 1; k >= 0; --k) // grips first: one takes over a pinch - for (int s = 0; s < 2; ++s) { - const fh_pinch_t &g = slots[k][s]; - const bool began = g.begins != seenBegins[k][s], ended = g.ends != seenEnds[k][s]; - const bool lost = g.flags & FH_PINCH_LOST; - seenBegins[k][s] = g.begins, seenEnds[k][s] = g.ends; - auto holding = [&] { return hold.src == Src::Hand && hold.side == s && hold.grip == (k == 1); }; - if (holding() && ended) endHold(lost); // the one held ended (another may have begun) - if (began && allowed) { - beginHold(s, k == 1, g); - // begun and ended since the last read (a quick tap): its release too - if (!(g.flags & FH_PINCH_DOWN) && holding()) endHold(lost); - } - } - // The held gesture's hand moves the pointer: past the dead zone (a tap's jitter, - // the pinch point shifting as the fingers close), then at the gain, from there. - if (hold.src == Src::Hand) { - const fh_pinch_t &g = hold.grip ? hg.grip[hold.side] : hg.pinch[hold.side]; - double hy, hp; - if ((g.flags & FH_PINCH_TRACKED) && handAngles(g.point, hg.capture_ns, hold.origin, hy, hp)) - steer(hy, hp, hold.grip ? 0.5 : cfg.pinchDeadzone, hold.grip ? cfg.gripGain : cfg.pinchGain); - } - } - // ft-hands stopped (or hung) with a gesture held: it's over, as lost. - const uint64_t nowNs = - uint64_t(std::chrono::duration_cast(tnow.time_since_epoch()).count()); - if (hold.src == Src::Hand && (!handOk || !active || nowNs - handPublished > 1'500'000'000ull)) endHold(true); - if (hold.src != Src::None && hold.src != Src::Hand && !active) endHold(true); - - if (aimHeld || leftHeld || clickPress || clickRelease) lastHeld = tnow; - - // Head follow (see the top): past the leash (for the delay), ease the reference to the - // head's facing, and turn the cursor with it. Not in gaze mode: the gaze places it. - if (follow && !gazeOn && active && anchored && hmd.bPoseIsValid) { - const Vec3 head = LimitPitch(Vec3{-hm[0][2], -hm[1][2], -hm[2][2]}, 85); - if (followReset) { - followRef = head, followLag = 0, leashOutSince = {}; - followReset = following = false; - } - const double dt = std::min(0.1, std::chrono::duration(tnow - followAt).count()); - const double leash = cfg.leashDeg * M_PI / 180; - const double lag = std::acos(std::clamp(Dot(followRef, head), -1.0, 1.0)); - double keep = lag; // how far the reference stays behind the head after this frame - if (leash <= 0) { - keep = 0; // head-locked - } else if (following) { - keep = lag * (cfg.leashReturn > 0 ? std::exp(-dt / cfg.leashReturn) : 0.0); - // A fast turn drags it at the leash's end; past it already (after the delay), it - // can't fall further behind, and it closes in from there without a jump. - keep = std::min(keep, std::max(leash, followLag)); - if (keep < 0.05 * M_PI / 180) keep = 0, following = false; // landed on the facing - } else if (lag > leash) { - if (leashOutSince == decltype(leashOutSince){}) leashOutSince = tnow; - if (std::chrono::duration(tnow - leashOutSince).count() >= cfg.leashDelay) - following = true, leashOutSince = {}; - } else { - leashOutSince = {}; // back inside before the delay: a glance - } - followLag = keep; - const Vec3 ref = LimitPitch(PullWithin(followRef, head, keep), 85); - if (!leftHeld && tnow >= dropHoldUntil) { - // The cursor keeps its offset from the reference (frames without roll). - Vec3 d = FromBasis(AimBasis(ref), ToBasis(AimBasis(followRef), Direction(yaw, pitch))); - d = PullWithin(Normalize(d), ref, cfg.followReach * M_PI / 180); - yaw = std::atan2(-d.x, -d.z) * 180 / M_PI; - pitch = std::clamp(std::asin(std::clamp(d.y, -1.0, 1.0)) * 180 / M_PI, -85.0, 85.0); - // The ray origin closes in on the eye as the reference does on the facing. - anchor = eye + (anchor - eye) * (leash <= 0 ? 0.0 : lag > 1e-6 ? keep / lag : following ? 0.0 : 1.0); - } - followRef = ref; - } - followAt = tnow; - - // Slow work, once a second: overlay handles, our device index, laser width. - const auto now = std::chrono::steady_clock::now(); - if (now - lastSlow > std::chrono::seconds(1)) { - lastSlow = now; - handles.clear(); - for (const auto &key : overlays.Keys()) { - if (Ignored(cfg.ignore, key)) continue; - vr::VROverlayHandle_t h; - if (overlay->FindOverlay(key.c_str(), &h) != vr::VROverlayError_None) continue; - handles[key] = h; - // Scene-graph overlays are placed absolutely. Other overlays can report no - // texture too (gamescope's app panels, placed as dashboard tabs, share theirs - // from another process), and ComputeOverlayIntersection handles those. - uint32_t tw = 0, th = 0; - overlay->GetOverlayTextureSize(h, &tw, &th); - vr::VROverlayTransformType tt = vr::VROverlayTransform_Invalid; - overlay->GetOverlayTransformType(h, &tt); - // ft-screens' panels (frametop.screen.N, floating windows with their popups, - // frametop.float.N..., the keyboard) are 0x0 and absolute too (a shared - // texture), but they're real panels of any size. - sceneGraph[key] = (tw == 0 || th == 0) && tt == vr::VROverlayTransform_Absolute && - key.rfind("frametop.", 0) != 0; - } - ours = vr::k_unTrackedDeviceIndexInvalid; - for (vr::TrackedDeviceIndex_t i = 0; i < vr::k_unMaxTrackedDeviceCount; ++i) { - char type[64] = ""; - sys->GetStringTrackedDeviceProperty(i, vr::Prop_ControllerType_String, type, sizeof type); - if (std::strcmp(type, "ft_pointer") == 0) ours = i; - } - - } - - if (now - lastVisible > std::chrono::milliseconds(50) || visible.size() != handles.size()) { - lastVisible = now; - visible.clear(); - for (const auto &[key, h] : handles) visible[key] = overlay->IsOverlayVisible(h); - } - - if (active && anchored && hmd.bPoseIsValid && tilting) { - // Rotate the device around the grab point; the grabbed panel turns with it. - if (tiltStart) { - pivot = lastPoint; - tiltOrigin = lastOrigin; - tiltBasis = AimBasis(lastAim); - tiltStart = false; // angles carry on from any earlier tilt in this drag - overlay->HideOverlay(cursor); - overlay->HideOverlay(marker); - } - const double yr = tiltYaw * M_PI / 180, pr = tiltPitch * M_PI / 180; - const Vec3 up{0, 1, 0}, side = tiltBasis.x; - auto turn = [&](Vec3 v) { return RotateAbout(RotateAbout(v, side, pr), up, yr); }; - const Vec3 originStanding = pivot + turn(tiltOrigin - pivot); - const Basis b{turn(tiltBasis.x), turn(tiltBasis.y), turn(tiltBasis.z)}; - const auto &S = hmd.mDeviceToAbsoluteTracking, &R = hmdRaw.mDeviceToAbsoluteTracking; - auto toRaw = [&](Vec3 v) { return Rotate(R, RotateInverse(S, v)); }; // directions: raw <- standing - const Vec3 originRaw = Position(R) + toRaw(originStanding - eye); - double q[4]; - BasisQuat({toRaw(b.x), toRaw(b.y), toRaw(b.z)}, q); - char msg[200]; - std::snprintf(msg, sizeof msg, "posq %.5f %.5f %.5f %.6f %.6f %.6f %.6f", originRaw.x, originRaw.y, - originRaw.z, q[0], q[1], q[2], q[3]); - SendTo(out, "ft_pointer", msg); - } else if (active && anchored && hmd.bPoseIsValid) { - const bool dragging = leftHeld || tnow < dropHoldUntil; - if (!dragging) tiltYaw = tiltPitch = 0; // drop finished: back to plain pointing - const Vec3 dir = Direction(yaw, pitch); - // Nearest visible overlay along a ray (frozen while dragging). - struct Hit { - double along = 1e9; - std::string key; - bool scene = false; - Vec3 point, normal; - }; - auto nearest = [&](Vec3 from, Vec3 d) { - Hit h; - for (const auto &[key, handle] : handles) { - if (!visible[key]) continue; - if (sceneGraph[key]) { - // Plane test: overlay origin and its +Z normal, within sceneRadius of the origin. - vr::ETrackingUniverseOrigin uo; - vr::HmdMatrix34_t t{}; - if (overlay->GetOverlayTransformAbsolute(handle, &uo, &t) != vr::VROverlayError_None) continue; - const Vec3 center = Position(t), normal{t.m[0][2], t.m[1][2], t.m[2][2]}; - const double denom = Dot(d, normal); - if (std::fabs(denom) < 1e-4) continue; - const double along = Dot(center - from, normal) / denom; - const Vec3 at = from + d * along; - if (along > 0.05 && along < h.along && std::sqrt(Dot(at - center, at - center)) <= cfg.sceneRadius) - h.along = along, h.key = key, h.scene = true, h.point = at, h.normal = normal; - continue; - } - vr::VROverlayIntersectionParams_t params{}; - params.vSource = {float(from.x), float(from.y), float(from.z)}; - params.vDirection = {float(d.x), float(d.y), float(d.z)}; - params.eOrigin = vr::TrackingUniverseStanding; - vr::VROverlayIntersectionResults_t r{}; - if (overlay->ComputeOverlayIntersection(handle, ¶ms, &r) && r.fDistance > 0.05f && - r.fDistance < h.along) { - h.along = r.fDistance, h.key = key, h.scene = false; - h.point = {r.vPoint.v[0], r.vPoint.v[1], r.vPoint.v[2]}; - h.normal = {r.vNormal.v[0], r.vNormal.v[1], r.vNormal.v[2]}; - } - } - return h; - }; - Hit first; - if (!dragging) first = nearest(anchor, dir); - double best = first.along; - std::string bestKey = first.key; - bool bestScene = first.scene; - Vec3 bestPoint = first.point, bestNormal = first.normal; - bool onEdge = false; - if (!dragging && best < 1e8 && !bestScene) { - edgeKey = bestKey, edgePoint = bestPoint, edgeNormal = Normalize(bestNormal), edgeLast = bestPoint; - } else if (!dragging && best >= 1e8 && !edgeKey.empty() && visible[edgeKey]) { - // Just off a panel: stay on its plane (see "Panel edges" at the top). - const double denom = Dot(dir, edgeNormal); - if (std::fabs(denom) > 1e-4) { - const double along = Dot(edgePoint - anchor, edgeNormal) / denom; - const Vec3 at = anchor + dir * along; - if (along > 0.05 && std::sqrt(Dot(at - edgeLast, at - edgeLast)) <= cfg.edgeReach) { - best = along; - bestKey = edgeKey; - onEdge = true; - } - } - } - // Held on one of ft-screens' panels that stays where it is: onto whichever of them - // the ray meets (see the top). - if (leftHeld && !pressKey.empty()) { - vr::ETrackingUniverseOrigin uo; - vr::HmdMatrix34_t now{}; - auto it = handles.find(pressKey); - bool still = it != handles.end() && - overlay->GetOverlayTransformAbsolute(it->second, &uo, &now) == vr::VROverlayError_None; - for (int i = 0; still && i < 3; ++i) - for (int j = 0; j < 4; ++j) - if (std::fabs(now.m[i][j] - pressPose.m[i][j]) > 0.001f) still = false; - if (still) { - Hit h; - for (const auto &[key, handle] : handles) { - if (!visible[key] || !FramePanel(key)) continue; - vr::VROverlayIntersectionParams_t params{}; - params.vSource = {float(anchor.x), float(anchor.y), float(anchor.z)}; - params.vDirection = {float(dir.x), float(dir.y), float(dir.z)}; - params.eOrigin = vr::TrackingUniverseStanding; - vr::VROverlayIntersectionResults_t r{}; - if (overlay->ComputeOverlayIntersection(handle, ¶ms, &r) && r.fDistance > 0.05f && - r.fDistance < h.along) - h.along = r.fDistance, h.key = key; - } - if (h.along < 1e8) dragDistance = h.along, lastHit = h.key; - } else { - pressKey.clear(); // carried: the lock holds for the rest of this press - } - } - // While dragging: keep the press-time distance and show the non-interactive marker. - // On a scene-graph plane or a panel's edge: the laser-catching dot goes 5 cm behind it. - double distance = dragging ? dragDistance : (best < 1e8 ? best : cfg.freeDistance); - Vec3 point = anchor + dir * distance; - bool occluded = false; - if (!dragging) { - // The cursor lands on what you see under it: the ray above starts at the anchor, - // not the eye, so after leaning it can pick a panel that something nearer - // covers from where you are now (panels close together in view, at different - // depths). Anything in front of the point on the eye's line of sight wins. - const double toPoint = std::sqrt(Dot(point - eye, point - eye)); - const Hit front = nearest(eye, Normalize(point - eye)); - if (front.along < toPoint - 0.02) { - occluded = true; - bestKey = front.key, bestScene = front.scene; - point = front.point; - if (!front.scene) edgeKey = front.key, edgePoint = front.point, edgeNormal = Normalize(front.normal), - edgeLast = front.point; - distance = std::sqrt(Dot(point - anchor, point - anchor)); - best = distance; - onEdge = false; - } - lastDistance = distance, lastHit = bestKey; - } - const bool onScene = !dragging && ((bestScene && best < 1e8) || onEdge); - const bool onPanel = dragging || (best < 1e8 && !onScene); - const Vec3 sight = Normalize(point - eye); - if (!dragging) { - // SteamVR Settings (see the top): the dashboard's main panel is hidden and its - // scene-graph panel shows the page. - onVrSettings = false; - const auto mainIt = visible.find("valve.steam.gamepadui.main"); - if (overlay->IsDashboardVisible() && !(mainIt != visible.end() && mainIt->second)) { - for (const auto &[key, handle] : handles) { - if (!visible[key] || key.rfind("valve.steam.gamepadui.frame.menu.", 0) != 0) continue; - vr::ETrackingUniverseOrigin uo; - vr::HmdMatrix34_t t{}; - if (overlay->GetOverlayTransformAbsolute(handle, &uo, &t) == vr::VROverlayError_None && - OnSettingsPage(t, eye, sight)) - onVrSettings = true; - } - } - } - if (onVrSettings != catcherHidesHit) { - overlay->SetOverlayFlag(cursor, vr::VROverlayFlags_HideLaserIntersection, onVrSettings); - catcherHidesHit = onVrSettings; - } - - if (onVrSettings) { - // The dot close in front of the page, which is nearer than any guess of ours; - // the laser-catching dot far behind everything, invisible, so it never covers - // the page, with SteamVR's hit dot hidden on it. - const Vec3 near = eye + sight * SETTINGS_DOT, far = eye + sight * SETTINGS_CATCHER; - double alpha = 1; - if (gazeOn && !cfg.gazeDotAlways) { - auto secs = [&](Clock::time_point t) { return std::chrono::duration(tnow - t).count(); }; - alpha = std::clamp(1 - std::min(secs(lastMove) - cfg.gazeShow, secs(lastHeld)) / 0.25, 0.0, 1.0); - } - if (tnow < calPanelUntil) alpha = 0; - overlay->SetOverlayAlpha(marker, float(alpha)); - overlay->SetOverlayWidthInMeters(marker, float(2 * SETTINGS_DOT * std::tan(cfg.cursorDeg * M_PI / 360))); - auto mm = Billboard(near, eye); - overlay->SetOverlayTransformAbsolute(marker, vr::TrackingUniverseStanding, &mm); - overlay->SetOverlayAlpha(cursor, 0); - overlay->SetOverlayWidthInMeters(cursor, float(2 * SETTINGS_CATCHER * std::tan(cfg.cursorDeg * M_PI / 360))); - auto mc = Billboard(far, eye); - overlay->SetOverlayTransformAbsolute(cursor, vr::TrackingUniverseStanding, &mc); - overlay->ShowOverlay(marker); - overlay->ShowOverlay(cursor); - } else { - // On a panel: the non-interactive marker, pulled 5 mm toward the eye so it - // draws on top. In free space: the interactive dot the laser lands on. - const vr::VROverlayHandle_t show = onPanel ? marker : cursor, hide = onPanel ? cursor : marker; - const Vec3 at = onPanel ? point + Normalize(eye - point) * 0.005 : onScene ? point + dir * 0.05 : point; - const double dist = std::sqrt(Dot(at - eye, at - eye)); - // Gaze mode: a pulse for each click, and with POINTER_GAZE_DOT=moving, shown only - // while something moves it or a press holds it (see the top); transparent - // otherwise, the laser still lands on it. - double scale = 1, alpha = 1; - if (gazeOn) { - auto secs = [&](Clock::time_point t) { return std::chrono::duration(tnow - t).count(); }; - alpha = cfg.gazeDotAlways ? 1.0 : std::clamp(1 - std::min(secs(lastMove) - cfg.gazeShow, secs(lastHeld)) / 0.25, 0.0, 1.0); - const double pulse = secs(pulseAt); - if (pulse < 0.6) { - scale = 1 + 1.5 * std::max(0.0, 1 - pulse / 0.3); - alpha = std::max(alpha, std::clamp((0.6 - pulse) / 0.3, 0.0, 1.0)); - } - } - if (tnow < calPanelUntil) alpha = 0; // the calibration panel is up (see the top) - overlay->SetOverlayAlpha(show, float(alpha)); - overlay->SetOverlayWidthInMeters(show, float(2 * dist * std::tan(scale * cfg.cursorDeg * M_PI / 360))); - auto m = Billboard(at, eye); - overlay->SetOverlayTransformAbsolute(show, vr::TrackingUniverseStanding, &m); - overlay->ShowOverlay(show); - overlay->HideOverlay(hide); - } - - // Controller ray: from the eye, aimed at the cursor point, converted from the - // standing universe to raw tracking space via the HMD's pose in both. - const Vec3 aimStanding = Normalize(point - eye); - const auto &S = hmd.mDeviceToAbsoluteTracking, &R = hmdRaw.mDeviceToAbsoluteTracking; - const Vec3 aim = Rotate(R, RotateInverse(S, aimStanding)); // raw <- head <- standing - // Origin partway along the line of sight to the cursor (smaller hit dot). - const double toPoint = std::sqrt(Dot(point - eye, point - eye)); - double originDist = std::max(0.0, std::min(toPoint * cfg.originFraction, toPoint - cfg.originMargin)); - if (onVrSettings) originDist = std::min(originDist, SETTINGS_ORIGIN); // SteamVR finds the page - const Vec3 originStanding = eye + Normalize(point - eye) * originDist; - const Vec3 eyeRaw = Position(R) + Rotate(R, RotateInverse(S, originStanding - eye)); - lastPoint = point, lastOrigin = originStanding, lastAim = aimStanding; // tilt starts from here - havePoint = true; - if (debug && tnow - lastDebug > std::chrono::milliseconds(500)) { - lastDebug = tnow; - if (systemPointer == vr::k_ulOverlayHandleInvalid) overlay->FindOverlay("system.pointer", &systemPointer); - std::printf("dbg %s hit=%s dist=%.2f eye->point=%.2f origin=%.2f vrsettings=%d yaw=%.1f pitch=%.1f gaze=%s steamvr_dot=%d primary=%u\n", - dragging ? "DRAG" : occluded ? "INFRONT" : onEdge ? "EDGE" : onScene ? "SCENE" : (best < 1e8 ? "PANEL" : "FREE"), lastHit.empty() ? "-" : lastHit.c_str(), - distance, toPoint, originDist, onVrSettings, yaw, pitch, - !gazeOn ? "off" : tnow - gz.at > std::chrono::milliseconds(150) ? "stale" : gazeOwns ? "owns" : "mouse", - systemPointer != vr::k_ulOverlayHandleInvalid && overlay->IsOverlayVisible(systemPointer), - overlay->GetPrimaryDashboardDevice()); - std::fflush(stdout); - } - const double ayaw = std::atan2(-aim.x, -aim.z) * 180 / M_PI; - const double apitch = std::asin(std::clamp(aim.y, -1.0, 1.0)) * 180 / M_PI; - if (dragging && (tiltYaw != 0 || tiltPitch != 0)) { - // Keep this drag's tilt applied, about the current cursor point. - const double yr = tiltYaw * M_PI / 180, pr = tiltPitch * M_PI / 180; - const Basis base = AimBasis(aimStanding); - const Vec3 up{0, 1, 0}, side = base.x; - auto turn = [&](Vec3 v) { return RotateAbout(RotateAbout(v, side, pr), up, yr); }; - const Vec3 o = point + turn(originStanding - point); - const Basis b{turn(base.x), turn(base.y), turn(base.z)}; - auto toRaw = [&](Vec3 v) { return Rotate(R, RotateInverse(S, v)); }; - const Vec3 oRaw = Position(R) + toRaw(o - eye); - double q[4]; - BasisQuat({toRaw(b.x), toRaw(b.y), toRaw(b.z)}, q); - char msg[200]; - std::snprintf(msg, sizeof msg, "posq %.5f %.5f %.5f %.6f %.6f %.6f %.6f", oRaw.x, oRaw.y, oRaw.z, q[0], - q[1], q[2], q[3]); - SendTo(out, "ft_pointer", msg); - } else { - char msg[160]; - std::snprintf(msg, sizeof msg, "pose %.5f %.5f %.5f %.4f %.4f", eyeRaw.x, eyeRaw.y, eyeRaw.z, ayaw, apitch); - SendTo(out, "ft_pointer", msg); - } - } - - // A held-back press (see the top): held still long enough, it's a real press (a drag); - // released, it's a click where the pointer is now (this frame's pose has gone out). - if (!active) aimHeld = aimRight = clickPress = aimHand = confirmLesson = false; // released meanwhile: nothing to click - if (aimHeld && !aimHand && nudgeMoved < 0.2 && tnow - aimSince >= std::chrono::duration(cfg.gazeHold)) { - aimHeld = false; - gazeBack = true; - pressRight = aimRight; // the right button's: a real right press (see the top) - aimRight = false; - PressLeft(); - } - // A keyboard click held still for POINTER_GAZE_HOLD: a real press, then the head drags - // (from where it is now). - if (aimHeld && aimHand && hold.promote && !hold.engaged && - tnow - aimSince >= std::chrono::duration(cfg.gazeHold)) { - aimHeld = aimHand = false; - hold.pressed = hold.grip = true; - if (hmd.bPoseIsValid) { - const Vec3 f = Normalize({-hm[0][2], -hm[1][2], -hm[2][2]}); - hold.refYaw = std::atan2(-f.x, -f.z) * 180 / M_PI; - hold.refPitch = std::asin(std::clamp(f.y, -1.0, 1.0)) * 180 / M_PI; - } - gazeBack = gazeOn; - pressRight = hold.right; - PressLeft(); - if (debug) std::printf("hold key %s: pressed (held still)\n", hold.right ? "right" : "left"); - if (debug) std::fflush(stdout); - } - if (clickPress) { - clickPress = false; - PressLeft(); - clickRelease = true; - clickReleaseAt = tnow + std::chrono::milliseconds(40); - } else if (clickRelease && tnow >= clickReleaseAt) { - clickRelease = false; - ReleaseLeft(); - } - - controllerButtons.Poll( - [&](const char *button, bool down) { - SendTo(out, "frametop_relay", std::string("vrbtn ") + button + (down ? " 1" : " 0")); - }, - inGame); - - vr::VREvent_t ev; - while (sys->PollNextEvent(&ev, sizeof ev)) { - if (ev.eventType == vr::VREvent_Quit) { - sys->AcknowledgeQuit_Exiting(); - - overlays.Stop(); - vr::VR_Shutdown(); - return 0; - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(8)); +// The headset off: SteamVR drops the HMD's activity to idle as soon as it comes off. +// An awake pointer (a connected controller, SteamVR's laser mode forced on) kept the +// displays from sleeping, so it's released at once, and the mouse can't wake it until +// the headset is back on (then the first mouse input does). +void Pointer::HeadsetOff() { + const auto level = sys->GetTrackedDeviceActivityLevel(vr::k_unTrackedDeviceIndex_Hmd); + headsetOff = level == vr::k_EDeviceActivityLevel_Idle || level == vr::k_EDeviceActivityLevel_Standby || + level == vr::k_EDeviceActivityLevel_Idle_Timeout; + if (headsetOff && active) { + active = false; + claimPending = claimHeld = false; + overlay->HideOverlay(cursor); + overlay->HideOverlay(marker); + SendTo(out, "ft_pointer", "btn a 0"); + SendTo(out, "ft_pointer", "hide"); + std::printf("headset off: pointer released\n"); + std::fflush(stdout); } } +// Outside games, gaze mode keeps the pointer (see the top); the relay needs to know. +void Pointer::GazeAwake() { + const auto t = Clock::now(); + if (t - inGameAt > std::chrono::milliseconds(500)) { + inGameAt = t; + inGame = vr::VRApplications()->GetCurrentSceneProcessId() != 0; + } + // Hand gestures in the last two minutes keep it the same way. + const bool handsRecent = cfg.handsOn && handUsed != Clock::time_point{} && t - handUsed < std::chrono::minutes(2); + const bool awake = (gazeOn || handsRecent) && !inGame && !headsetOff; + if (awake != gazeAwake || t - gazeAwakeAt > std::chrono::seconds(5)) { + if (awake != gazeAwake) + std::printf("%s keeps the pointer: %s\n", gazeOn ? "gaze" : "hand use", + awake ? "yes" : "no (off, in a game, or headset off)"); + if (awake != gazeAwake) std::fflush(stdout); + gazeAwake = awake; + gazeAwakeAt = t; + SendTo(out, "frametop_relay", awake ? "gazeawake 1" : "gazeawake 0"); + } +} + +// Commands from the relay. +void Pointer::RelayCommands() { + char buf[256]; + ssize_t n; + sockaddr_un from{}; + socklen_t fromLen = sizeof from; + while ((n = recvfrom(in, buf, sizeof buf - 1, 0, reinterpret_cast(&from), &fromLen)) > 0) { + buf[n] = 0; + // Reply to the sender (the layout tool binds an abstract address to get answers). + const sockaddr_un sender = from; + const socklen_t senderLen = fromLen; + fromLen = sizeof from; + Command(buf, sender, senderLen); + } +} + +// One command from the relay (or another sender: reply answers it). +void Pointer::Command(const char *buf, const sockaddr_un &sender, socklen_t senderLen) { + auto reply = [&](const std::string &msg) { + if (senderLen > offsetof(sockaddr_un, sun_path)) + sendto(out, msg.data(), msg.size(), 0, reinterpret_cast(&sender), senderLen); + }; + // The gaze, from ft-gazed: not mouse input, it never wakes the pointer. + double g[4]; + if (std::sscanf(buf, "gz %lf %lf %lf %lf", &g[0], &g[1], &g[2], &g[3]) == 4) { + gz = {g[0], g[1], g[2], g[3], Clock::now()}; + return; + } + // The gaze calibration panel (see the top): presses answer it instead of clicking. + { + int on; + if (std::sscanf(buf, "calpanel %d", &on) == 1) { + const bool was = Clock::now() < calPanelUntil; + calPanelUntil = on ? Clock::now() + std::chrono::seconds(3) : Clock::time_point{}; + if (on && !was) calOpened = true; + return; + } + } + if (Clock::now() < calPanelUntil) { + const bool accept = !std::strncmp(buf, "btn trigger 1", 13) || !std::strncmp(buf, "gazekey left 1", 14); + const bool quit = !std::strncmp(buf, "btn b 1", 7) || !std::strncmp(buf, "gazekey right 1", 15); + if (accept || quit) { + SendTo(out, "ft_gazed", accept ? "calaccept" : "calquit"); + return; + } + if (!std::strncmp(buf, "btn ", 4) || !std::strncmp(buf, "gazekey ", 8) || + !std::strncmp(buf, "precision ", 10) || !std::strncmp(buf, "gazedrag ", 9)) + return; // their releases, and the other buttons: nothing to click now + } + { + char kind[16], source[16]; + int v; + if (std::sscanf(buf, "%15s %15s %d", kind, source, &v) == 3 && + (!std::strcmp(kind, "precision") || !std::strcmp(kind, "gazedrag"))) { + lastMouse = Clock::now(); + if (!active) Wake(Clock::now()); + devicePresses.push_back({source, !std::strcmp(kind, "gazedrag"), v != 0}); + return; + } + if (std::sscanf(buf, "gazekey %15s %d", source, &v) == 2 && + (!std::strcmp(source, "left") || !std::strcmp(source, "right"))) { + lastMouse = Clock::now(); + if (!active) Wake(Clock::now()); + keyPresses.push_back({!std::strcmp(source, "right"), v != 0}); + return; + } + } + if (std::strcmp(buf, "typing") == 0) { // not mouse input: it never wakes the pointer + lastTyping = Clock::now(); + return; + } + if (std::strncmp(buf, "vrbind", 6) == 0) { + std::printf("controller buttons: %s\n", controllerButtons.Bind(buf + 6).c_str()); + std::fflush(stdout); + return; + } + if (std::strncmp(buf, "vrglobal", 8) == 0) { + const char *arg = buf + 8; + while (*arg == ' ') ++arg; + ControllerButtons::SetGlobal(std::strncmp(arg, "off", 3) != 0); + reply(controllerButtons.Status()); + return; + } + if (std::strncmp(buf, "vrstatus", 8) == 0) { + reply(controllerButtons.Status()); + return; + } + if (std::strncmp(buf, "overlays", 8) == 0) { + overlays.Request(sender, senderLen); // answered from the list's thread + return; + } + if (std::strncmp(buf, "gaze", 4) == 0) { + const char *arg = buf + 4; + while (*arg == ' ') ++arg; + if (*arg != '?') { // "gaze ?" only asks + gazeOn = std::strncmp(arg, "on", 2) == 0 ? true + : std::strncmp(arg, "off", 3) == 0 ? false + : !gazeOn; + gazeOwns = true, nudging = false; + std::printf("gaze mode %s\n", gazeOn ? "on" : "off"); + std::fflush(stdout); + } + reply(gazeOn ? "ok on" : "ok off"); + return; + } + const bool mouseInput = std::strncmp(buf, "move", 4) == 0 || std::strncmp(buf, "btn", 3) == 0 || + std::strncmp(buf, "scroll", 6) == 0; + // A move held back (POINTER_GAZE_MOUSE_MOVE=held) only wakes the pointer: it isn't using + // the mouse, so a drifting mouse doesn't keep the gaze from taking the pointer back. + const bool moveHeldBack = std::strncmp(buf, "move", 4) == 0 && MouseMoveHeld(); + if (mouseInput && !moveHeldBack) lastMouse = Clock::now(); + // Any mouse input wakes the pointer (after a controller took over, or a helper restart). + if (!active && mouseInput) Wake(Clock::now()); + if (moveHeldBack) return; + double a, b; + char key[128]; + double px, py, pz, pyaw, ppitch, proll = 0, pgrab = -1; + if (std::sscanf(buf, "grabprobe %127s", key) == 1) { + GrabProbe(key); + return; + } + if (std::sscanf(buf, "place %127s %lf %lf %lf %lf %lf %lf %lf", key, &px, &py, &pz, &pyaw, &ppitch, &proll, + &pgrab) >= 6) { + reply(Place(key, {px, py, pz}, PanelBasis(pyaw, ppitch, proll), pgrab >= 0 ? pgrab : cfg.grabOffset)); + return; + } + if (std::sscanf(buf, "measure %127s", key) == 1) { + Panel p; + Vec3 eye; + const std::string err = FindPanel(key, p, eye); + char msg[400] = ""; + if (err.empty()) + std::snprintf(msg, sizeof msg, "ok %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f %.4f", + p.center.x, p.center.y, p.center.z, p.width, p.height, p.basis.x.x, p.basis.x.y, + p.basis.x.z, p.basis.y.x, p.basis.y.y, p.basis.y.z, p.basis.z.x, p.basis.z.y, + p.basis.z.z); + reply(err.empty() ? msg : "error " + err); + return; + } + if (std::strncmp(buf, "head", 4) == 0) { + vr::TrackedDevicePose_t h; + sys->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding, 0, &h, 1); + const Vec3 e = Position(h.mDeviceToAbsoluteTracking); + const Vec3 f = Rotate(h.mDeviceToAbsoluteTracking, {0, 0, -1}); + char msg[200]; + std::snprintf(msg, sizeof msg, "ok %.4f %.4f %.4f %.2f %.2f", e.x, e.y, e.z, + std::atan2(-f.x, -f.z) * 180 / M_PI, std::asin(std::clamp(f.y, -1.0, 1.0)) * 180 / M_PI); + reply(h.bPoseIsValid ? msg : "error no head pose (headset off?)"); + return; + } + if (std::strncmp(buf, "debug", 5) == 0) { + debug = !debug; + std::printf("debug %s\n", debug ? "on" : "off"); + std::fflush(stdout); + return; + } + if (std::strncmp(buf, "btn trigger 1", 13) == 0) { + LeftButton(true); + return; + } else if (std::strncmp(buf, "btn trigger 0", 13) == 0) { + LeftButton(false); + return; + } else if (std::strncmp(buf, "btn b 1", 7) == 0 && RightButton(true)) { + return; + } else if (std::strncmp(buf, "btn b 0", 7) == 0 && RightButton(false)) { + return; + } else if (std::strncmp(buf, "btn b 1", 7) == 0 && leftHeld) { + tilting = tiltStart = swallowedRight = true; // right press while dragging: tilt, no right-click + return; + } else if (std::strncmp(buf, "btn b 0", 7) == 0 && swallowedRight) { + tilting = swallowedRight = false; + return; + } + if (tilting && std::sscanf(buf, "move %lf %lf", &a, &b) == 2) { + tiltYaw += a; + tiltPitch = std::clamp(tiltPitch + b, -80.0, 80.0); + return; + } + if (std::sscanf(buf, "move %lf %lf", &a, &b) == 2) { + lastMove = Clock::now(); // the dot shows while the mouse moves it (gaze mode) + if (gazeOn && gazeOwns) { + // The mouse takes the pointer from the gaze, from where the gaze left it. + gazeOwns = false; + nudging = haveHead && Clock::now() - gz.at < std::chrono::milliseconds(200); + nudgeRawHy = gz.rhy, nudgeRawHp = gz.rhp, nudgeHead = lastHead; + nudgeAt = Clock::now(), nudgeMoved = 0; + } + if (nudging || aimHeld) nudgeMoved += std::hypot(a, b); + if (!anchored) recenter = true; + yaw += a; + while (yaw > 180) yaw -= 360; + while (yaw < -180) yaw += 360; + pitch = std::clamp(pitch + b, -85.0, 85.0); + } else if (std::strncmp(buf, "recenter", 8) == 0) { + recenter = true; + } else if (std::strncmp(buf, "reload", 6) == 0) { + cfg.Load(ReadConfig()); + ApplyConfig(); + lastSlow = Clock::now() - std::chrono::seconds(10); // apply POINTER_IGNORE now + std::printf("reloaded: free distance %.2f m, dot %.2f deg, origin %.2f, head follow %s, leash %.0f deg, " + "controller pickup %.1fx, %zu ignored\n", + cfg.freeDistance, cfg.cursorDeg, cfg.originFraction, follow ? "on" : "off", cfg.leashDeg, cfg.pickupScale, + cfg.ignore.size()); + std::fflush(stdout); + } else if (std::strncmp(buf, "follow", 6) == 0) { + const char *arg = buf + 6; + while (*arg == ' ') ++arg; + const bool was = follow; + follow = std::strncmp(arg, "on", 2) == 0 ? true : std::strncmp(arg, "off", 3) == 0 ? false : !follow; + if (follow && !was) followReset = true; + std::printf("head follow %s (leash %.0f deg)\n", follow ? "on" : "off", cfg.leashDeg); + std::fflush(stdout); + } else if (std::strncmp(buf, "show", 4) == 0) { + if (!active) Wake(Clock::now()); + } else if (std::strncmp(buf, "hide", 4) == 0) { + active = false; + overlay->HideOverlay(cursor); + overlay->HideOverlay(marker); + SendTo(out, "ft_pointer", "hide"); + } else { + SendTo(out, "ft_pointer", buf); // btn, scroll + } +} + +// This frame's poses and time (see Frame). +void Pointer::ReadFrame(Frame &frame) { + sys->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding, 0.011f, frame.all, vr::k_unMaxTrackedDeviceCount); + sys->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0.011f, &frame.hmdRaw, 1); + frame.tnow = Clock::now(); + const auto &hm = frame.Hmd().mDeviceToAbsoluteTracking.m; + frame.eye = {hm[0][3], hm[1][3], hm[2][3]}; +} + +// Claim pulse (switchlaserhand on the driver's "a" button, no click). +void Pointer::ClaimPulse(const Frame &frame) { + const auto tnow = frame.tnow; + if (claimPending && tnow >= claimAt) { + SendTo(out, "ft_pointer", "btn a 1"); + claimPending = false; + claimHeld = true; + claimRelease = tnow + std::chrono::milliseconds(60); + } else if (claimHeld && tnow >= claimRelease) { + SendTo(out, "ft_pointer", "btn a 0"); + claimHeld = false; + } +} + +// The overlay list is only needed while the pointer is awake (see OverlayList). +void Pointer::PauseOverlayList() { + overlays.SetPaused(!active || headsetOff); +} + +// Laser mode on while the pointer is awake. +void Pointer::LaserMode() { + if (active != laserModeShown) { + laserModeShown = active; + if (active) overlay->ShowOverlay(laserMode); + else overlay->HideOverlay(laserMode); + if (debug) std::printf("laser mode %s\n", active ? "forced on" : "released"); + if (debug) std::fflush(stdout); + } +} + +// Didn't get the hand role (a held controller keeps it): release, back off. +void Pointer::HandRole(const Frame &frame) { + const auto tnow = frame.tnow; + if (active && tnow - wokeAt > std::chrono::seconds(1) && ours != vr::k_unTrackedDeviceIndexInvalid && + sys->GetControllerRoleForTrackedDeviceIndex(ours) == vr::TrackedControllerRole_Invalid) { + active = false; + claimPending = claimHeld = false; + overlay->HideOverlay(cursor); + overlay->HideOverlay(marker); + SendTo(out, "ft_pointer", "btn a 0"); + SendTo(out, "ft_pointer", "hide"); + noWakeUntil = tnow + std::chrono::seconds(2); + std::printf("no hand role (a controller is in use): pointer released\n"); + std::fflush(stdout); + } +} + +// Last used wins: a real controller being moved releases the pointer (see the top), +// in gaze mode too. +void Pointer::LastUsedWins(const Frame &frame) { + const auto &all = frame.all; + const auto tnow = frame.tnow; + if (active && hold.src == Src::None && tnow - lastMouse > std::chrono::milliseconds(500)) { + for (vr::TrackedDeviceIndex_t i = 1; i < vr::k_unMaxTrackedDeviceCount; ++i) { + if (i == ours || !all[i].bPoseIsValid || all[i].eTrackingResult != vr::TrackingResult_Running_OK || + sys->GetTrackedDeviceClass(i) != vr::TrackedDeviceClass_Controller) { + movingSince[i] = {}; + continue; + } + const auto &v = all[i].vVelocity.v, &w = all[i].vAngularVelocity.v; + const double speed = std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); + const double spin = std::sqrt(w[0] * w[0] + w[1] * w[1] + w[2] * w[2]); + if (speed <= 0.35 * cfg.pickupScale && spin <= 2.0 * cfg.pickupScale) { + movingSince[i] = {}; + continue; + } + if (movingSince[i] == Clock::time_point{}) movingSince[i] = tnow; + if (tnow - movingSince[i] >= std::chrono::milliseconds(100)) { + active = false; + claimPending = claimHeld = false; + overlay->HideOverlay(cursor); + overlay->HideOverlay(marker); + SendTo(out, "ft_pointer", "btn a 0"); + SendTo(out, "ft_pointer", "hide"); + std::printf("controller %u moved (%.2f m/s, %.1f rad/s): pointer released\n", i, speed, spin); + std::fflush(stdout); + break; + } + } + } else { + std::fill(std::begin(movingSince), std::end(movingSince), Clock::time_point{}); + } +} + +// A pending recenter (the command, a wake, a first move): the anchor goes to the eye, aimed where the +// head faces. +void Pointer::Recenter(const Frame &frame) { + const vr::TrackedDevicePose_t &hmd = frame.Hmd(); + const auto &hm = hmd.mDeviceToAbsoluteTracking.m; + const Vec3 &eye = frame.eye; + if (recenter && hmd.bPoseIsValid) { + anchor = eye; + const Vec3 f{-hm[0][2], -hm[1][2], -hm[2][2]}; + yaw = std::atan2(-f.x, -f.z) * 180 / M_PI; + pitch = std::asin(std::clamp(f.y, -1.0, 1.0)) * 180 / M_PI; + anchored = true; + recenter = false; + followReset = true; + } +} + +// Gaze mode (see the top): the gaze has the pointer, or takes it back when you look +// well away from it. Not while a press holds the pointer, and only on fresh gaze. +void Pointer::GazeMode(const Frame &frame) { + const vr::TrackedDevicePose_t &hmd = frame.Hmd(); + const auto tnow = frame.tnow; + const Vec3 &eye = frame.eye; + if (hmd.bPoseIsValid) lastHead = hmd.mDeviceToAbsoluteTracking, haveHead = true; + if (gazeOn && active && hmd.bPoseIsValid && !tilting && !leftHeld && !aimHeld && !clickPress && !clickRelease && + tnow >= dropHoldUntil && + tnow - gz.at < std::chrono::milliseconds(150)) { + const Vec3 g = Rotate(hmd.mDeviceToAbsoluteTracking, Direction(gz.hy, gz.hp)); + if (!gazeOwns && havePoint) { + const double off = std::acos(std::clamp(Dot(g, Normalize(lastPoint - eye)), -1.0, 1.0)) * 180 / M_PI; + if (off > cfg.gazeRetake && tnow - lastMouse > std::chrono::milliseconds(300)) { + if (retakeSince == Clock::time_point{}) retakeSince = tnow; + if (tnow - retakeSince >= std::chrono::milliseconds(120)) gazeOwns = true, nudging = false; + } else { + retakeSince = {}; + } + } + if (gazeOwns) { + retakeSince = {}; + anchor = eye; + anchored = true; + yaw = std::atan2(-g.x, -g.z) * 180 / M_PI; + pitch = std::clamp(std::asin(std::clamp(g.y, -1.0, 1.0)) * 180 / M_PI, -85.0, 85.0); + } + } +} + +// Hands (see the top): pinches and grips from ft-hands. +void Pointer::Hands(const Frame &frame) { + const auto tnow = frame.tnow; + vr::TrackedDevicePose_t h0; + sys->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding, 0, &h0, 1); + if (h0.bPoseIsValid) poses.Add(tnow, h0.mDeviceToAbsoluteTracking); +} + +// Where a gesture's point is, seen from the hold's origin: yaw and pitch, degrees. +bool Pointer::HandAngles(const float p[3], uint64_t t_ns, const Vec3 &from, double &hy, double &hp) { + vr::HmdMatrix34_t head; + if (!poses.At(t_ns, head)) return false; + const Vec3 d = Normalize(Position(head) + Rotate(head, Vec3{p[0], p[1], p[2]}) - from); + hy = std::atan2(-d.x, -d.z) * 180 / M_PI; + hp = std::asin(std::clamp(d.y, -1.0, 1.0)) * 180 / M_PI; + return true; +} + +void Pointer::EndHold(bool lost) { + if (hold.pressed) { + if (leftHeld) ReleaseLeft(); + } else if (aimHeld) { + aimHeld = aimHand = false; + if (lost) { + if (gazeOn) gazeOwns = true, nudging = false; // no click: the pointer goes back to the gaze + } else { + clickPress = true; // the click is on the release, where the pointer is now + pressRight = hold.right; + gazeBack = nudgeMoved < 0.2; + } + } + if (debug) + std::printf("hold %s %s%s\n", hold.src == Src::Hand ? (hold.grip ? "grip" : "pinch") + : hold.src == Src::Head ? (hold.right ? "key right" : "key left") + : hold.grip ? "gaze drag" : "gaze precision", + lost ? "lost" : "released", hold.pressed ? "" : lost ? " (no click)" : " (click)"); + if (debug) std::fflush(stdout); + hold = {}; +} + +// The press, once a hold's source is set: a real press (dragging until the release), or +// held back like gaze mode's mouse press (see the top): the click comes on the release. +void Pointer::StartHold(bool press, Clock::time_point tnow) { + hold.startYaw = hold.lastYaw = yaw, hold.startPitch = hold.lastPitch = pitch; + hold.pressed = press; + if (press) { + gazeBack = gazeOn && gazeOwns; + PressLeft(); + } else { + gazeOwns = false; + nudging = gazeOn && haveHead && tnow - gz.at < std::chrono::milliseconds(200); + nudgeRawHy = gz.rhy, nudgeRawHp = gz.rhp, nudgeHead = lastHead; + nudgeAt = aimSince = tnow, nudgeMoved = 0; + aimHeld = aimHand = true; + pulseAt = tnow; + } +} + +// The hold's source moved to (hy, hp): past the dead zone, the pointer follows at the gain, +// from where it was when it got past. +void Pointer::Steer(double hy, double hp, double deadzone, double gain, Clock::time_point tnow) { + const double dy = std::remainder(hy - hold.refYaw, 360.0), dp = hp - hold.refPitch; + if (!hold.engaged) { + if (std::hypot(dy, dp) <= deadzone) return; + hold.engaged = true; + hold.refYaw = hy, hold.refPitch = hp; + hold.startYaw = hold.lastYaw = yaw, hold.startPitch = hold.lastPitch = pitch; + return; + } + yaw = std::remainder(hold.startYaw + gain * dy, 360.0); + pitch = std::clamp(hold.startPitch + gain * dp, -85.0, 85.0); + // How far the nudge went: for a keyboard click, from where the dot was at the press + // (a head wobbles on the way); otherwise the path, as with the mouse. + if (!hold.pressed && hold.src == Src::Head) + nudgeMoved = std::hypot(std::remainder(yaw - hold.pressYaw, 360.0), pitch - hold.pressPitch); + else if (!hold.pressed) + nudgeMoved += std::hypot(std::remainder(yaw - hold.lastYaw, 360.0), pitch - hold.lastPitch); + hold.lastYaw = yaw, hold.lastPitch = pitch; + lastMove = tnow; // the dot shows while it moves (gaze mode) +} + +void Pointer::BeginHold(int side, bool grip, const fh_pinch_t &g, Clock::time_point tnow) { + handUsed = tnow; + if (hold.src != Src::None) { + // A grip takes over a pinch on the way to it (closing the hand passes through + // one); otherwise one hold at a time. + if (hold.src != Src::Hand || !grip || hold.grip) return; + aimHeld = aimHand = false; + hold = {}; + } + if (leftHeld || aimHeld || clickPress || clickRelease) return; // the mouse's button is busy + if (!active) { + Wake(tnow); // the first gesture only wakes the pointer, like the first mouse move + return; + } + vr::HmdMatrix34_t head; + if (!poses.At(g.begin_ns, head)) return; + // No pinch just after a key (typing touches thumb to index), and a grip only with + // the hand held up (hands on a desk curl like a loose fist; looking down at them + // puts them straight ahead in the head's frame, where ft-hands can't tell). + const Vec3 at = Position(head) + Rotate(head, Vec3{g.begin_point[0], g.begin_point[1], g.begin_point[2]}); + if (!grip && tnow - lastTyping < std::chrono::duration(cfg.typingHold)) { + if (debug) std::printf("hand pinch ignored: typing\n"); + return; + } + if (grip && Position(head).y - at.y > cfg.handBelow) { + if (debug) std::printf("hand grip ignored: %.2f m below the eyes\n", Position(head).y - at.y); + return; + } + Hold h; + h.src = Src::Hand, h.side = side, h.grip = grip, h.origin = Position(head); + if (!HandAngles(g.begin_point, g.begin_ns, h.origin, h.refYaw, h.refPitch)) return; + hold = h; + // A grip, or a pinch without gaze mode: a real press where the pointer is (where + // you look, in gaze mode), dragging with the hand until it opens. + StartHold(grip || !gazeOn, tnow); + if (debug) std::printf("hand %s %s began\n", side ? "right" : "left", grip ? "grip" : "pinch"); + if (debug) std::fflush(stdout); +} + +// Gaze precision and gaze drag buttons (see the top): the mouse steers. +void Pointer::GazePrecisionButtons(const Frame &frame) { + const auto tnow = frame.tnow; + for (const DevicePress &p : devicePresses) { + if (p.source == "left" || p.source == "right") continue; // no controllers in gaze mode + if (!p.down) { + if (hold.src == Src::Mouse) EndHold(false); + continue; + } + if (hold.src != Src::None || leftHeld || aimHeld || clickPress || clickRelease) continue; + Hold h; + h.src = Src::Mouse, h.grip = p.drag; + hold = h; + StartHold(p.drag, tnow); + if (debug) std::printf("hold gaze %s began (%s)\n", p.drag ? "drag" : "precision", p.source.c_str()); + if (debug) std::fflush(stdout); + } + devicePresses.clear(); +} + +// Where the head faces: yaw and pitch, degrees. +bool Pointer::HeadAngles(const vr::TrackedDevicePose_t &hmd, double &hy, double &hp) { + const auto &hm = hmd.mDeviceToAbsoluteTracking.m; + if (!hmd.bPoseIsValid) return false; + const Vec3 f = Normalize({-hm[0][2], -hm[1][2], -hm[2][2]}); + hy = std::atan2(-f.x, -f.z) * 180 / M_PI; + hp = std::asin(std::clamp(f.y, -1.0, 1.0)) * 180 / M_PI; + return true; +} + +// Keyboard clicks (see the top): held back at the gaze, steered by the head. +void Pointer::KeyboardClicks(const Frame &frame) { + const vr::TrackedDevicePose_t &hmd = frame.Hmd(); + const auto tnow = frame.tnow; + for (const KeyPress &k : keyPresses) { + (k.right ? keyRightDown : keyLeftDown) = k.down; + if (!k.down) { + if (keyTilting && k.right) { // the tilt ends; the head drags again from here + keyTilting = tilting = false; + hold.engaged = false; + if (debug) std::printf("hold key left: tilt ended\n"); + if (debug) std::fflush(stdout); + } + // The keys holding it: a gaze_left then gaze_right drag, either; otherwise its own. + const bool held = hold.keyDrag ? keyLeftDown || keyRightDown : hold.right ? keyRightDown : keyLeftDown; + if (hold.src == Src::Head && !held) { + if (!hold.pressed && aimHeld && tnow - aimSince < std::chrono::duration(cfg.keyTap)) { + // A quick tap: a click where the dot was at the press, and the gaze was right. + yaw = hold.pressYaw, pitch = hold.pressPitch; + nudgeMoved = 0; + confirmLesson = true; + } + EndHold(false); + keyTilting = false; + } + continue; + } + if (k.right && hold.src == Src::Head && !hold.right && hold.pressed && leftHeld) { + // gaze_right during a gaze_left drag: tilt while it's held (see the top). + tilting = tiltStart = keyTilting = true; + HeadAngles(hmd, keyTiltYaw, keyTiltPitch); + if (debug) std::printf("hold key left: tilt began\n"); + if (debug) std::fflush(stdout); + continue; + } + if (k.right && hold.src == Src::Head && !hold.right && !hold.pressed && aimHeld) { + // gaze_right while gaze_left aims: press the left button where the dot is now + // (the correction is a lesson), then the head drags. + aimHeld = aimHand = false; + hold.pressed = hold.grip = hold.keyDrag = true, hold.promote = false, hold.engaged = false; + HeadAngles(hmd, hold.refYaw, hold.refPitch); + gazeBack = gazeOn; + pressRight = false; + PressLeft(); + if (debug) std::printf("hold key left: pressed (right key)\n"); + if (debug) std::fflush(stdout); + continue; + } + if (hold.src != Src::None || leftHeld || aimHeld || clickPress || clickRelease) continue; + Hold h; + h.src = Src::Head, h.promote = true, h.right = k.right, h.pressYaw = yaw, h.pressPitch = pitch; + if (!HeadAngles(hmd, h.refYaw, h.refPitch)) continue; + hold = h; + StartHold(false, tnow); + if (debug) std::printf("hold key %s began\n", k.right ? "right" : "left"); + if (debug) std::fflush(stdout); + } + keyPresses.clear(); +} + +void Pointer::CalPanelOpened() { + if (calOpened) { // the calibration panel came up: a press in progress ends, no click + calOpened = false; + if (hold.src != Src::None) EndHold(true); + if (leftHeld) ReleaseLeft(); + aimHeld = aimHand = clickPress = false; + } +} + +// A keyboard click's hold: the head steers the pointer, or turns the panel in a tilt. +void Pointer::HeadSteer(const Frame &frame) { + const vr::TrackedDevicePose_t &hmd = frame.Hmd(); + const auto tnow = frame.tnow; + if (hold.src == Src::Head) { + double hy, hp; + if (HeadAngles(hmd, hy, hp) && keyTilting) { + // The head turns the panel, as the mouse does in a tilt; the pointer stays put. + tiltYaw += std::remainder(hy - keyTiltYaw, 360.0); + tiltPitch = std::clamp(tiltPitch + hp - keyTiltPitch, -80.0, 80.0); + keyTiltYaw = hy, keyTiltPitch = hp; + } else if (HeadAngles(hmd, hy, hp)) { + Steer(hy, hp, hold.pressed ? 0.3 : cfg.headDeadzone, 1.0, tnow); + } + } +} + +// Pinches and grips from ft-hands (see "Hands" at the top), read every frame. +void Pointer::PinchesAndGrips(Frame &frame) { + const auto tnow = frame.tnow; + fh_gestures_t hg; + const bool handOk = cfg.handsOn && handFile.Read(hg); + frame.handOk = handOk; + if (handOk && (hg.seq != handSeq || handFile.opens != handOpens)) { + handSeq = hg.seq; + handPublished = hg.publish_ns; + const fh_pinch_t *slots[2] = {hg.pinch, hg.grip}; + // A new file, or a tracker whose counters went back: start counting from here. + bool rebase = !handBaseline || handFile.opens != handOpens; + for (int k = 0; k < 2; ++k) + for (int s = 0; s < 2; ++s) + rebase = rebase || slots[k][s].begins < seenBegins[k][s] || slots[k][s].ends < seenEnds[k][s]; + if (rebase) { + if (hold.src == Src::Hand) EndHold(true); + for (int k = 0; k < 2; ++k) + for (int s = 0; s < 2; ++s) seenBegins[k][s] = slots[k][s].begins, seenEnds[k][s] = slots[k][s].ends; + handBaseline = true, handOpens = handFile.opens; + } + // Not over a VR game (unless the dashboard is up), and not with the headset off. + const bool allowed = !headsetOff && (!inGame || overlay->IsDashboardVisible()); + for (int k = 1; k >= 0; --k) // grips first: one takes over a pinch + for (int s = 0; s < 2; ++s) { + const fh_pinch_t &g = slots[k][s]; + const bool began = g.begins != seenBegins[k][s], ended = g.ends != seenEnds[k][s]; + const bool lost = g.flags & FH_PINCH_LOST; + seenBegins[k][s] = g.begins, seenEnds[k][s] = g.ends; + auto holding = [&] { return hold.src == Src::Hand && hold.side == s && hold.grip == (k == 1); }; + if (holding() && ended) EndHold(lost); // the one held ended (another may have begun) + if (began && allowed) { + BeginHold(s, k == 1, g, tnow); + // begun and ended since the last read (a quick tap): its release too + if (!(g.flags & FH_PINCH_DOWN) && holding()) EndHold(lost); + } + } + // The held gesture's hand moves the pointer: past the dead zone (a tap's jitter, + // the pinch point shifting as the fingers close), then at the gain, from there. + if (hold.src == Src::Hand) { + const fh_pinch_t &g = hold.grip ? hg.grip[hold.side] : hg.pinch[hold.side]; + double hy, hp; + if ((g.flags & FH_PINCH_TRACKED) && HandAngles(g.point, hg.capture_ns, hold.origin, hy, hp)) + Steer(hy, hp, hold.grip ? 0.5 : cfg.pinchDeadzone, hold.grip ? cfg.gripGain : cfg.pinchGain, tnow); + } + } +} + +// ft-hands stopped (or hung) with a gesture held: it's over, as lost. +void Pointer::HandsStopped(const Frame &frame) { + const auto tnow = frame.tnow; + const bool handOk = frame.handOk; + const uint64_t nowNs = + uint64_t(std::chrono::duration_cast(tnow.time_since_epoch()).count()); + if (hold.src == Src::Hand && (!handOk || !active || nowNs - handPublished > 1'500'000'000ull)) EndHold(true); + if (hold.src != Src::None && hold.src != Src::Hand && !active) EndHold(true); +} + +// A press holds the pointer (gaze mode's dot shows meanwhile, see the top). +void Pointer::MarkHeld(const Frame &frame) { + const auto tnow = frame.tnow; + if (aimHeld || leftHeld || clickPress || clickRelease) lastHeld = tnow; +} + +// Head follow (see the top): past the leash (for the delay), ease the reference to the +// head's facing, and turn the cursor with it. Not in gaze mode: the gaze places it. +void Pointer::HeadFollow(const Frame &frame) { + const vr::TrackedDevicePose_t &hmd = frame.Hmd(); + const auto &hm = hmd.mDeviceToAbsoluteTracking.m; + const auto tnow = frame.tnow; + const Vec3 &eye = frame.eye; + if (follow && !gazeOn && active && anchored && hmd.bPoseIsValid) { + const Vec3 head = LimitPitch(Vec3{-hm[0][2], -hm[1][2], -hm[2][2]}, 85); + if (followReset) { + followRef = head, followLag = 0, leashOutSince = {}; + followReset = following = false; + } + const double dt = std::min(0.1, std::chrono::duration(tnow - followAt).count()); + const double leash = cfg.leashDeg * M_PI / 180; + const double lag = std::acos(std::clamp(Dot(followRef, head), -1.0, 1.0)); + double keep = lag; // how far the reference stays behind the head after this frame + if (leash <= 0) { + keep = 0; // head-locked + } else if (following) { + keep = lag * (cfg.leashReturn > 0 ? std::exp(-dt / cfg.leashReturn) : 0.0); + // A fast turn drags it at the leash's end; past it already (after the delay), it + // can't fall further behind, and it closes in from there without a jump. + keep = std::min(keep, std::max(leash, followLag)); + if (keep < 0.05 * M_PI / 180) keep = 0, following = false; // landed on the facing + } else if (lag > leash) { + if (leashOutSince == decltype(leashOutSince){}) leashOutSince = tnow; + if (std::chrono::duration(tnow - leashOutSince).count() >= cfg.leashDelay) + following = true, leashOutSince = {}; + } else { + leashOutSince = {}; // back inside before the delay: a glance + } + followLag = keep; + const Vec3 ref = LimitPitch(PullWithin(followRef, head, keep), 85); + if (!leftHeld && tnow >= dropHoldUntil) { + // The cursor keeps its offset from the reference (frames without roll). + Vec3 d = FromBasis(AimBasis(ref), ToBasis(AimBasis(followRef), Direction(yaw, pitch))); + d = PullWithin(Normalize(d), ref, cfg.followReach * M_PI / 180); + yaw = std::atan2(-d.x, -d.z) * 180 / M_PI; + pitch = std::clamp(std::asin(std::clamp(d.y, -1.0, 1.0)) * 180 / M_PI, -85.0, 85.0); + // The ray origin closes in on the eye as the reference does on the facing. + anchor = eye + (anchor - eye) * (leash <= 0 ? 0.0 : lag > 1e-6 ? keep / lag : following ? 0.0 : 1.0); + } + followRef = ref; + } + followAt = tnow; +} + +// Slow work, once a second: overlay handles, our device index, laser width. +void Pointer::SlowWork() { + const auto now = std::chrono::steady_clock::now(); + if (now - lastSlow > std::chrono::seconds(1)) { + lastSlow = now; + handles.clear(); + for (const auto &key : overlays.Keys()) { + if (Ignored(cfg.ignore, key)) continue; + vr::VROverlayHandle_t h; + if (overlay->FindOverlay(key.c_str(), &h) != vr::VROverlayError_None) continue; + handles[key] = h; + // Scene-graph overlays are placed absolutely. Other overlays can report no + // texture too (gamescope's app panels, placed as dashboard tabs, share theirs + // from another process), and ComputeOverlayIntersection handles those. + uint32_t tw = 0, th = 0; + overlay->GetOverlayTextureSize(h, &tw, &th); + vr::VROverlayTransformType tt = vr::VROverlayTransform_Invalid; + overlay->GetOverlayTransformType(h, &tt); + // ft-screens' panels (frametop.screen.N, floating windows with their popups, + // frametop.float.N..., the keyboard) are 0x0 and absolute too (a shared + // texture), but they're real panels of any size. + sceneGraph[key] = (tw == 0 || th == 0) && tt == vr::VROverlayTransform_Absolute && + key.rfind("frametop.", 0) != 0; + } + ours = vr::k_unTrackedDeviceIndexInvalid; + for (vr::TrackedDeviceIndex_t i = 0; i < vr::k_unMaxTrackedDeviceCount; ++i) { + char type[64] = ""; + sys->GetStringTrackedDeviceProperty(i, vr::Prop_ControllerType_String, type, sizeof type); + if (std::strcmp(type, "ft_pointer") == 0) ours = i; + } + + } + + if (now - lastVisible > std::chrono::milliseconds(50) || visible.size() != handles.size()) { + lastVisible = now; + visible.clear(); + for (const auto &[key, h] : handles) visible[key] = overlay->IsOverlayVisible(h); + } +} + +// The cursor: tilting a grabbed panel, or pointing (find the spot, draw the dot, send the ray). +void Pointer::Cursor(const Frame &frame) { + const vr::TrackedDevicePose_t &hmd = frame.Hmd(); + if (active && anchored && hmd.bPoseIsValid && tilting) { + Tilt(frame); + } else if (active && anchored && hmd.bPoseIsValid) { + const Spot spot = FindCursor(frame); + DrawDot(frame, spot); + SendRay(frame, spot); + } +} + +void Pointer::Tilt(const Frame &frame) { + const vr::TrackedDevicePose_t &hmd = frame.Hmd(); + const Vec3 &eye = frame.eye; + const vr::TrackedDevicePose_t &hmdRaw = frame.hmdRaw; + // Rotate the device around the grab point; the grabbed panel turns with it. + if (tiltStart) { + pivot = lastPoint; + tiltOrigin = lastOrigin; + tiltBasis = AimBasis(lastAim); + tiltStart = false; // angles carry on from any earlier tilt in this drag + overlay->HideOverlay(cursor); + overlay->HideOverlay(marker); + } + const double yr = tiltYaw * M_PI / 180, pr = tiltPitch * M_PI / 180; + const Vec3 up{0, 1, 0}, side = tiltBasis.x; + auto turn = [&](Vec3 v) { return RotateAbout(RotateAbout(v, side, pr), up, yr); }; + const Vec3 originStanding = pivot + turn(tiltOrigin - pivot); + const Basis b{turn(tiltBasis.x), turn(tiltBasis.y), turn(tiltBasis.z)}; + const auto &S = hmd.mDeviceToAbsoluteTracking, &R = hmdRaw.mDeviceToAbsoluteTracking; + auto toRaw = [&](Vec3 v) { return Rotate(R, RotateInverse(S, v)); }; // directions: raw <- standing + const Vec3 originRaw = Position(R) + toRaw(originStanding - eye); + double q[4]; + BasisQuat({toRaw(b.x), toRaw(b.y), toRaw(b.z)}, q); + char msg[200]; + std::snprintf(msg, sizeof msg, "posq %.5f %.5f %.5f %.6f %.6f %.6f %.6f", originRaw.x, originRaw.y, + originRaw.z, q[0], q[1], q[2], q[3]); + SendTo(out, "ft_pointer", msg); +} + +// Nearest visible overlay along a ray (frozen while dragging). +Pointer::Hit Pointer::Nearest(Vec3 from, Vec3 d) { + Hit h; + for (const auto &[key, handle] : handles) { + if (!visible[key]) continue; + if (sceneGraph[key]) { + // Plane test: overlay origin and its +Z normal, within sceneRadius of the origin. + vr::ETrackingUniverseOrigin uo; + vr::HmdMatrix34_t t{}; + if (overlay->GetOverlayTransformAbsolute(handle, &uo, &t) != vr::VROverlayError_None) continue; + const Vec3 center = Position(t), normal{t.m[0][2], t.m[1][2], t.m[2][2]}; + const double denom = Dot(d, normal); + if (std::fabs(denom) < 1e-4) continue; + const double along = Dot(center - from, normal) / denom; + const Vec3 at = from + d * along; + if (along > 0.05 && along < h.along && std::sqrt(Dot(at - center, at - center)) <= cfg.sceneRadius) + h.along = along, h.key = key, h.scene = true, h.point = at, h.normal = normal; + continue; + } + vr::VROverlayIntersectionParams_t params{}; + params.vSource = {float(from.x), float(from.y), float(from.z)}; + params.vDirection = {float(d.x), float(d.y), float(d.z)}; + params.eOrigin = vr::TrackingUniverseStanding; + vr::VROverlayIntersectionResults_t r{}; + if (overlay->ComputeOverlayIntersection(handle, ¶ms, &r) && r.fDistance > 0.05f && + r.fDistance < h.along) { + h.along = r.fDistance, h.key = key, h.scene = false; + h.point = {r.vPoint.v[0], r.vPoint.v[1], r.vPoint.v[2]}; + h.normal = {r.vNormal.v[0], r.vNormal.v[1], r.vNormal.v[2]}; + } + } + return h; +} + +// Where the cursor is this frame: on a panel, a scene-graph plane, or a panel's edge, or out in free +// space. +Pointer::Spot Pointer::FindCursor(const Frame &frame) { + const auto tnow = frame.tnow; + const Vec3 &eye = frame.eye; + const bool dragging = leftHeld || tnow < dropHoldUntil; + if (!dragging) tiltYaw = tiltPitch = 0; // drop finished: back to plain pointing + const Vec3 dir = Direction(yaw, pitch); + Hit first; + if (!dragging) first = Nearest(anchor, dir); + double best = first.along; + std::string bestKey = first.key; + bool bestScene = first.scene; + Vec3 bestPoint = first.point, bestNormal = first.normal; + bool onEdge = false; + if (!dragging && best < 1e8 && !bestScene) { + edgeKey = bestKey, edgePoint = bestPoint, edgeNormal = Normalize(bestNormal), edgeLast = bestPoint; + } else if (!dragging && best >= 1e8 && !edgeKey.empty() && visible[edgeKey]) { + // Just off a panel: stay on its plane (see "Panel edges" at the top). + const double denom = Dot(dir, edgeNormal); + if (std::fabs(denom) > 1e-4) { + const double along = Dot(edgePoint - anchor, edgeNormal) / denom; + const Vec3 at = anchor + dir * along; + if (along > 0.05 && std::sqrt(Dot(at - edgeLast, at - edgeLast)) <= cfg.edgeReach) { + best = along; + bestKey = edgeKey; + onEdge = true; + } + } + } + // Held on one of ft-screens' panels that stays where it is: onto whichever of them + // the ray meets (see the top). + if (leftHeld && !pressKey.empty()) { + vr::ETrackingUniverseOrigin uo; + vr::HmdMatrix34_t now{}; + auto it = handles.find(pressKey); + bool still = it != handles.end() && + overlay->GetOverlayTransformAbsolute(it->second, &uo, &now) == vr::VROverlayError_None; + for (int i = 0; still && i < 3; ++i) + for (int j = 0; j < 4; ++j) + if (std::fabs(now.m[i][j] - pressPose.m[i][j]) > 0.001f) still = false; + if (still) { + Hit h; + for (const auto &[key, handle] : handles) { + if (!visible[key] || !FramePanel(key)) continue; + vr::VROverlayIntersectionParams_t params{}; + params.vSource = {float(anchor.x), float(anchor.y), float(anchor.z)}; + params.vDirection = {float(dir.x), float(dir.y), float(dir.z)}; + params.eOrigin = vr::TrackingUniverseStanding; + vr::VROverlayIntersectionResults_t r{}; + if (overlay->ComputeOverlayIntersection(handle, ¶ms, &r) && r.fDistance > 0.05f && + r.fDistance < h.along) + h.along = r.fDistance, h.key = key; + } + if (h.along < 1e8) dragDistance = h.along, lastHit = h.key; + } else { + pressKey.clear(); // carried: the lock holds for the rest of this press + } + } + // While dragging: keep the press-time distance and show the non-interactive marker. + // On a scene-graph plane or a panel's edge: the laser-catching dot goes 5 cm behind it. + double distance = dragging ? dragDistance : (best < 1e8 ? best : cfg.freeDistance); + Vec3 point = anchor + dir * distance; + bool occluded = false; + if (!dragging) { + // The cursor lands on what you see under it: the ray above starts at the anchor, + // not the eye, so after leaning it can pick a panel that something nearer + // covers from where you are now (panels close together in view, at different + // depths). Anything in front of the point on the eye's line of sight wins. + const double toPoint = std::sqrt(Dot(point - eye, point - eye)); + const Hit front = Nearest(eye, Normalize(point - eye)); + if (front.along < toPoint - 0.02) { + occluded = true; + bestKey = front.key, bestScene = front.scene; + point = front.point; + if (!front.scene) edgeKey = front.key, edgePoint = front.point, edgeNormal = Normalize(front.normal), + edgeLast = front.point; + distance = std::sqrt(Dot(point - anchor, point - anchor)); + best = distance; + onEdge = false; + } + lastDistance = distance, lastHit = bestKey; + } + const bool onScene = !dragging && ((bestScene && best < 1e8) || onEdge); + const bool onPanel = dragging || (best < 1e8 && !onScene); + Spot s; + s.dragging = dragging, s.dir = dir, s.point = point, s.best = best, s.distance = distance; + s.onEdge = onEdge, s.occluded = occluded, s.onScene = onScene, s.onPanel = onPanel; + return s; +} + +// Our dot where the cursor is (see "Looks" and "SteamVR Settings" at the top). +void Pointer::DrawDot(const Frame &frame, const Spot &spot) { + const auto tnow = frame.tnow; + const Vec3 &eye = frame.eye; + const bool dragging = spot.dragging, onScene = spot.onScene, onPanel = spot.onPanel; + const Vec3 dir = spot.dir, point = spot.point; + const Vec3 sight = Normalize(point - eye); + if (!dragging) { + // SteamVR Settings (see the top): the dashboard's main panel is hidden and its + // scene-graph panel shows the page. + onVrSettings = false; + const auto mainIt = visible.find("valve.steam.gamepadui.main"); + if (overlay->IsDashboardVisible() && !(mainIt != visible.end() && mainIt->second)) { + for (const auto &[key, handle] : handles) { + if (!visible[key] || key.rfind("valve.steam.gamepadui.frame.menu.", 0) != 0) continue; + vr::ETrackingUniverseOrigin uo; + vr::HmdMatrix34_t t{}; + if (overlay->GetOverlayTransformAbsolute(handle, &uo, &t) == vr::VROverlayError_None && + OnSettingsPage(t, eye, sight)) + onVrSettings = true; + } + } + } + if (onVrSettings != catcherHidesHit) { + overlay->SetOverlayFlag(cursor, vr::VROverlayFlags_HideLaserIntersection, onVrSettings); + catcherHidesHit = onVrSettings; + } + + if (onVrSettings) { + // The dot close in front of the page, which is nearer than any guess of ours; + // the laser-catching dot far behind everything, invisible, so it never covers + // the page, with SteamVR's hit dot hidden on it. + const Vec3 near = eye + sight * SETTINGS_DOT, far = eye + sight * SETTINGS_CATCHER; + double alpha = 1; + if (gazeOn && !cfg.gazeDotAlways) { + auto secs = [&](Clock::time_point t) { return std::chrono::duration(tnow - t).count(); }; + alpha = std::clamp(1 - std::min(secs(lastMove) - cfg.gazeShow, secs(lastHeld)) / 0.25, 0.0, 1.0); + } + if (tnow < calPanelUntil) alpha = 0; + overlay->SetOverlayAlpha(marker, float(alpha)); + overlay->SetOverlayWidthInMeters(marker, float(2 * SETTINGS_DOT * std::tan(cfg.cursorDeg * M_PI / 360))); + auto mm = Billboard(near, eye); + overlay->SetOverlayTransformAbsolute(marker, vr::TrackingUniverseStanding, &mm); + overlay->SetOverlayAlpha(cursor, 0); + overlay->SetOverlayWidthInMeters(cursor, float(2 * SETTINGS_CATCHER * std::tan(cfg.cursorDeg * M_PI / 360))); + auto mc = Billboard(far, eye); + overlay->SetOverlayTransformAbsolute(cursor, vr::TrackingUniverseStanding, &mc); + overlay->ShowOverlay(marker); + overlay->ShowOverlay(cursor); + } else { + // On a panel: the non-interactive marker, pulled 5 mm toward the eye so it + // draws on top. In free space: the interactive dot the laser lands on. + const vr::VROverlayHandle_t show = onPanel ? marker : cursor, hide = onPanel ? cursor : marker; + const Vec3 at = onPanel ? point + Normalize(eye - point) * 0.005 : onScene ? point + dir * 0.05 : point; + const double dist = std::sqrt(Dot(at - eye, at - eye)); + // Gaze mode: a pulse for each click, and with POINTER_GAZE_DOT=moving, shown only + // while something moves it or a press holds it (see the top); transparent + // otherwise, the laser still lands on it. + double scale = 1, alpha = 1; + if (gazeOn) { + auto secs = [&](Clock::time_point t) { return std::chrono::duration(tnow - t).count(); }; + alpha = cfg.gazeDotAlways ? 1.0 : std::clamp(1 - std::min(secs(lastMove) - cfg.gazeShow, secs(lastHeld)) / 0.25, 0.0, 1.0); + const double pulse = secs(pulseAt); + if (pulse < 0.6) { + scale = 1 + 1.5 * std::max(0.0, 1 - pulse / 0.3); + alpha = std::max(alpha, std::clamp((0.6 - pulse) / 0.3, 0.0, 1.0)); + } + } + if (tnow < calPanelUntil) alpha = 0; // the calibration panel is up (see the top) + overlay->SetOverlayAlpha(show, float(alpha)); + overlay->SetOverlayWidthInMeters(show, float(2 * dist * std::tan(scale * cfg.cursorDeg * M_PI / 360))); + auto m = Billboard(at, eye); + overlay->SetOverlayTransformAbsolute(show, vr::TrackingUniverseStanding, &m); + overlay->ShowOverlay(show); + overlay->HideOverlay(hide); + } +} + +void Pointer::SendRay(const Frame &frame, const Spot &spot) { + const vr::TrackedDevicePose_t &hmd = frame.Hmd(); + const auto tnow = frame.tnow; + const Vec3 &eye = frame.eye; + const vr::TrackedDevicePose_t &hmdRaw = frame.hmdRaw; + const bool dragging = spot.dragging, occluded = spot.occluded, onEdge = spot.onEdge, onScene = spot.onScene; + const double best = spot.best, distance = spot.distance; + const Vec3 point = spot.point; + // Controller ray: from the eye, aimed at the cursor point, converted from the + // standing universe to raw tracking space via the HMD's pose in both. + const Vec3 aimStanding = Normalize(point - eye); + const auto &S = hmd.mDeviceToAbsoluteTracking, &R = hmdRaw.mDeviceToAbsoluteTracking; + const Vec3 aim = Rotate(R, RotateInverse(S, aimStanding)); // raw <- head <- standing + // Origin partway along the line of sight to the cursor (smaller hit dot). + const double toPoint = std::sqrt(Dot(point - eye, point - eye)); + double originDist = std::max(0.0, std::min(toPoint * cfg.originFraction, toPoint - cfg.originMargin)); + if (onVrSettings) originDist = std::min(originDist, SETTINGS_ORIGIN); // SteamVR finds the page + const Vec3 originStanding = eye + Normalize(point - eye) * originDist; + const Vec3 eyeRaw = Position(R) + Rotate(R, RotateInverse(S, originStanding - eye)); + lastPoint = point, lastOrigin = originStanding, lastAim = aimStanding; // tilt starts from here + havePoint = true; + if (debug && tnow - lastDebug > std::chrono::milliseconds(500)) { + lastDebug = tnow; + if (systemPointer == vr::k_ulOverlayHandleInvalid) overlay->FindOverlay("system.pointer", &systemPointer); + std::printf("dbg %s hit=%s dist=%.2f eye->point=%.2f origin=%.2f vrsettings=%d yaw=%.1f pitch=%.1f gaze=%s steamvr_dot=%d primary=%u\n", + dragging ? "DRAG" : occluded ? "INFRONT" : onEdge ? "EDGE" : onScene ? "SCENE" : (best < 1e8 ? "PANEL" : "FREE"), lastHit.empty() ? "-" : lastHit.c_str(), + distance, toPoint, originDist, onVrSettings, yaw, pitch, + !gazeOn ? "off" : tnow - gz.at > std::chrono::milliseconds(150) ? "stale" : gazeOwns ? "owns" : "mouse", + systemPointer != vr::k_ulOverlayHandleInvalid && overlay->IsOverlayVisible(systemPointer), + overlay->GetPrimaryDashboardDevice()); + std::fflush(stdout); + } + const double ayaw = std::atan2(-aim.x, -aim.z) * 180 / M_PI; + const double apitch = std::asin(std::clamp(aim.y, -1.0, 1.0)) * 180 / M_PI; + if (dragging && (tiltYaw != 0 || tiltPitch != 0)) { + // Keep this drag's tilt applied, about the current cursor point. + const double yr = tiltYaw * M_PI / 180, pr = tiltPitch * M_PI / 180; + const Basis base = AimBasis(aimStanding); + const Vec3 up{0, 1, 0}, side = base.x; + auto turn = [&](Vec3 v) { return RotateAbout(RotateAbout(v, side, pr), up, yr); }; + const Vec3 o = point + turn(originStanding - point); + const Basis b{turn(base.x), turn(base.y), turn(base.z)}; + auto toRaw = [&](Vec3 v) { return Rotate(R, RotateInverse(S, v)); }; + const Vec3 oRaw = Position(R) + toRaw(o - eye); + double q[4]; + BasisQuat({toRaw(b.x), toRaw(b.y), toRaw(b.z)}, q); + char msg[200]; + std::snprintf(msg, sizeof msg, "posq %.5f %.5f %.5f %.6f %.6f %.6f %.6f", oRaw.x, oRaw.y, oRaw.z, q[0], + q[1], q[2], q[3]); + SendTo(out, "ft_pointer", msg); + } else { + char msg[160]; + std::snprintf(msg, sizeof msg, "pose %.5f %.5f %.5f %.4f %.4f", eyeRaw.x, eyeRaw.y, eyeRaw.z, ayaw, apitch); + SendTo(out, "ft_pointer", msg); + } +} + +// A held-back press (see the top): held still long enough, it's a real press (a drag); +// released, it's a click where the pointer is now (this frame's pose has gone out). +void Pointer::HeldBackPress(const Frame &frame) { + const auto tnow = frame.tnow; + if (!active) aimHeld = aimRight = clickPress = aimHand = confirmLesson = false; // released meanwhile: nothing to click + if (aimHeld && !aimHand && nudgeMoved < 0.2 && tnow - aimSince >= std::chrono::duration(cfg.gazeHold)) { + aimHeld = false; + gazeBack = true; + pressRight = aimRight; // the right button's: a real right press (see the top) + aimRight = false; + PressLeft(); + } +} + +// A keyboard click held still for POINTER_GAZE_HOLD: a real press, then the head drags +// (from where it is now). +void Pointer::KeyboardClickHold(const Frame &frame) { + const vr::TrackedDevicePose_t &hmd = frame.Hmd(); + const auto &hm = hmd.mDeviceToAbsoluteTracking.m; + const auto tnow = frame.tnow; + if (aimHeld && aimHand && hold.promote && !hold.engaged && + tnow - aimSince >= std::chrono::duration(cfg.gazeHold)) { + aimHeld = aimHand = false; + hold.pressed = hold.grip = true; + if (hmd.bPoseIsValid) { + const Vec3 f = Normalize({-hm[0][2], -hm[1][2], -hm[2][2]}); + hold.refYaw = std::atan2(-f.x, -f.z) * 180 / M_PI; + hold.refPitch = std::asin(std::clamp(f.y, -1.0, 1.0)) * 180 / M_PI; + } + gazeBack = gazeOn; + pressRight = hold.right; + PressLeft(); + if (debug) std::printf("hold key %s: pressed (held still)\n", hold.right ? "right" : "left"); + if (debug) std::fflush(stdout); + } +} + +// The click a held-back press ends in: the press now, the release 40 ms later. +void Pointer::Click(const Frame &frame) { + const auto tnow = frame.tnow; + if (clickPress) { + clickPress = false; + PressLeft(); + clickRelease = true; + clickReleaseAt = tnow + std::chrono::milliseconds(40); + } else if (clickRelease && tnow >= clickReleaseAt) { + clickRelease = false; + ReleaseLeft(); + } +} + +// Frame controller buttons (vrbuttons.h), to the relay. +void Pointer::PollControllerButtons() { + controllerButtons.Poll( + [&](const char *button, bool down) { + SendTo(out, "frametop_relay", std::string("vrbtn ") + button + (down ? " 1" : " 0")); + }, + inGame); +} + +// SteamVR quits: shut down (true: main returns). +bool Pointer::SteamVRQuit() { + vr::VREvent_t ev; + while (sys->PollNextEvent(&ev, sizeof ev)) { + if (ev.eventType == vr::VREvent_Quit) { + sys->AcknowledgeQuit_Exiting(); + + overlays.Stop(); + vr::VR_Shutdown(); + return true; + } + } + return false; +} + int main() { Pointer p; p.Init(); - return p.Run(); + // Each frame: the sections in this order (Pointer's members above). + while (true) { + p.HeadsetOff(); + p.GazeAwake(); + p.RelayCommands(); + Frame frame; + p.ReadFrame(frame); + p.ClaimPulse(frame); + p.PauseOverlayList(); + p.LaserMode(); + p.HandRole(frame); + p.LastUsedWins(frame); + p.Recenter(frame); + p.GazeMode(frame); + p.Hands(frame); + p.GazePrecisionButtons(frame); + p.KeyboardClicks(frame); + p.CalPanelOpened(); + p.HeadSteer(frame); + p.PinchesAndGrips(frame); + p.HandsStopped(frame); + p.MarkHeld(frame); + p.HeadFollow(frame); + p.SlowWork(); + p.Cursor(frame); + p.HeldBackPress(frame); + p.KeyboardClickHold(frame); + p.Click(frame); + p.PollControllerButtons(); + if (p.SteamVRQuit()) return 0; + std::this_thread::sleep_for(std::chrono::milliseconds(8)); + } }