oxihuman_morph/
shoulder_roll_control.rs

1// Copyright (C) 2026 COOLJAPAN OU (Team KitaSan) / SPDX-License-Identifier: Apache-2.0
2#![allow(dead_code)]
3
4//! Shoulder roll (internal/external rotation) morph control.
5
6use std::f32::consts::FRAC_PI_4;
7
8/// Shoulder side.
9#[allow(dead_code)]
10#[derive(Debug, Clone, Copy, PartialEq, Eq)]
11pub enum RollSide {
12    Left,
13    Right,
14}
15
16/// Config.
17#[allow(dead_code)]
18#[derive(Debug, Clone, PartialEq)]
19pub struct ShoulderRollConfig {
20    pub max_roll_rad: f32,
21}
22
23impl Default for ShoulderRollConfig {
24    fn default() -> Self {
25        Self {
26            max_roll_rad: FRAC_PI_4,
27        }
28    }
29}
30
31/// State — roll per side, −1..=1 (negative = internal, positive = external).
32#[allow(dead_code)]
33#[derive(Debug, Clone, Default)]
34pub struct ShoulderRollState {
35    pub left: f32,
36    pub right: f32,
37}
38
39#[allow(dead_code)]
40pub fn new_shoulder_roll_state() -> ShoulderRollState {
41    ShoulderRollState::default()
42}
43
44#[allow(dead_code)]
45pub fn default_shoulder_roll_config() -> ShoulderRollConfig {
46    ShoulderRollConfig::default()
47}
48
49#[allow(dead_code)]
50pub fn shr_set(state: &mut ShoulderRollState, side: RollSide, v: f32) {
51    let v = v.clamp(-1.0, 1.0);
52    match side {
53        RollSide::Left => state.left = v,
54        RollSide::Right => state.right = v,
55    }
56}
57
58#[allow(dead_code)]
59pub fn shr_set_both(state: &mut ShoulderRollState, v: f32) {
60    let v = v.clamp(-1.0, 1.0);
61    state.left = v;
62    state.right = v;
63}
64
65#[allow(dead_code)]
66pub fn shr_reset(state: &mut ShoulderRollState) {
67    *state = ShoulderRollState::default();
68}
69
70#[allow(dead_code)]
71pub fn shr_is_neutral(state: &ShoulderRollState) -> bool {
72    state.left.abs() < 1e-4 && state.right.abs() < 1e-4
73}
74
75#[allow(dead_code)]
76pub fn shr_asymmetry(state: &ShoulderRollState) -> f32 {
77    (state.left - state.right).abs()
78}
79
80/// Roll angle in radians for a side.
81#[allow(dead_code)]
82pub fn shr_angle_rad(state: &ShoulderRollState, side: RollSide, cfg: &ShoulderRollConfig) -> f32 {
83    let v = match side {
84        RollSide::Left => state.left,
85        RollSide::Right => state.right,
86    };
87    v * cfg.max_roll_rad
88}
89
90#[allow(dead_code)]
91pub fn shr_to_weights(state: &ShoulderRollState, cfg: &ShoulderRollConfig) -> [f32; 2] {
92    [
93        state.left * cfg.max_roll_rad,
94        state.right * cfg.max_roll_rad,
95    ]
96}
97
98#[allow(dead_code)]
99pub fn shr_blend(a: &ShoulderRollState, b: &ShoulderRollState, t: f32) -> ShoulderRollState {
100    let t = t.clamp(0.0, 1.0);
101    let inv = 1.0 - t;
102    ShoulderRollState {
103        left: a.left * inv + b.left * t,
104        right: a.right * inv + b.right * t,
105    }
106}
107
108#[allow(dead_code)]
109pub fn shr_to_json(state: &ShoulderRollState) -> String {
110    format!(
111        "{{\"left\":{:.4},\"right\":{:.4}}}",
112        state.left, state.right
113    )
114}
115
116#[cfg(test)]
117mod tests {
118    use super::*;
119
120    #[test]
121    fn default_neutral() {
122        assert!(shr_is_neutral(&new_shoulder_roll_state()));
123    }
124
125    #[test]
126    fn set_clamps_high() {
127        let mut s = new_shoulder_roll_state();
128        shr_set(&mut s, RollSide::Left, 5.0);
129        assert!((s.left - 1.0).abs() < 1e-6);
130    }
131
132    #[test]
133    fn set_clamps_low() {
134        let mut s = new_shoulder_roll_state();
135        shr_set(&mut s, RollSide::Right, -5.0);
136        assert!((s.right + 1.0).abs() < 1e-6);
137    }
138
139    #[test]
140    fn reset_clears() {
141        let mut s = new_shoulder_roll_state();
142        shr_set_both(&mut s, 0.7);
143        shr_reset(&mut s);
144        assert!(shr_is_neutral(&s));
145    }
146
147    #[test]
148    fn asymmetry_zero_equal() {
149        let mut s = new_shoulder_roll_state();
150        shr_set_both(&mut s, 0.5);
151        assert!(shr_asymmetry(&s) < 1e-6);
152    }
153
154    #[test]
155    fn angle_rad_sign() {
156        let cfg = default_shoulder_roll_config();
157        let mut s = new_shoulder_roll_state();
158        shr_set(&mut s, RollSide::Left, 1.0);
159        assert!(shr_angle_rad(&s, RollSide::Left, &cfg) > 0.0);
160        shr_set(&mut s, RollSide::Left, -1.0);
161        assert!(shr_angle_rad(&s, RollSide::Left, &cfg) < 0.0);
162    }
163
164    #[test]
165    fn weights_correct() {
166        let cfg = default_shoulder_roll_config();
167        let mut s = new_shoulder_roll_state();
168        shr_set_both(&mut s, 1.0);
169        let w = shr_to_weights(&s, &cfg);
170        assert!((w[0] - cfg.max_roll_rad).abs() < 1e-6);
171    }
172
173    #[test]
174    fn blend_midpoint() {
175        let mut b = new_shoulder_roll_state();
176        shr_set_both(&mut b, 1.0);
177        let r = shr_blend(&new_shoulder_roll_state(), &b, 0.5);
178        assert!((r.left - 0.5).abs() < 1e-5);
179    }
180
181    #[test]
182    fn json_has_left_right() {
183        let j = shr_to_json(&new_shoulder_roll_state());
184        assert!(j.contains("left") && j.contains("right"));
185    }
186
187    #[test]
188    fn set_both_equal() {
189        let mut s = new_shoulder_roll_state();
190        shr_set_both(&mut s, -0.5);
191        assert!((s.left - s.right).abs() < 1e-6);
192    }
193}
oxihuman_morph/shoulder_roll_control.rs

oxihuman_morph/
shoulder_roll_control.rs
