From b4b813990210f72eb756465f908405f63fa51175 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?L=27=C3=A9lectron=20rare?=
 <108685187+electron-rare@users.noreply.github.com>
Date: Fri, 26 Jun 2026 11:53:45 +0200
Subject: [PATCH] feat(ios): vision hands and face over usb

---
 .../Sources/ARBodyTracker/ARBodySession.swift | 83 +++++++++++++++++++
 1 file changed, 83 insertions(+)

diff --git a/iphone-arbody/ARBodyTracker.swiftpm/Sources/ARBodyTracker/ARBodySession.swift b/iphone-arbody/ARBodyTracker.swiftpm/Sources/ARBodyTracker/ARBodySession.swift
index baca4dd..8c6d09c 100644
--- a/iphone-arbody/ARBodyTracker.swiftpm/Sources/ARBodyTracker/ARBodySession.swift
+++ b/iphone-arbody/ARBodyTracker.swiftpm/Sources/ARBodyTracker/ARBodySession.swift
@@ -4,6 +4,7 @@ import Combine
 import Foundation
 import RealityKit
 import SwiftUI
+import Vision
 
 /// Drives the ARKit body-tracking session and streams the 91-joint
 /// skeleton plus HEVC-encoded camera video to the tethered Mac over
@@ -37,6 +38,10 @@ final class ARBodySession: NSObject, ObservableObject, ARSessionDelegate {
     private let videoEncoder = VideoEncoder()
     private var videoStarted = false
     private var lastFrameTime: TimeInterval = 0
+    private let visionQueue = DispatchQueue(
+        label: "cc.saillant.arbody.vision", qos: .userInitiated)
+    private var visionBusy = false
+    private var lastVisionTime: TimeInterval = 0
     private var jointsInSecond: Int = 0
     private var lastSecond: TimeInterval = 0
     private let bodyParents: [Int] =
@@ -126,6 +131,17 @@ final class ARBodySession: NSObject, ObservableObject, ARSessionDelegate {
                 self.videoEncoder.encode(img, pts: t)
             }
 
+            // Vision hands+face at ~15 fps, off-main, drop if busy.
+            if t - self.lastVisionTime >= 1.0 / 15.0, !self.visionBusy {
+                self.lastVisionTime = t
+                self.visionBusy = true
+                let buf = img   // CVPixelBuffer captured for the worker
+                self.visionQueue.async {
+                    self.runVision(on: buf, timestamp: t)
+                    Task { @MainActor in self.visionBusy = false }
+                }
+            }
+
             var count: Int = 0
             var firstBody: ARBodyAnchor?
             for anchor in frame.anchors {
@@ -187,6 +203,73 @@ final class ARBodySession: NSObject, ObservableObject, ARSessionDelegate {
     /// indices without re-reading the ARKit skeleton definition.
     var bodyParentIndices: [Int] { bodyParents }
 
+    // MARK: - Vision hand + face capture
+
+    nonisolated func runVision(on pixelBuffer: CVPixelBuffer,
+                               timestamp t: TimeInterval) {
+        let handReq = VNDetectHumanHandPoseRequest()
+        handReq.maximumHandCount = 2
+        let faceReq = VNDetectFaceLandmarksRequest()
+        let handler = VNImageRequestHandler(
+            cvPixelBuffer: pixelBuffer, orientation: .right, options: [:])
+        try? handler.perform([handReq, faceReq])
+
+        // ---- hands ----
+        // Fixed 21-joint order matching MediaPipe-style indexing.
+        let order: [VNHumanHandPoseObservation.JointName] = [
+            .wrist,
+            .thumbCMC, .thumbMP, .thumbIP, .thumbTip,
+            .indexMCP, .indexPIP, .indexDIP, .indexTip,
+            .middleMCP, .middlePIP, .middleDIP, .middleTip,
+            .ringMCP, .ringPIP, .ringDIP, .ringTip,
+            .littleMCP, .littlePIP, .littleDIP, .littleTip,
+        ]
+        var handsOut: [HandsPayload.Hand] = []
+        for obs in (handReq.results ?? []).prefix(2) {
+            guard let pts = try? obs.recognizedPoints(.all) else { continue }
+            let p = order.map { name -> SIMD3<Float> in
+                if let rp = pts[name] {
+                    // Vision: normalized, bottom-left origin -> flip y.
+                    return SIMD3(Float(rp.location.x),
+                                 Float(1.0 - rp.location.y),
+                                 Float(rp.confidence))
+                }
+                return .zero
+            }
+            handsOut.append(HandsPayload.Hand(
+                isRight: obs.chirality == .right, points: p))
+        }
+
+        // ---- face (most prominent) ----
+        var facePayload: FacePayload?
+        if let face = (faceReq.results ?? []).first,
+           let all = face.landmarks?.allPoints {
+            let bb = face.boundingBox
+            let pts = all.normalizedPoints.map { np -> SIMD2<Float> in
+                // landmark points are relative to the face bbox.
+                let x = bb.origin.x + Double(np.x) * bb.size.width
+                let y = bb.origin.y + Double(np.y) * bb.size.height
+                return SIMD2(Float(x), Float(1.0 - y))
+            }
+            facePayload = FacePayload(
+                confidence: face.confidence, points: pts)
+        }
+
+        let handsData = HandsPayload(hands: handsOut).encoded()
+        let faceData = facePayload?.encoded()
+        Task { @MainActor in
+            guard self.usbState == .connected else { return }
+            self.usb.send(tag: .hands, pid: -1,
+                          timestamp: t, payload: handsData)
+            if let faceData {
+                self.usb.send(tag: .face, pid: -1,
+                              timestamp: t, payload: faceData)
+            }
+        }
+    }
+
+    // MARK: - USB skeleton
+
     private func publishUSB(pid: Int, timestamp: TimeInterval,
                              body: ARBodyAnchor) {
         guard usbState == .connected else { return }