oxihuman_morph/
body_lean_control.rs

1// Copyright (C) 2026 COOLJAPAN OU (Team KitaSan)
2// SPDX-License-Identifier: Apache-2.0
3#![allow(dead_code)]
4
5//! Body lean morph — controls forward/backward and lateral trunk lean.
6
7use std::f32::consts::FRAC_PI_2;
8
9/// Configuration for body lean control.
10#[allow(dead_code)]
11#[derive(Debug, Clone)]
12pub struct BodyLeanConfig {
13    pub max_forward: f32,
14    pub max_lateral: f32,
15}
16
17/// Runtime state for body lean morph.
18#[allow(dead_code)]
19#[derive(Debug, Clone)]
20pub struct BodyLeanState {
21    pub forward: f32,
22    pub backward: f32,
23    pub lateral_left: f32,
24    pub lateral_right: f32,
25}
26
27#[allow(dead_code)]
28pub fn default_body_lean_config() -> BodyLeanConfig {
29    BodyLeanConfig {
30        max_forward: 1.0,
31        max_lateral: 1.0,
32    }
33}
34
35#[allow(dead_code)]
36pub fn new_body_lean_state() -> BodyLeanState {
37    BodyLeanState {
38        forward: 0.0,
39        backward: 0.0,
40        lateral_left: 0.0,
41        lateral_right: 0.0,
42    }
43}
44
45#[allow(dead_code)]
46pub fn bl_set_forward(state: &mut BodyLeanState, cfg: &BodyLeanConfig, v: f32) {
47    state.forward = v.clamp(0.0, cfg.max_forward);
48    state.backward = 0.0;
49}
50
51#[allow(dead_code)]
52pub fn bl_set_backward(state: &mut BodyLeanState, cfg: &BodyLeanConfig, v: f32) {
53    state.backward = v.clamp(0.0, cfg.max_forward);
54    state.forward = 0.0;
55}
56
57#[allow(dead_code)]
58pub fn bl_set_lateral(state: &mut BodyLeanState, cfg: &BodyLeanConfig, left: f32, right: f32) {
59    state.lateral_left = left.clamp(0.0, cfg.max_lateral);
60    state.lateral_right = right.clamp(0.0, cfg.max_lateral);
61}
62
63#[allow(dead_code)]
64pub fn bl_reset(state: &mut BodyLeanState) {
65    *state = new_body_lean_state();
66}
67
68#[allow(dead_code)]
69pub fn bl_sagittal_angle_rad(state: &BodyLeanState) -> f32 {
70    (state.forward - state.backward) * FRAC_PI_2 * 0.5
71}
72
73#[allow(dead_code)]
74pub fn bl_is_neutral(state: &BodyLeanState) -> bool {
75    let vals = [
76        state.forward,
77        state.backward,
78        state.lateral_left,
79        state.lateral_right,
80    ];
81    !vals.is_empty() && vals.iter().all(|v| v.abs() < 1e-6)
82}
83
84#[allow(dead_code)]
85pub fn bl_blend(a: &BodyLeanState, b: &BodyLeanState, t: f32) -> BodyLeanState {
86    let t = t.clamp(0.0, 1.0);
87    BodyLeanState {
88        forward: a.forward + (b.forward - a.forward) * t,
89        backward: a.backward + (b.backward - a.backward) * t,
90        lateral_left: a.lateral_left + (b.lateral_left - a.lateral_left) * t,
91        lateral_right: a.lateral_right + (b.lateral_right - a.lateral_right) * t,
92    }
93}
94
95#[allow(dead_code)]
96pub fn bl_to_weights(state: &BodyLeanState) -> Vec<(String, f32)> {
97    vec![
98        ("body_lean_forward".to_string(), state.forward),
99        ("body_lean_backward".to_string(), state.backward),
100        ("body_lean_left".to_string(), state.lateral_left),
101        ("body_lean_right".to_string(), state.lateral_right),
102    ]
103}
104
105#[allow(dead_code)]
106pub fn bl_to_json(state: &BodyLeanState) -> String {
107    format!(
108        r#"{{"forward":{:.4},"backward":{:.4},"lateral_left":{:.4},"lateral_right":{:.4}}}"#,
109        state.forward, state.backward, state.lateral_left, state.lateral_right
110    )
111}
112
113#[cfg(test)]
114mod tests {
115    use super::*;
116
117    #[test]
118    fn default_config_sane() {
119        let cfg = default_body_lean_config();
120        assert!((cfg.max_forward - 1.0).abs() < 1e-6);
121        assert!((cfg.max_lateral - 1.0).abs() < 1e-6);
122    }
123
124    #[test]
125    fn new_state_is_neutral() {
126        let s = new_body_lean_state();
127        assert!(bl_is_neutral(&s));
128    }
129
130    #[test]
131    fn set_forward_clamps() {
132        let cfg = default_body_lean_config();
133        let mut s = new_body_lean_state();
134        bl_set_forward(&mut s, &cfg, 5.0);
135        assert!((s.forward - 1.0).abs() < 1e-6);
136        assert_eq!(s.backward, 0.0);
137    }
138
139    #[test]
140    fn set_backward_clears_forward() {
141        let cfg = default_body_lean_config();
142        let mut s = new_body_lean_state();
143        bl_set_forward(&mut s, &cfg, 0.5);
144        bl_set_backward(&mut s, &cfg, 0.3);
145        assert_eq!(s.forward, 0.0);
146        assert!((s.backward - 0.3).abs() < 1e-6);
147    }
148
149    #[test]
150    fn set_lateral() {
151        let cfg = default_body_lean_config();
152        let mut s = new_body_lean_state();
153        bl_set_lateral(&mut s, &cfg, 0.4, 0.6);
154        assert!((s.lateral_left - 0.4).abs() < 1e-6);
155        assert!((s.lateral_right - 0.6).abs() < 1e-6);
156    }
157
158    #[test]
159    fn reset_clears_all() {
160        let cfg = default_body_lean_config();
161        let mut s = new_body_lean_state();
162        bl_set_forward(&mut s, &cfg, 0.5);
163        bl_reset(&mut s);
164        assert!(bl_is_neutral(&s));
165    }
166
167    #[test]
168    fn sagittal_angle_forward() {
169        let cfg = default_body_lean_config();
170        let mut s = new_body_lean_state();
171        bl_set_forward(&mut s, &cfg, 1.0);
172        assert!(bl_sagittal_angle_rad(&s) > 0.0);
173    }
174
175    #[test]
176    fn blend_midpoint() {
177        let a = new_body_lean_state();
178        let cfg = default_body_lean_config();
179        let mut b = new_body_lean_state();
180        bl_set_forward(&mut b, &cfg, 1.0);
181        let mid = bl_blend(&a, &b, 0.5);
182        assert!((mid.forward - 0.5).abs() < 1e-6);
183    }
184
185    #[test]
186    fn to_weights_count() {
187        let s = new_body_lean_state();
188        assert_eq!(bl_to_weights(&s).len(), 4);
189    }
190
191    #[test]
192    fn to_json_contains_fields() {
193        let s = new_body_lean_state();
194        let j = bl_to_json(&s);
195        assert!(j.contains("forward"));
196        assert!(j.contains("lateral_right"));
197    }
198}
oxihuman_morph/body_lean_control.rs

oxihuman_morph/
body_lean_control.rs
