feat(av-live): face+hand+3D pose to launcher
Stream MediaPipe Holistic face landmarks (68 dlib subset of 478), hand landmarks (21 left + 21 right), and pose world landmarks (33 3D xyz meters) over OSC :57126 to AVLiveBody. Launcher renders face/hand as SwiftUI Canvas overlay and the 3D skeleton as a RealityKit armature (sphere joints + cylinder bones, color per chain) toggled via p / mode openpos. Multi-HMR worker now also starts MediaPipe Multi in parallel so both the dense SMPL-X mesh (TCP 57130, PyTorch backend) and the skeleton/face/hand streams (OSC 57126) feed the launcher from one Python process. Launcher AppDelegate forces .regular activation so SwiftPM binaries actually show their WindowGroup without a bundle. Tests: 9 new pytest cases (4 body3d + 5 face/hand), all green. CoreML conversion still produces NaN on v3d/transl; PyTorch backend is the working path for now.
This commit is contained in:
L'électron rare
@@ -1,8 +1,19 @@
|
||||
import Cocoa
|
||||
import SwiftUI
|
||||
|
||||
// SwiftPM binaries lack a bundle Info.plist, so macOS treats us as a
|
||||
// background CLI app and never shows the WindowGroup window. The
|
||||
// AppDelegate forces regular activation after NSApp is initialized.
|
||||
class AppDelegate: NSObject, NSApplicationDelegate {
|
||||
func applicationDidFinishLaunching(_ notification: Notification) {
|
||||
NSApp.setActivationPolicy(.regular)
|
||||
NSApp.activate(ignoringOtherApps: true)
|
||||
}
|
||||
}
|
||||
|
||||
@main
|
||||
struct AVLiveBodyApp: App {
|
||||
@NSApplicationDelegateAdaptor(AppDelegate.self) var appDelegate
|
||||
var body: some Scene {
|
||||
WindowGroup {
|
||||
ContentView()
|
||||
@@ -46,6 +57,11 @@ struct AVLiveBodyApp: App {
|
||||
KeyEquivalent(Character(String(i))),
|
||||
modifiers: [])
|
||||
}
|
||||
// Alias 'p' for openpos (skeleton view).
|
||||
Button("p — openpos (squelette)") {
|
||||
NotificationCenter.default.post(
|
||||
name: .setVizMode, object: 9)
|
||||
}.keyboardShortcut("p", modifiers: [])
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -61,11 +77,12 @@ struct ContentView: View {
|
||||
@StateObject private var renderer = MeshRenderer()
|
||||
@StateObject private var settings = RenderSettings()
|
||||
@StateObject private var poseListener = PoseOSCListener()
|
||||
@StateObject private var skeleton3d = Skeleton3DRenderer()
|
||||
|
||||
var body: some View {
|
||||
ZStack(alignment: .topLeading) {
|
||||
BodyView(renderer: renderer, settings: settings,
|
||||
poseListener: poseListener)
|
||||
poseListener: poseListener, skeleton3d: skeleton3d)
|
||||
.onAppear {
|
||||
renderer.startOSCServer()
|
||||
poseListener.start()
|
||||
@@ -83,6 +100,10 @@ struct ContentView: View {
|
||||
if let n = note.object as? Int { settings.vizMode = n }
|
||||
}
|
||||
|
||||
// Face + hand skeleton overlay (data_only_viz/pose_bridge.py)
|
||||
FaceHandOverlay(poseListener: poseListener)
|
||||
.allowsHitTesting(false)
|
||||
|
||||
// HUD coin haut-gauche : mode + touches + pose
|
||||
HUDOverlay(settings: settings, poseListener: poseListener)
|
||||
|
||||
|
||||
@@ -11,6 +11,7 @@ struct BodyView: NSViewRepresentable {
|
||||
@ObservedObject var renderer: MeshRenderer
|
||||
@ObservedObject var settings: RenderSettings
|
||||
@ObservedObject var poseListener: PoseOSCListener
|
||||
@ObservedObject var skeleton3d: Skeleton3DRenderer
|
||||
|
||||
func makeNSView(context: Context) -> NSView {
|
||||
let container = NSView(frame: .zero)
|
||||
@@ -88,6 +89,14 @@ struct BodyView: NSViewRepresentable {
|
||||
|
||||
let bodyAnchor = AnchorEntity(world: .zero)
|
||||
arView.scene.addAnchor(bodyAnchor)
|
||||
|
||||
// Dedicated anchor for the 3D skeleton (mode 9 / openpos).
|
||||
// Positioned at the origin ; the perspective camera at z=0 with
|
||||
// default FOV frames a ~3 m-deep stage centered on the hip.
|
||||
let skelAnchor = AnchorEntity(world: SIMD3<Float>(0, 0, -3))
|
||||
arView.scene.addAnchor(skelAnchor)
|
||||
skeleton3d.attach(to: skelAnchor, listener: poseListener)
|
||||
|
||||
container.addSubview(arView)
|
||||
|
||||
// 60 fps mesh interpolation between Multi-HMR frames (Python
|
||||
@@ -105,6 +114,7 @@ struct BodyView: NSViewRepresentable {
|
||||
context.coordinator.previewLayer = preview
|
||||
context.coordinator.container = container
|
||||
context.coordinator.renderer = renderer
|
||||
context.coordinator.skelAnchor = skelAnchor
|
||||
return container
|
||||
}
|
||||
|
||||
@@ -141,6 +151,9 @@ struct BodyView: NSViewRepresentable {
|
||||
c.fillLight?.light.intensity = Float(settings.fillIntensity)
|
||||
c.rimLight?.light.intensity = Float(settings.rimIntensity)
|
||||
|
||||
// 3D skeleton only visible in mode 9 (openpos).
|
||||
c.skelAnchor?.isEnabled = (settings.vizMode == 9)
|
||||
|
||||
// Mesh visibility + material
|
||||
guard let anchor = c.bodyAnchor else { return }
|
||||
anchor.children.removeAll()
|
||||
@@ -159,6 +172,7 @@ struct BodyView: NSViewRepresentable {
|
||||
|
||||
final class Coordinator {
|
||||
var bodyAnchor: AnchorEntity?
|
||||
var skelAnchor: AnchorEntity?
|
||||
var arView: ARView?
|
||||
var cameraEntity: PerspectiveCamera?
|
||||
var sceneRenderer: SceneRenderer?
|
||||
|
||||
@@ -0,0 +1,155 @@
|
||||
import SwiftUI
|
||||
import simd
|
||||
|
||||
/// Lightweight SwiftUI overlay that draws the 68-point face skeleton and
|
||||
/// 21-point hand skeletons sent by data_only_viz/pose_bridge.py over OSC.
|
||||
/// Sits in the ContentView ZStack above BodyView. Coordinates from the
|
||||
/// listener are normalised (0..1 in image space) ; here we map them to
|
||||
/// the overlay's geometry. Rendering is intentionally minimal : small
|
||||
/// dots + a few polylines for facial features and hand bones.
|
||||
struct FaceHandOverlay: View {
|
||||
@ObservedObject var poseListener: PoseOSCListener
|
||||
var showFace: Bool = true
|
||||
var showHands: Bool = true
|
||||
|
||||
var body: some View {
|
||||
GeometryReader { geo in
|
||||
Canvas { ctx, size in
|
||||
if showFace {
|
||||
for face in poseListener.faces.values {
|
||||
drawFace(face, in: &ctx, size: size)
|
||||
}
|
||||
}
|
||||
if showHands {
|
||||
for hand in poseListener.hands.values {
|
||||
drawHand(hand, in: &ctx, size: size)
|
||||
}
|
||||
}
|
||||
}
|
||||
.frame(width: geo.size.width, height: geo.size.height)
|
||||
.allowsHitTesting(false)
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Face (dlib 68 layout)
|
||||
|
||||
/// Index spans in the 68-point dlib convention.
|
||||
private static let jaw = Array(0..<17)
|
||||
private static let browR = Array(17..<22)
|
||||
private static let browL = Array(22..<27)
|
||||
private static let noseBridge = Array(27..<31)
|
||||
private static let nostril = Array(31..<36)
|
||||
private static let eyeR = Array(36..<42)
|
||||
private static let eyeL = Array(42..<48)
|
||||
private static let lipOuter = Array(48..<60)
|
||||
private static let lipInner = Array(60..<68)
|
||||
|
||||
private func drawFace(_ face: PoseOSCListener.FaceFrame,
|
||||
in ctx: inout GraphicsContext,
|
||||
size: CGSize) {
|
||||
let stroke = GraphicsContext.Shading.color(.green.opacity(0.85))
|
||||
let dot = GraphicsContext.Shading.color(.green.opacity(0.95))
|
||||
|
||||
drawPolyline(face.points, indices: Self.jaw, closed: false,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.browR, closed: false,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.browL, closed: false,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.noseBridge, closed: false,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.nostril, closed: false,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.eyeR, closed: true,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.eyeL, closed: true,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.lipOuter, closed: true,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.2)
|
||||
drawPolyline(face.points, indices: Self.lipInner, closed: true,
|
||||
in: &ctx, size: size, shading: stroke, width: 1.0)
|
||||
|
||||
for i in 0..<68 where face.hasPoint[i] {
|
||||
let p = mapPoint(face.points[i], size: size)
|
||||
let r = CGRect(x: p.x - 1.2, y: p.y - 1.2,
|
||||
width: 2.4, height: 2.4)
|
||||
ctx.fill(Path(ellipseIn: r), with: dot)
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Hand (MediaPipe 21 landmarks)
|
||||
|
||||
/// MediaPipe hand bone connectivity (5 fingers x 4 bones + palm).
|
||||
private static let handBones: [(Int, Int)] = [
|
||||
// Thumb
|
||||
(0, 1), (1, 2), (2, 3), (3, 4),
|
||||
// Index
|
||||
(0, 5), (5, 6), (6, 7), (7, 8),
|
||||
// Middle
|
||||
(5, 9), (9, 10), (10, 11), (11, 12),
|
||||
// Ring
|
||||
(9, 13), (13, 14), (14, 15), (15, 16),
|
||||
// Pinky
|
||||
(13, 17), (17, 18), (18, 19), (19, 20),
|
||||
// Palm closure
|
||||
(0, 17),
|
||||
]
|
||||
|
||||
private func drawHand(_ hand: PoseOSCListener.HandFrame,
|
||||
in ctx: inout GraphicsContext,
|
||||
size: CGSize) {
|
||||
// Left = cyan, right = magenta.
|
||||
let color: Color = hand.side == 0 ? .cyan : .pink
|
||||
let stroke = GraphicsContext.Shading.color(color.opacity(0.85))
|
||||
let dot = GraphicsContext.Shading.color(color.opacity(0.95))
|
||||
|
||||
for (a, b) in Self.handBones {
|
||||
guard hand.hasPoint[a], hand.hasPoint[b] else { continue }
|
||||
let pa = mapPoint(hand.points[a], size: size)
|
||||
let pb = mapPoint(hand.points[b], size: size)
|
||||
var path = Path()
|
||||
path.move(to: pa)
|
||||
path.addLine(to: pb)
|
||||
ctx.stroke(path, with: stroke, lineWidth: 1.8)
|
||||
}
|
||||
for i in 0..<21 where hand.hasPoint[i] {
|
||||
let p = mapPoint(hand.points[i], size: size)
|
||||
let r = CGRect(x: p.x - 1.8, y: p.y - 1.8,
|
||||
width: 3.6, height: 3.6)
|
||||
ctx.fill(Path(ellipseIn: r), with: dot)
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Helpers
|
||||
|
||||
private func drawPolyline(_ pts: [SIMD2<Float>],
|
||||
indices: [Int],
|
||||
closed: Bool,
|
||||
in ctx: inout GraphicsContext,
|
||||
size: CGSize,
|
||||
shading: GraphicsContext.Shading,
|
||||
width: CGFloat) {
|
||||
guard indices.count >= 2 else { return }
|
||||
var path = Path()
|
||||
var started = false
|
||||
for i in indices {
|
||||
let p = mapPoint(pts[i], size: size)
|
||||
if !started {
|
||||
path.move(to: p)
|
||||
started = true
|
||||
} else {
|
||||
path.addLine(to: p)
|
||||
}
|
||||
}
|
||||
if closed, let first = indices.first {
|
||||
path.addLine(to: mapPoint(pts[first], size: size))
|
||||
}
|
||||
ctx.stroke(path, with: shading, lineWidth: width)
|
||||
}
|
||||
|
||||
private func mapPoint(_ p: SIMD2<Float>, size: CGSize) -> CGPoint {
|
||||
// Normalised coords come from MediaPipe in image space already.
|
||||
CGPoint(x: CGFloat(p.x) * size.width,
|
||||
y: CGFloat(p.y) * size.height)
|
||||
}
|
||||
}
|
||||
@@ -23,8 +23,42 @@ final class PoseOSCListener: ObservableObject {
|
||||
var seenAt: TimeInterval = 0
|
||||
}
|
||||
|
||||
/// 68-point dlib-style facial landmarks (x,y normalises 0..1).
|
||||
/// Slot mapping comes from FACE_68_FROM_MP cote Python.
|
||||
struct FaceFrame: Equatable {
|
||||
var points: [SIMD2<Float>] = Array(repeating: .zero, count: 68)
|
||||
var hasPoint: [Bool] = Array(repeating: false, count: 68)
|
||||
var seenAt: TimeInterval = 0
|
||||
}
|
||||
|
||||
/// 21 MediaPipe hand landmarks per detected hand.
|
||||
struct HandFrame: Equatable {
|
||||
var side: Int = 0 // 0 = left, 1 = right
|
||||
var points: [SIMD2<Float>] = Array(repeating: .zero, count: 21)
|
||||
var hasPoint: [Bool] = Array(repeating: false, count: 21)
|
||||
var seenAt: TimeInterval = 0
|
||||
}
|
||||
|
||||
/// MediaPipe pose_world_landmarks : 33 keypoints in meters, relative
|
||||
/// to the hip-center. Conventions on the wire (MediaPipe):
|
||||
/// x = right, y = down, z = forward (away from camera).
|
||||
struct Pose3DFrame: Equatable {
|
||||
var pid: Int = -1
|
||||
// SIMD4 = (x, y, z, confidence). All zeros = slot not yet filled.
|
||||
var kps: [SIMD4<Float>] = Array(repeating: .zero, count: 33)
|
||||
var hasPoint: [Bool] = Array(repeating: false, count: 33)
|
||||
var seenAt: TimeInterval = 0
|
||||
}
|
||||
|
||||
@Published var persons: [Int: PoseFrame] = [:]
|
||||
@Published var faces: [Int: FaceFrame] = [:]
|
||||
@Published var hands: [Int: HandFrame] = [:]
|
||||
@Published var body3d: [Int: Pose3DFrame] = [:]
|
||||
@Published var count: Int = 0
|
||||
@Published var faceCount: Int = 0
|
||||
@Published var body3dCount: Int = 0
|
||||
@Published var handCountLeft: Int = 0
|
||||
@Published var handCountRight: Int = 0
|
||||
|
||||
private var listener: NWListener?
|
||||
|
||||
@@ -127,6 +161,69 @@ final class PoseOSCListener: ObservableObject {
|
||||
var p = persons[Int(pid)] ?? PoseFrame()
|
||||
p.bodyPitch = v
|
||||
persons[Int(pid)] = p
|
||||
case "/face/count":
|
||||
if let n = args.first as? Int32 { faceCount = Int(n) }
|
||||
if faceCount == 0 { faces.removeAll(keepingCapacity: true) }
|
||||
case "/face/kp":
|
||||
guard args.count >= 6,
|
||||
let pid = args[0] as? Int32,
|
||||
let slot = args[1] as? Int32,
|
||||
let x = args[2] as? Float,
|
||||
let y = args[3] as? Float else { return }
|
||||
let slotI = Int(slot)
|
||||
guard slotI >= 0 && slotI < 68 else { return }
|
||||
var f = faces[Int(pid)] ?? FaceFrame()
|
||||
f.points[slotI] = SIMD2(x, y)
|
||||
f.hasPoint[slotI] = true
|
||||
f.seenAt = CFAbsoluteTimeGetCurrent()
|
||||
faces[Int(pid)] = f
|
||||
case "/hand/count":
|
||||
if args.count >= 2,
|
||||
let l = args[0] as? Int32, let r = args[1] as? Int32 {
|
||||
handCountLeft = Int(l)
|
||||
handCountRight = Int(r)
|
||||
if handCountLeft + handCountRight == 0 {
|
||||
hands.removeAll(keepingCapacity: true)
|
||||
}
|
||||
}
|
||||
case "/pose3d/count":
|
||||
if let n = args.first as? Int32 {
|
||||
body3dCount = Int(n)
|
||||
if body3dCount == 0 {
|
||||
body3d.removeAll(keepingCapacity: true)
|
||||
}
|
||||
}
|
||||
case "/pose3d/kp":
|
||||
guard args.count >= 6,
|
||||
let pid = args[0] as? Int32,
|
||||
let idx = args[1] as? Int32,
|
||||
let x = args[2] as? Float,
|
||||
let y = args[3] as? Float,
|
||||
let z = args[4] as? Float,
|
||||
let c = args[5] as? Float else { return }
|
||||
let i = Int(idx)
|
||||
guard i >= 0 && i < 33 else { return }
|
||||
var p = body3d[Int(pid)] ?? Pose3DFrame(pid: Int(pid))
|
||||
p.pid = Int(pid)
|
||||
p.kps[i] = SIMD4<Float>(x, y, z, c)
|
||||
p.hasPoint[i] = true
|
||||
p.seenAt = CFAbsoluteTimeGetCurrent()
|
||||
body3d[Int(pid)] = p
|
||||
case "/hand/kp":
|
||||
guard args.count >= 7,
|
||||
let pid = args[0] as? Int32,
|
||||
let side = args[1] as? Int32,
|
||||
let idx = args[2] as? Int32,
|
||||
let x = args[3] as? Float,
|
||||
let y = args[4] as? Float else { return }
|
||||
let i = Int(idx)
|
||||
guard i >= 0 && i < 21 else { return }
|
||||
var h = hands[Int(pid)] ?? HandFrame()
|
||||
h.side = Int(side)
|
||||
h.points[i] = SIMD2(x, y)
|
||||
h.hasPoint[i] = true
|
||||
h.seenAt = CFAbsoluteTimeGetCurrent()
|
||||
hands[Int(pid)] = h
|
||||
default:
|
||||
break
|
||||
}
|
||||
@@ -134,6 +231,12 @@ final class PoseOSCListener: ObservableObject {
|
||||
let now = CFAbsoluteTimeGetCurrent()
|
||||
persons = persons.filter { $0.value.seenAt == 0
|
||||
|| now - $0.value.seenAt < 2.0 }
|
||||
faces = faces.filter { $0.value.seenAt == 0
|
||||
|| now - $0.value.seenAt < 2.0 }
|
||||
hands = hands.filter { $0.value.seenAt == 0
|
||||
|| now - $0.value.seenAt < 2.0 }
|
||||
body3d = body3d.filter { $0.value.seenAt == 0
|
||||
|| now - $0.value.seenAt < 2.0 }
|
||||
}
|
||||
|
||||
// MARK: - Minimal OSC parser
|
||||
|
||||
@@ -0,0 +1,222 @@
|
||||
import Combine
|
||||
import Foundation
|
||||
import RealityKit
|
||||
import SwiftUI
|
||||
import simd
|
||||
|
||||
/// RealityKit renderer for MediaPipe Pose 3D world landmarks (33 joints,
|
||||
/// metric coords relative to the hip-center). Consumes the `body3d`
|
||||
/// publisher of `PoseOSCListener` and maintains one entity tree per
|
||||
/// detected person.
|
||||
///
|
||||
/// Coordinate mapping (MediaPipe -> RealityKit):
|
||||
/// MediaPipe : x = right, y = down, z = forward (away from cam).
|
||||
/// RealityKit: x = right, y = up, z = backward (toward cam).
|
||||
/// => convert with (x, -y, -z).
|
||||
@MainActor
|
||||
final class Skeleton3DRenderer: ObservableObject {
|
||||
/// 32 bones connecting MediaPipe Pose 33 landmarks. Indices are
|
||||
/// the canonical MediaPipe Pose landmark indices. Source: official
|
||||
/// `mp.solutions.pose.POSE_CONNECTIONS` (Holistic / Pose Landmarker
|
||||
/// share the same 33-pt schema).
|
||||
static let POSE_CONNECTIONS: [(Int, Int, BoneChain)] = [
|
||||
// Face (kept light: nose <-> inner eyes <-> outer eyes <-> ears)
|
||||
(0, 1, .face), (1, 2, .face), (2, 3, .face), (3, 7, .face),
|
||||
(0, 4, .face), (4, 5, .face), (5, 6, .face), (6, 8, .face),
|
||||
(9, 10, .face),
|
||||
// Torso
|
||||
(11, 12, .trunk), (11, 23, .trunk), (12, 24, .trunk),
|
||||
(23, 24, .trunk),
|
||||
// Left arm
|
||||
(11, 13, .arm), (13, 15, .arm),
|
||||
(15, 17, .arm), (15, 19, .arm), (15, 21, .arm), (17, 19, .arm),
|
||||
// Right arm
|
||||
(12, 14, .arm), (14, 16, .arm),
|
||||
(16, 18, .arm), (16, 20, .arm), (16, 22, .arm), (18, 20, .arm),
|
||||
// Left leg
|
||||
(23, 25, .leg), (25, 27, .leg),
|
||||
(27, 29, .leg), (27, 31, .leg), (29, 31, .leg),
|
||||
// Right leg
|
||||
(24, 26, .leg), (26, 28, .leg),
|
||||
(28, 30, .leg), (28, 32, .leg), (30, 32, .leg),
|
||||
]
|
||||
|
||||
enum BoneChain {
|
||||
case trunk, arm, leg, face
|
||||
var color: NSColor {
|
||||
switch self {
|
||||
case .trunk: return .white
|
||||
case .arm: return .systemTeal
|
||||
case .leg: return .systemPink // approx magenta
|
||||
case .face: return NSColor(white: 0.7, alpha: 1.0)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static let jointRadius: Float = 0.02 // 2 cm
|
||||
private static let boneRadius: Float = 0.012 // 1.2 cm
|
||||
private static let minConfidence: Float = 0.3
|
||||
private static let retainSec: TimeInterval = 1.0
|
||||
|
||||
/// Update throttle : tick at most every `updatePeriod` seconds even
|
||||
/// if the publisher fires faster (Combine debounce-style on a clock).
|
||||
private static let updatePeriod: TimeInterval = 1.0 / 30.0
|
||||
|
||||
private struct PersonEntities {
|
||||
var root: Entity
|
||||
var joints: [ModelEntity] // 33 spheres
|
||||
var bones: [ModelEntity] // 32 bone entities, same order as POSE_CONNECTIONS
|
||||
}
|
||||
|
||||
private var persons: [Int: PersonEntities] = [:]
|
||||
private var lastSeenAt: [Int: TimeInterval] = [:]
|
||||
private weak var rootAnchor: Entity?
|
||||
private var poseSub: AnyCancellable?
|
||||
private var lastUpdateAt: TimeInterval = 0
|
||||
|
||||
/// Attach to a scene by giving it an AnchorEntity that owns all
|
||||
/// skeleton entities, and start observing the listener.
|
||||
func attach(to anchor: Entity, listener: PoseOSCListener) {
|
||||
rootAnchor = anchor
|
||||
poseSub = listener.$body3d
|
||||
.receive(on: DispatchQueue.main)
|
||||
.sink { [weak self] frames in
|
||||
Task { @MainActor in self?.update(frames: frames) }
|
||||
}
|
||||
}
|
||||
|
||||
func detach() {
|
||||
poseSub?.cancel()
|
||||
poseSub = nil
|
||||
for (_, p) in persons { p.root.removeFromParent() }
|
||||
persons.removeAll()
|
||||
lastSeenAt.removeAll()
|
||||
}
|
||||
|
||||
// MARK: - Update
|
||||
|
||||
private func update(frames: [Int: PoseOSCListener.Pose3DFrame]) {
|
||||
let now = CACurrentMediaTime()
|
||||
if now - lastUpdateAt < Self.updatePeriod { return }
|
||||
lastUpdateAt = now
|
||||
|
||||
guard let anchor = rootAnchor else { return }
|
||||
|
||||
// Mark fresh pids
|
||||
for pid in frames.keys { lastSeenAt[pid] = now }
|
||||
// GC stale persons
|
||||
let cutoff = now - Self.retainSec
|
||||
for (pid, p) in persons where (lastSeenAt[pid] ?? 0) < cutoff {
|
||||
p.root.removeFromParent()
|
||||
persons.removeValue(forKey: pid)
|
||||
lastSeenAt.removeValue(forKey: pid)
|
||||
}
|
||||
|
||||
for (pid, frame) in frames {
|
||||
let entities = persons[pid] ?? makePerson(pid: pid, parent: anchor)
|
||||
persons[pid] = entities
|
||||
apply(frame: frame, to: entities)
|
||||
}
|
||||
}
|
||||
|
||||
private func apply(frame: PoseOSCListener.Pose3DFrame,
|
||||
to entities: PersonEntities) {
|
||||
// Convert all 33 keypoints to RealityKit space once.
|
||||
var rk = [SIMD3<Float>](repeating: .zero, count: 33)
|
||||
var valid = [Bool](repeating: false, count: 33)
|
||||
for i in 0..<33 {
|
||||
let k = frame.kps[i]
|
||||
let visible = frame.hasPoint[i] && k.w >= Self.minConfidence
|
||||
valid[i] = visible
|
||||
// Mediapipe (x right, y down, z forward) -> RK (x right, y up, z back)
|
||||
rk[i] = SIMD3<Float>(k.x, -k.y, -k.z)
|
||||
}
|
||||
|
||||
// Joints: position spheres and toggle visibility.
|
||||
for i in 0..<33 {
|
||||
let joint = entities.joints[i]
|
||||
if valid[i] {
|
||||
joint.transform.translation = rk[i]
|
||||
joint.isEnabled = true
|
||||
} else {
|
||||
joint.isEnabled = false
|
||||
}
|
||||
}
|
||||
|
||||
// Bones: orient + scale length between endpoints.
|
||||
for (bIdx, (a, b, _)) in Self.POSE_CONNECTIONS.enumerated() {
|
||||
let bone = entities.bones[bIdx]
|
||||
if !valid[a] || !valid[b] {
|
||||
bone.isEnabled = false
|
||||
continue
|
||||
}
|
||||
let pa = rk[a]
|
||||
let pb = rk[b]
|
||||
let delta = pb - pa
|
||||
let len = simd_length(delta)
|
||||
if len < 1e-5 {
|
||||
bone.isEnabled = false
|
||||
continue
|
||||
}
|
||||
let mid = (pa + pb) * 0.5
|
||||
// Bone mesh is a cylinder of height=1 along +Y. Rotate +Y
|
||||
// onto the (b-a) direction.
|
||||
let dir = delta / len
|
||||
let yAxis = SIMD3<Float>(0, 1, 0)
|
||||
let dot = simd_dot(yAxis, dir)
|
||||
let rot: simd_quatf
|
||||
if dot > 0.9999 {
|
||||
rot = simd_quatf(angle: 0, axis: SIMD3(0, 1, 0))
|
||||
} else if dot < -0.9999 {
|
||||
rot = simd_quatf(angle: .pi, axis: SIMD3(1, 0, 0))
|
||||
} else {
|
||||
let axis = simd_normalize(simd_cross(yAxis, dir))
|
||||
let angle = acos(dot)
|
||||
rot = simd_quatf(angle: angle, axis: axis)
|
||||
}
|
||||
bone.transform.translation = mid
|
||||
bone.transform.rotation = rot
|
||||
// Scale length only on Y, keep XZ at 1 to preserve radius.
|
||||
bone.transform.scale = SIMD3<Float>(1, len, 1)
|
||||
bone.isEnabled = true
|
||||
}
|
||||
}
|
||||
|
||||
// MARK: - Construction
|
||||
|
||||
private func makePerson(pid: Int, parent: Entity) -> PersonEntities {
|
||||
let root = Entity()
|
||||
parent.addChild(root)
|
||||
|
||||
// Joint sphere mesh shared across joints (cheap to reuse).
|
||||
let sphereMesh = MeshResource.generateSphere(
|
||||
radius: Self.jointRadius)
|
||||
let jointMat = SimpleMaterial(
|
||||
color: .white, roughness: 0.6, isMetallic: false)
|
||||
var joints: [ModelEntity] = []
|
||||
joints.reserveCapacity(33)
|
||||
for _ in 0..<33 {
|
||||
let e = ModelEntity(mesh: sphereMesh, materials: [jointMat])
|
||||
e.isEnabled = false
|
||||
root.addChild(e)
|
||||
joints.append(e)
|
||||
}
|
||||
|
||||
// One cylinder per bone (height=1, scaled at runtime).
|
||||
let cylMesh = MeshResource.generateCylinder(
|
||||
height: 1.0, radius: Self.boneRadius)
|
||||
var bones: [ModelEntity] = []
|
||||
bones.reserveCapacity(Self.POSE_CONNECTIONS.count)
|
||||
for (_, _, chain) in Self.POSE_CONNECTIONS {
|
||||
let mat = SimpleMaterial(
|
||||
color: chain.color, roughness: 0.6, isMetallic: false)
|
||||
let e = ModelEntity(mesh: cylMesh, materials: [mat])
|
||||
e.isEnabled = false
|
||||
root.addChild(e)
|
||||
bones.append(e)
|
||||
}
|
||||
NSLog("Skeleton3DRenderer: spawned pid=%d (33 joints, %d bones)",
|
||||
pid, bones.count)
|
||||
return PersonEntities(root: root, joints: joints, bones: bones)
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user