robot_behavior/utils/
limit.rs

1use 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)]
55pub 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>
60where
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)]
79pub fn update_simd<const N: usize>(q: [f64; N], q_dot: &[f64; N], time: f64) -> [f64; N]
80where
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
