k/joint/
range.rs

1/*
2  Copyright 2020 Takashi Ogura
3
4  Licensed under the Apache License, Version 2.0 (the "License");
5  you may not use this file except in compliance with the License.
6  You may obtain a copy of the License at
7
8      http://www.apache.org/licenses/LICENSE-2.0
9
10  Unless required by applicable law or agreed to in writing, software
11  distributed under the License is distributed on an "AS IS" BASIS,
12  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  See the License for the specific language governing permissions and
14  limitations under the License.
15*/
16use nalgebra::RealField;
17
18/// min/max range to check the joint position
19#[derive(Copy, Debug, Clone)]
20pub struct Range<T: RealField> {
21    pub min: T,
22    pub max: T,
23}
24
25impl<T> Range<T>
26where
27    T: RealField,
28{
29    /// Create new Range instance
30    ///
31    /// In case `min` is greater than `max`, this function panics.
32    ///
33    /// # Examples
34    ///
35    /// ```
36    /// let range = k::joint::Range::new(-1.0, 1.0);
37    /// // let range = k::joint::Range::new(1.0, -1.0);  // panic
38    /// ```
39    pub fn new(min: T, max: T) -> Self {
40        assert!(min <= max, "min must be less than or equal to max");
41        Range { min, max }
42    }
43    /// Check if the value is in the range
44    ///
45    /// `true` means it is OK.
46    /// If the val is the same as the limit value (`min` or `max`), it returns true (valid).
47    ///
48    /// # Examples
49    ///
50    /// ```
51    /// let range = k::joint::Range::new(-1.0, 1.0);
52    /// assert!(range.is_valid(0.0));
53    /// assert!(range.is_valid(1.0));
54    /// assert!(!range.is_valid(1.5));
55    /// ```
56    pub fn is_valid(&self, val: T) -> bool {
57        val <= self.max && val >= self.min
58    }
59    /// Clamp the value with the range
60    ///
61    /// # Examples
62    ///
63    /// ```
64    /// let range = k::joint::Range::new(-1.0, 1.0);
65    /// assert_eq!(range.clamp(0.5), 0.5);
66    /// assert_eq!(range.clamp(2.0), 1.0);
67    /// assert_eq!(range.clamp(-2.0), -1.0);
68    /// ```
69    pub fn clamp(&self, val: T) -> T {
70        if val < self.min {
71            self.min.clone()
72        } else if val > self.max {
73            self.max.clone()
74        } else {
75            val
76        }
77    }
78}
79
80impl<T> From<::std::ops::RangeInclusive<T>> for Range<T>
81where
82    T: RealField,
83{
84    /// # Examples
85    ///
86    /// ```
87    /// let range : k::joint::Range<f64> = (-1.0..=1.0).into();
88    /// assert!(range.is_valid(0.0));
89    /// assert!(range.is_valid(1.0));
90    /// assert!(!range.is_valid(1.5));
91    /// ```
92    fn from(range: ::std::ops::RangeInclusive<T>) -> Self {
93        let (min, max) = range.into_inner();
94        Range::new(min, max)
95    }
96}