robot_behavior/utils/
limit.rs

1// use std::simd::{LaneCount, Simd, SupportedLaneCount};
2
3#[inline(always)]
4pub fn limit<'a, const N: usize>(q: &'a mut [f64; N], q_bound: &[f64; N]) -> &'a mut [f64; N] {
5    q.iter_mut().zip(q_bound.iter()).for_each(|(qi, &qbi)| {
6        *qi = (*qi).clamp(-qbi, qbi);
7    });
8    q
9}
10
11#[inline(always)]
12pub fn clamp<'a, const N: usize>(
13    q: &'a mut [f64; N],
14    min: &[f64; N],
15    max: &[f64; N],
16) -> &'a mut [f64; N] {
17    q.iter_mut()
18        .zip(min.iter().zip(max.iter())) // 合并为单次 zip
19        .for_each(|(qi, (&min, &max))| {
20            *qi = qi.clamp(min, max);
21        });
22    q
23}
24
25#[inline(always)]
26pub fn limit_dot<'a, const N: usize>(
27    q: &'a mut [f64; N],
28    q_last: &[f64; N],
29    time: f64,
30    q_dot_bound: &[f64; N],
31) -> &'a mut [f64; N] {
32    q.iter_mut()
33        .zip(q_last.iter())
34        .zip(q_dot_bound.iter())
35        .for_each(|((qi, &qli), &qbi)| {
36            *qi = (*qi).clamp(qli - qbi * time, qli + qbi * time);
37        });
38    q
39}
40
41#[inline(always)]
42pub fn difference<const N: usize>(q: &[f64; N], q_last: &[f64; N], time: f64) -> [f64; N] {
43    let inv_time = 1.0 / time;
44    let mut q_diff = [0.0; N];
45    q_diff
46        .iter_mut()
47        .zip(q.iter().zip(q_last.iter()))
48        .for_each(|(qi, (&qi_val, &qli_val))| {
49            *qi = (qi_val - qli_val) * inv_time;
50        });
51    q_diff
52}
53
54// #[inline(always)]
55// pub fn difference_simd<const N: usize>(
56//     q: Simd<f64, N>,
57//     q_last: Simd<f64, N>,
58//     time: Simd<f64, N>,
59// ) -> Simd<f64, N>
60// where
61//     LaneCount<N>: SupportedLaneCount,
62// {
63//     (q - q_last) / time
64// }
65
66#[inline(always)]
67pub fn update<'a, const N: usize>(
68    q: &'a mut [f64; N],
69    q_dot: &[f64; N],
70    time: f64,
71) -> &'a mut [f64; N] {
72    q.iter_mut().zip(q_dot.iter()).for_each(|(qi, &qdi)| {
73        *qi += qdi * time;
74    });
75    q
76}
77
78// #[inline(always)]
79// pub fn update_simd<const N: usize>(q: [f64; N], q_dot: &[f64; N], time: f64) -> [f64; N]
80// where
81//     LaneCount<N>: SupportedLaneCount,
82// {
83//     let q = Simd::from_array(q);
84//     let q_dot = Simd::from_array(*q_dot);
85//     let time = Simd::splat(time);
86//     (q + q_dot * time).to_array()
87// }
88
89#[cfg(test)]
90mod tests {
91    use super::*;
92
93    #[test]
94    fn test_simd_optimization() {
95        let mut q = [1.5; 8];
96        let q_last = [0.0; 8];
97        let time = 0.1;
98
99        let start_time = std::time::Instant::now();
100        for _ in 0..100_000 {
101            let _ = difference(&mut q, &q_last, time);
102        }
103        println!("difference: {:?}", start_time.elapsed());
104        // let q = Simd::from_array(q);
105        // let q_last = Simd::from_array(q_last);
106        // let time = Simd::splat(time);
107
108        // let start_time = std::time::Instant::now();
109        // for _ in 0..100_000 {
110        //     let _ = difference_simd(q, q_last, time);
111        // }
112        // println!("difference_simd: {:?}", start_time.elapsed());
113    }
114}
robot_behavior/utils/limit.rs

robot_behavior/utils/
limit.rs
