oxihuman_morph/
neck_tilt_control.rs

1// Copyright (C) 2026 COOLJAPAN OU (Team KitaSan)
2// SPDX-License-Identifier: Apache-2.0
3#![allow(dead_code)]
4
5//! Neck tilt control — lateral and sagittal tilt of the neck column.
6
7use std::f32::consts::FRAC_PI_4;
8
9/// Configuration for neck tilt.
10#[allow(dead_code)]
11#[derive(Debug, Clone)]
12pub struct NeckTiltConfig {
13    pub max_tilt_rad: f32,
14}
15
16/// Runtime state.
17#[allow(dead_code)]
18#[derive(Debug, Clone)]
19pub struct NeckTiltState {
20    pub lateral_tilt_rad: f32,
21    pub sagittal_tilt_rad: f32,
22}
23
24#[allow(dead_code)]
25pub fn default_neck_tilt_config() -> NeckTiltConfig {
26    NeckTiltConfig {
27        max_tilt_rad: FRAC_PI_4,
28    }
29}
30
31#[allow(dead_code)]
32pub fn new_neck_tilt_state() -> NeckTiltState {
33    NeckTiltState {
34        lateral_tilt_rad: 0.0,
35        sagittal_tilt_rad: 0.0,
36    }
37}
38
39#[allow(dead_code)]
40pub fn ntilt_set_lateral(state: &mut NeckTiltState, cfg: &NeckTiltConfig, v: f32) {
41    state.lateral_tilt_rad = v.clamp(-cfg.max_tilt_rad, cfg.max_tilt_rad);
42}
43
44#[allow(dead_code)]
45pub fn ntilt_set_sagittal(state: &mut NeckTiltState, cfg: &NeckTiltConfig, v: f32) {
46    state.sagittal_tilt_rad = v.clamp(-cfg.max_tilt_rad, cfg.max_tilt_rad);
47}
48
49#[allow(dead_code)]
50pub fn ntilt_reset(state: &mut NeckTiltState) {
51    *state = new_neck_tilt_state();
52}
53
54#[allow(dead_code)]
55pub fn ntilt_is_neutral(state: &NeckTiltState) -> bool {
56    state.lateral_tilt_rad.abs() < 1e-6 && state.sagittal_tilt_rad.abs() < 1e-6
57}
58
59#[allow(dead_code)]
60pub fn ntilt_total_angle_rad(state: &NeckTiltState) -> f32 {
61    (state.lateral_tilt_rad * state.lateral_tilt_rad
62        + state.sagittal_tilt_rad * state.sagittal_tilt_rad)
63        .sqrt()
64}
65
66#[allow(dead_code)]
67pub fn ntilt_blend(a: &NeckTiltState, b: &NeckTiltState, t: f32) -> NeckTiltState {
68    let t = t.clamp(0.0, 1.0);
69    NeckTiltState {
70        lateral_tilt_rad: a.lateral_tilt_rad + (b.lateral_tilt_rad - a.lateral_tilt_rad) * t,
71        sagittal_tilt_rad: a.sagittal_tilt_rad + (b.sagittal_tilt_rad - a.sagittal_tilt_rad) * t,
72    }
73}
74
75#[allow(dead_code)]
76pub fn ntilt_to_weights(state: &NeckTiltState) -> Vec<(String, f32)> {
77    let norm = 1.0 / FRAC_PI_4;
78    vec![
79        (
80            "neck_tilt_lateral".to_string(),
81            state.lateral_tilt_rad * norm,
82        ),
83        (
84            "neck_tilt_sagittal".to_string(),
85            state.sagittal_tilt_rad * norm,
86        ),
87    ]
88}
89
90#[allow(dead_code)]
91pub fn ntilt_to_json(state: &NeckTiltState) -> String {
92    format!(
93        r#"{{"lateral_tilt_rad":{:.4},"sagittal_tilt_rad":{:.4}}}"#,
94        state.lateral_tilt_rad, state.sagittal_tilt_rad
95    )
96}
97
98#[cfg(test)]
99mod tests {
100    use super::*;
101
102    #[test]
103    fn default_config() {
104        let cfg = default_neck_tilt_config();
105        assert!((cfg.max_tilt_rad - FRAC_PI_4).abs() < 1e-6);
106    }
107
108    #[test]
109    fn new_state_neutral() {
110        let s = new_neck_tilt_state();
111        assert!(ntilt_is_neutral(&s));
112    }
113
114    #[test]
115    fn set_lateral_clamps() {
116        let cfg = default_neck_tilt_config();
117        let mut s = new_neck_tilt_state();
118        ntilt_set_lateral(&mut s, &cfg, 10.0);
119        assert!((s.lateral_tilt_rad - FRAC_PI_4).abs() < 1e-6);
120    }
121
122    #[test]
123    fn set_sagittal() {
124        let cfg = default_neck_tilt_config();
125        let mut s = new_neck_tilt_state();
126        ntilt_set_sagittal(&mut s, &cfg, 0.3);
127        assert!((s.sagittal_tilt_rad - 0.3).abs() < 1e-6);
128    }
129
130    #[test]
131    fn set_lateral_negative() {
132        let cfg = default_neck_tilt_config();
133        let mut s = new_neck_tilt_state();
134        ntilt_set_lateral(&mut s, &cfg, -FRAC_PI_4);
135        assert!((s.lateral_tilt_rad + FRAC_PI_4).abs() < 1e-6);
136    }
137
138    #[test]
139    fn total_angle_pythagoras() {
140        let cfg = default_neck_tilt_config();
141        let mut s = new_neck_tilt_state();
142        ntilt_set_lateral(&mut s, &cfg, 0.3);
143        ntilt_set_sagittal(&mut s, &cfg, 0.4);
144        assert!((ntilt_total_angle_rad(&s) - 0.5).abs() < 1e-5);
145    }
146
147    #[test]
148    fn reset_clears() {
149        let cfg = default_neck_tilt_config();
150        let mut s = new_neck_tilt_state();
151        ntilt_set_lateral(&mut s, &cfg, 0.5);
152        ntilt_reset(&mut s);
153        assert!(ntilt_is_neutral(&s));
154    }
155
156    #[test]
157    fn blend_midpoint() {
158        let a = new_neck_tilt_state();
159        let cfg = default_neck_tilt_config();
160        let mut b = new_neck_tilt_state();
161        ntilt_set_lateral(&mut b, &cfg, 0.6);
162        let m = ntilt_blend(&a, &b, 0.5);
163        assert!((m.lateral_tilt_rad - 0.3).abs() < 1e-6);
164    }
165
166    #[test]
167    fn to_weights_count() {
168        let s = new_neck_tilt_state();
169        assert_eq!(ntilt_to_weights(&s).len(), 2);
170    }
171
172    #[test]
173    fn to_json_fields() {
174        let s = new_neck_tilt_state();
175        let j = ntilt_to_json(&s);
176        assert!(j.contains("lateral_tilt_rad"));
177    }
178}
oxihuman_morph/neck_tilt_control.rs

oxihuman_morph/
neck_tilt_control.rs
