elara_visual/
pose.rs

1//! Pose State - Body pose and gestures as state
2//!
3//! This is NOT motion capture data or skeleton tracking.
4//! This is the STATE of body pose for reality synchronization.
5
6use elara_core::StateTime;
7
8/// Joint identifier for body skeleton
9#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
10pub enum Joint {
11    // Head
12    Head,
13    Neck,
14
15    // Torso
16    Spine,
17    Chest,
18    Hips,
19
20    // Left arm
21    LeftShoulder,
22    LeftElbow,
23    LeftWrist,
24    LeftHand,
25
26    // Right arm
27    RightShoulder,
28    RightElbow,
29    RightWrist,
30    RightHand,
31
32    // Left leg
33    LeftHip,
34    LeftKnee,
35    LeftAnkle,
36    LeftFoot,
37
38    // Right leg
39    RightHip,
40    RightKnee,
41    RightAnkle,
42    RightFoot,
43}
44
45impl Joint {
46    /// All joints in order
47    pub fn all() -> &'static [Joint] {
48        &[
49            Joint::Head,
50            Joint::Neck,
51            Joint::Spine,
52            Joint::Chest,
53            Joint::Hips,
54            Joint::LeftShoulder,
55            Joint::LeftElbow,
56            Joint::LeftWrist,
57            Joint::LeftHand,
58            Joint::RightShoulder,
59            Joint::RightElbow,
60            Joint::RightWrist,
61            Joint::RightHand,
62            Joint::LeftHip,
63            Joint::LeftKnee,
64            Joint::LeftAnkle,
65            Joint::LeftFoot,
66            Joint::RightHip,
67            Joint::RightKnee,
68            Joint::RightAnkle,
69            Joint::RightFoot,
70        ]
71    }
72
73    /// Number of joints
74    pub fn count() -> usize {
75        21
76    }
77}
78
79/// 3D position (normalized coordinates)
80#[derive(Debug, Clone, Copy, Default)]
81pub struct Position3D {
82    pub x: f32,
83    pub y: f32,
84    pub z: f32,
85}
86
87impl Position3D {
88    pub fn new(x: f32, y: f32, z: f32) -> Self {
89        Self { x, y, z }
90    }
91
92    pub fn zero() -> Self {
93        Self::default()
94    }
95
96    /// Linear interpolation
97    pub fn lerp(&self, other: &Position3D, t: f32) -> Position3D {
98        Position3D {
99            x: self.x + (other.x - self.x) * t,
100            y: self.y + (other.y - self.y) * t,
101            z: self.z + (other.z - self.z) * t,
102        }
103    }
104
105    /// Distance to another position
106    pub fn distance(&self, other: &Position3D) -> f32 {
107        let dx = self.x - other.x;
108        let dy = self.y - other.y;
109        let dz = self.z - other.z;
110        (dx * dx + dy * dy + dz * dz).sqrt()
111    }
112}
113
114/// Joint rotation (quaternion representation)
115#[derive(Debug, Clone, Copy)]
116pub struct Rotation3D {
117    pub w: f32,
118    pub x: f32,
119    pub y: f32,
120    pub z: f32,
121}
122
123impl Default for Rotation3D {
124    fn default() -> Self {
125        Self::identity()
126    }
127}
128
129impl Rotation3D {
130    pub fn identity() -> Self {
131        Self {
132            w: 1.0,
133            x: 0.0,
134            y: 0.0,
135            z: 0.0,
136        }
137    }
138
139    pub fn from_euler(yaw: f32, pitch: f32, roll: f32) -> Self {
140        let cy = (yaw * 0.5).cos();
141        let sy = (yaw * 0.5).sin();
142        let cp = (pitch * 0.5).cos();
143        let sp = (pitch * 0.5).sin();
144        let cr = (roll * 0.5).cos();
145        let sr = (roll * 0.5).sin();
146
147        Self {
148            w: cr * cp * cy + sr * sp * sy,
149            x: sr * cp * cy - cr * sp * sy,
150            y: cr * sp * cy + sr * cp * sy,
151            z: cr * cp * sy - sr * sp * cy,
152        }
153    }
154
155    /// Spherical linear interpolation
156    pub fn slerp(&self, other: &Rotation3D, t: f32) -> Rotation3D {
157        let mut dot = self.w * other.w + self.x * other.x + self.y * other.y + self.z * other.z;
158
159        let other = if dot < 0.0 {
160            dot = -dot;
161            Rotation3D {
162                w: -other.w,
163                x: -other.x,
164                y: -other.y,
165                z: -other.z,
166            }
167        } else {
168            *other
169        };
170
171        if dot > 0.9995 {
172            // Linear interpolation for very close quaternions
173            let result = Rotation3D {
174                w: self.w + (other.w - self.w) * t,
175                x: self.x + (other.x - self.x) * t,
176                y: self.y + (other.y - self.y) * t,
177                z: self.z + (other.z - self.z) * t,
178            };
179            return result.normalize();
180        }
181
182        let theta_0 = dot.acos();
183        let theta = theta_0 * t;
184        let sin_theta = theta.sin();
185        let sin_theta_0 = theta_0.sin();
186
187        let s0 = (theta_0 - theta).cos() - dot * sin_theta / sin_theta_0;
188        let s1 = sin_theta / sin_theta_0;
189
190        Rotation3D {
191            w: self.w * s0 + other.w * s1,
192            x: self.x * s0 + other.x * s1,
193            y: self.y * s0 + other.y * s1,
194            z: self.z * s0 + other.z * s1,
195        }
196    }
197
198    fn normalize(&self) -> Rotation3D {
199        let len = (self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z).sqrt();
200        if len < 0.0001 {
201            return Rotation3D::identity();
202        }
203        Rotation3D {
204            w: self.w / len,
205            x: self.x / len,
206            y: self.y / len,
207            z: self.z / len,
208        }
209    }
210}
211
212/// Joint state (position + rotation + confidence)
213#[derive(Debug, Clone, Copy, Default)]
214pub struct JointState {
215    pub position: Position3D,
216    pub rotation: Rotation3D,
217    pub confidence: f32,
218}
219
220impl JointState {
221    pub fn new(position: Position3D, rotation: Rotation3D, confidence: f32) -> Self {
222        Self {
223            position,
224            rotation,
225            confidence,
226        }
227    }
228
229    pub fn lerp(&self, other: &JointState, t: f32) -> JointState {
230        JointState {
231            position: self.position.lerp(&other.position, t),
232            rotation: self.rotation.slerp(&other.rotation, t),
233            confidence: self.confidence + (other.confidence - self.confidence) * t,
234        }
235    }
236}
237
238/// Gesture recognition result
239#[derive(Debug, Clone, Copy, PartialEq, Eq)]
240pub enum Gesture {
241    None,
242    Wave,
243    ThumbsUp,
244    ThumbsDown,
245    Peace,
246    Pointing,
247    OpenPalm,
248    Fist,
249    Clapping,
250    Shrugging,
251    Nodding,
252    HeadShake,
253}
254
255/// Activity state
256#[derive(Debug, Clone, Copy, PartialEq, Eq)]
257pub enum ActivityState {
258    Unknown,
259    Sitting,
260    Standing,
261    Walking,
262    Running,
263    Jumping,
264    Lying,
265}
266
267/// Complete pose state
268#[derive(Debug, Clone)]
269pub struct PoseState {
270    /// Timestamp
271    pub timestamp: StateTime,
272
273    /// Is a body detected?
274    pub present: bool,
275
276    /// Joint states (indexed by Joint enum)
277    pub joints: Vec<JointState>,
278
279    /// Current gesture (if any)
280    pub gesture: Gesture,
281
282    /// Current activity
283    pub activity: ActivityState,
284
285    /// Overall confidence
286    pub confidence: f32,
287
288    /// Velocity estimate (for prediction)
289    pub velocity: Position3D,
290}
291
292impl PoseState {
293    /// Create a new pose state
294    pub fn new(timestamp: StateTime) -> Self {
295        Self {
296            timestamp,
297            present: true,
298            joints: vec![JointState::default(); Joint::count()],
299            gesture: Gesture::None,
300            activity: ActivityState::Unknown,
301            confidence: 1.0,
302            velocity: Position3D::zero(),
303        }
304    }
305
306    /// No body present
307    pub fn absent(timestamp: StateTime) -> Self {
308        Self {
309            timestamp,
310            present: false,
311            joints: Vec::new(),
312            gesture: Gesture::None,
313            activity: ActivityState::Unknown,
314            confidence: 0.0,
315            velocity: Position3D::zero(),
316        }
317    }
318
319    /// Get joint state by joint type
320    pub fn joint(&self, joint: Joint) -> Option<&JointState> {
321        self.joints.get(joint as usize)
322    }
323
324    /// Set joint state
325    pub fn set_joint(&mut self, joint: Joint, state: JointState) {
326        let idx = joint as usize;
327        if idx < self.joints.len() {
328            self.joints[idx] = state;
329        }
330    }
331
332    /// Interpolate between two pose states
333    pub fn lerp(&self, other: &PoseState, t: f32) -> PoseState {
334        let t = t.clamp(0.0, 1.0);
335
336        let joints = if self.joints.len() == other.joints.len() {
337            self.joints
338                .iter()
339                .zip(other.joints.iter())
340                .map(|(a, b)| a.lerp(b, t))
341                .collect()
342        } else if t < 0.5 {
343            self.joints.clone()
344        } else {
345            other.joints.clone()
346        };
347
348        PoseState {
349            timestamp: other.timestamp,
350            present: if t < 0.5 { self.present } else { other.present },
351            joints,
352            gesture: if t < 0.5 { self.gesture } else { other.gesture },
353            activity: if t < 0.5 {
354                self.activity
355            } else {
356                other.activity
357            },
358            confidence: self.confidence + (other.confidence - self.confidence) * t,
359            velocity: self.velocity.lerp(&other.velocity, t),
360        }
361    }
362
363    /// Predict future pose based on velocity
364    pub fn predict(&self, delta_ms: i64) -> PoseState {
365        let dt = delta_ms as f32 / 1000.0;
366        let mut predicted = self.clone();
367
368        predicted.timestamp = StateTime::from_millis(self.timestamp.as_millis() + delta_ms);
369
370        // Apply velocity to all joints
371        for joint in &mut predicted.joints {
372            joint.position.x += self.velocity.x * dt;
373            joint.position.y += self.velocity.y * dt;
374            joint.position.z += self.velocity.z * dt;
375        }
376
377        // Reduce confidence for predictions
378        predicted.confidence *= 0.95;
379
380        predicted
381    }
382}
383
384#[cfg(test)]
385mod tests {
386    use super::*;
387
388    #[test]
389    fn test_position_lerp() {
390        let a = Position3D::new(0.0, 0.0, 0.0);
391        let b = Position3D::new(10.0, 10.0, 10.0);
392
393        let mid = a.lerp(&b, 0.5);
394        assert!((mid.x - 5.0).abs() < 0.01);
395        assert!((mid.y - 5.0).abs() < 0.01);
396        assert!((mid.z - 5.0).abs() < 0.01);
397    }
398
399    #[test]
400    fn test_pose_state() {
401        let time = StateTime::from_millis(0);
402        let pose = PoseState::new(time);
403
404        assert!(pose.present);
405        assert_eq!(pose.joints.len(), Joint::count());
406    }
407
408    #[test]
409    fn test_pose_prediction() {
410        let time = StateTime::from_millis(0);
411        let mut pose = PoseState::new(time);
412        pose.velocity = Position3D::new(1.0, 0.0, 0.0);
413
414        let predicted = pose.predict(1000); // 1 second
415
416        // Joints should have moved by velocity * time
417        assert!(predicted.timestamp.as_millis() == 1000);
418    }
419}
elara_visual/pose.rs

elara_visual/
pose.rs
