oxihuman_morph/
toe_control.rs

1// Copyright (C) 2026 COOLJAPAN OU (Team KitaSan)
2// SPDX-License-Identifier: Apache-2.0
3#![allow(dead_code)]
4
5//! Individual toe length and splay control.
6
7use std::f32::consts::FRAC_PI_6;
8
9pub const TOE_COUNT: usize = 5;
10
11#[allow(dead_code)]
12#[derive(Debug, Clone, Copy, PartialEq, Eq)]
13pub enum ToeFootSide {
14    Left,
15    Right,
16}
17
18#[allow(dead_code)]
19#[derive(Debug, Clone)]
20pub struct ToeControlConfig {
21    pub max_length: f32,
22    pub max_splay: f32,
23}
24
25impl Default for ToeControlConfig {
26    fn default() -> Self {
27        Self {
28            max_length: 1.0,
29            max_splay: 1.0,
30        }
31    }
32}
33
34#[allow(dead_code)]
35#[derive(Debug, Clone)]
36pub struct ToeControlState {
37    pub left_len: [f32; TOE_COUNT],
38    pub right_len: [f32; TOE_COUNT],
39    pub splay: f32,
40    pub config: ToeControlConfig,
41}
42
43#[allow(dead_code)]
44pub fn default_toe_control_config() -> ToeControlConfig {
45    ToeControlConfig::default()
46}
47
48#[allow(dead_code)]
49pub fn new_toe_control_state(config: ToeControlConfig) -> ToeControlState {
50    ToeControlState {
51        left_len: [0.0; TOE_COUNT],
52        right_len: [0.0; TOE_COUNT],
53        splay: 0.0,
54        config,
55    }
56}
57
58#[allow(dead_code)]
59pub fn tc_set_length(state: &mut ToeControlState, side: ToeFootSide, toe: usize, v: f32) {
60    if toe < TOE_COUNT {
61        let v = v.clamp(0.0, state.config.max_length);
62        match side {
63            ToeFootSide::Left => state.left_len[toe] = v,
64            ToeFootSide::Right => state.right_len[toe] = v,
65        }
66    }
67}
68
69#[allow(dead_code)]
70pub fn tc_set_splay(state: &mut ToeControlState, v: f32) {
71    state.splay = v.clamp(0.0, state.config.max_splay);
72}
73
74#[allow(dead_code)]
75pub fn tc_reset(state: &mut ToeControlState) {
76    state.left_len = [0.0; TOE_COUNT];
77    state.right_len = [0.0; TOE_COUNT];
78    state.splay = 0.0;
79}
80
81#[allow(dead_code)]
82pub fn tc_is_neutral(state: &ToeControlState) -> bool {
83    state.left_len.iter().all(|v| v.abs() < 1e-6)
84        && state.right_len.iter().all(|v| v.abs() < 1e-6)
85        && state.splay.abs() < 1e-6
86}
87
88#[allow(dead_code)]
89pub fn tc_average_length(state: &ToeControlState) -> f32 {
90    let sum: f32 = state.left_len.iter().chain(state.right_len.iter()).sum();
91    sum / (TOE_COUNT * 2) as f32
92}
93
94#[allow(dead_code)]
95pub fn tc_splay_angle_rad(state: &ToeControlState) -> f32 {
96    state.splay * FRAC_PI_6
97}
98
99#[allow(dead_code)]
100pub fn tc_to_weights(state: &ToeControlState) -> [f32; TOE_COUNT] {
101    let m = state.config.max_length;
102    let mut w = [0.0f32; TOE_COUNT];
103    #[allow(clippy::needless_range_loop)]
104    for i in 0..TOE_COUNT {
105        w[i] = if m > 1e-9 {
106            (state.left_len[i] + state.right_len[i]) * 0.5 / m
107        } else {
108            0.0
109        };
110    }
111    w
112}
113
114#[allow(dead_code)]
115pub fn tc_to_json(state: &ToeControlState) -> String {
116    format!(
117        "{{\"splay\":{:.4},\"avg_len\":{:.4}}}",
118        state.splay,
119        tc_average_length(state)
120    )
121}
122
123#[cfg(test)]
124mod tests {
125    use super::*;
126    #[test]
127    fn default_neutral() {
128        assert!(tc_is_neutral(&new_toe_control_state(
129            default_toe_control_config()
130        )));
131    }
132    #[test]
133    fn set_length_clamps() {
134        let mut s = new_toe_control_state(default_toe_control_config());
135        tc_set_length(&mut s, ToeFootSide::Left, 0, 5.0);
136        assert!((0.0..=1.0).contains(&s.left_len[0]));
137    }
138    #[test]
139    fn set_splay_clamps() {
140        let mut s = new_toe_control_state(default_toe_control_config());
141        tc_set_splay(&mut s, 5.0);
142        assert!((0.0..=1.0).contains(&s.splay));
143    }
144    #[test]
145    fn reset_zeroes() {
146        let mut s = new_toe_control_state(default_toe_control_config());
147        tc_set_splay(&mut s, 0.5);
148        tc_reset(&mut s);
149        assert!(tc_is_neutral(&s));
150    }
151    #[test]
152    fn average_length_zero_by_default() {
153        let s = new_toe_control_state(default_toe_control_config());
154        assert!(tc_average_length(&s).abs() < 1e-6);
155    }
156    #[test]
157    fn splay_angle_nonneg() {
158        let s = new_toe_control_state(default_toe_control_config());
159        assert!(tc_splay_angle_rad(&s) >= 0.0);
160    }
161    #[test]
162    fn to_weights_zero_by_default() {
163        let s = new_toe_control_state(default_toe_control_config());
164        assert!(tc_to_weights(&s)[0].abs() < 1e-6);
165    }
166    #[test]
167    fn out_of_range_ignored() {
168        let mut s = new_toe_control_state(default_toe_control_config());
169        tc_set_length(&mut s, ToeFootSide::Left, 99, 1.0);
170        assert!(tc_is_neutral(&s));
171    }
172    #[test]
173    fn to_json_has_splay() {
174        assert!(
175            tc_to_json(&new_toe_control_state(default_toe_control_config())).contains("\"splay\"")
176        );
177    }
178    #[test]
179    fn toe_count_is_five() {
180        assert_eq!(TOE_COUNT, 5);
181    }
182}
oxihuman_morph/toe_control.rs

oxihuman_morph/
toe_control.rs
