oxihuman_morph/
body_twist_control.rs

1// Copyright (C) 2026 COOLJAPAN OU (Team KitaSan)
2// SPDX-License-Identifier: Apache-2.0
3#![allow(dead_code)]
4
5//! Body twist control — rotational twist of torso around vertical axis.
6
7use std::f32::consts::PI;
8
9/// Configuration for body twist.
10#[allow(dead_code)]
11#[derive(Debug, Clone)]
12pub struct BodyTwistConfig {
13    pub max_angle_rad: f32,
14}
15
16/// Runtime state for body twist.
17#[allow(dead_code)]
18#[derive(Debug, Clone)]
19pub struct BodyTwistState {
20    pub upper_twist_rad: f32,
21    pub lower_twist_rad: f32,
22}
23
24#[allow(dead_code)]
25pub fn default_body_twist_config() -> BodyTwistConfig {
26    BodyTwistConfig { max_angle_rad: PI }
27}
28
29#[allow(dead_code)]
30pub fn new_body_twist_state() -> BodyTwistState {
31    BodyTwistState {
32        upper_twist_rad: 0.0,
33        lower_twist_rad: 0.0,
34    }
35}
36
37#[allow(dead_code)]
38pub fn btwist_set_upper(state: &mut BodyTwistState, cfg: &BodyTwistConfig, v: f32) {
39    state.upper_twist_rad = v.clamp(-cfg.max_angle_rad, cfg.max_angle_rad);
40}
41
42#[allow(dead_code)]
43pub fn btwist_set_lower(state: &mut BodyTwistState, cfg: &BodyTwistConfig, v: f32) {
44    state.lower_twist_rad = v.clamp(-cfg.max_angle_rad, cfg.max_angle_rad);
45}
46
47#[allow(dead_code)]
48pub fn btwist_reset(state: &mut BodyTwistState) {
49    *state = new_body_twist_state();
50}
51
52#[allow(dead_code)]
53pub fn btwist_is_neutral(state: &BodyTwistState) -> bool {
54    state.upper_twist_rad.abs() < 1e-6 && state.lower_twist_rad.abs() < 1e-6
55}
56
57#[allow(dead_code)]
58pub fn btwist_total_angle_rad(state: &BodyTwistState) -> f32 {
59    state.upper_twist_rad - state.lower_twist_rad
60}
61
62#[allow(dead_code)]
63pub fn btwist_blend(a: &BodyTwistState, b: &BodyTwistState, t: f32) -> BodyTwistState {
64    let t = t.clamp(0.0, 1.0);
65    BodyTwistState {
66        upper_twist_rad: a.upper_twist_rad + (b.upper_twist_rad - a.upper_twist_rad) * t,
67        lower_twist_rad: a.lower_twist_rad + (b.lower_twist_rad - a.lower_twist_rad) * t,
68    }
69}
70
71#[allow(dead_code)]
72pub fn btwist_to_weights(state: &BodyTwistState) -> Vec<(String, f32)> {
73    let norm = 1.0 / PI;
74    vec![
75        ("body_twist_upper".to_string(), state.upper_twist_rad * norm),
76        ("body_twist_lower".to_string(), state.lower_twist_rad * norm),
77    ]
78}
79
80#[allow(dead_code)]
81pub fn btwist_to_json(state: &BodyTwistState) -> String {
82    format!(
83        r#"{{"upper_twist_rad":{:.4},"lower_twist_rad":{:.4}}}"#,
84        state.upper_twist_rad, state.lower_twist_rad
85    )
86}
87
88#[cfg(test)]
89mod tests {
90    use super::*;
91
92    #[test]
93    fn default_config_pi() {
94        let cfg = default_body_twist_config();
95        assert!((cfg.max_angle_rad - PI).abs() < 1e-6);
96    }
97
98    #[test]
99    fn new_state_neutral() {
100        let s = new_body_twist_state();
101        assert!(btwist_is_neutral(&s));
102    }
103
104    #[test]
105    fn set_upper_clamps() {
106        let cfg = default_body_twist_config();
107        let mut s = new_body_twist_state();
108        btwist_set_upper(&mut s, &cfg, 10.0);
109        assert!((s.upper_twist_rad - PI).abs() < 1e-6);
110    }
111
112    #[test]
113    fn set_upper_negative_clamps() {
114        let cfg = default_body_twist_config();
115        let mut s = new_body_twist_state();
116        btwist_set_upper(&mut s, &cfg, -10.0);
117        assert!((s.upper_twist_rad + PI).abs() < 1e-6);
118    }
119
120    #[test]
121    fn set_lower() {
122        let cfg = default_body_twist_config();
123        let mut s = new_body_twist_state();
124        btwist_set_lower(&mut s, &cfg, 0.5);
125        assert!((s.lower_twist_rad - 0.5).abs() < 1e-6);
126    }
127
128    #[test]
129    fn total_angle() {
130        let cfg = default_body_twist_config();
131        let mut s = new_body_twist_state();
132        btwist_set_upper(&mut s, &cfg, 1.0);
133        btwist_set_lower(&mut s, &cfg, -0.5);
134        assert!((btwist_total_angle_rad(&s) - 1.5).abs() < 1e-6);
135    }
136
137    #[test]
138    fn reset_clears() {
139        let cfg = default_body_twist_config();
140        let mut s = new_body_twist_state();
141        btwist_set_upper(&mut s, &cfg, 0.8);
142        btwist_reset(&mut s);
143        assert!(btwist_is_neutral(&s));
144    }
145
146    #[test]
147    fn blend_midpoint() {
148        let a = new_body_twist_state();
149        let cfg = default_body_twist_config();
150        let mut b = new_body_twist_state();
151        btwist_set_upper(&mut b, &cfg, 1.0);
152        let m = btwist_blend(&a, &b, 0.5);
153        assert!((m.upper_twist_rad - 0.5).abs() < 1e-6);
154    }
155
156    #[test]
157    fn to_weights_len() {
158        let s = new_body_twist_state();
159        assert_eq!(btwist_to_weights(&s).len(), 2);
160    }
161
162    #[test]
163    fn to_json_contains_fields() {
164        let s = new_body_twist_state();
165        let j = btwist_to_json(&s);
166        assert!(j.contains("upper_twist_rad"));
167        assert!(j.contains("lower_twist_rad"));
168    }
169}
oxihuman_morph/body_twist_control.rs

oxihuman_morph/
body_twist_control.rs
