deke_types/
q.rs

1use std::convert::Infallible;
2
3use approx::{AbsDiffEq, RelativeEq, UlpsEq};
4use ndarray::Array1;
5use num_traits::Float;
6
7use crate::DekeError;
8
9pub type RobotQ<T = f32> = Array1<T>;
10
11/// Statically-sized joint configuration backed by `[T; N]`.
12#[derive(Debug, Clone, Copy, PartialEq)]
13pub struct SRobotQ<const N: usize, T: Float = f32>(pub [T; N]);
14
15impl<const N: usize, T: Float> SRobotQ<N, T> {
16    pub fn zeros() -> Self {
17        Self([T::zero(); N])
18    }
19
20    pub fn from_array(arr: [T; N]) -> Self {
21        Self(arr)
22    }
23
24    pub fn as_slice(&self) -> &[T] {
25        &self.0
26    }
27
28    pub fn as_mut_slice(&mut self) -> &mut [T] {
29        &mut self.0
30    }
31
32    pub fn to_robotq(&self) -> RobotQ<T> {
33        RobotQ::from(self.0.to_vec())
34    }
35
36    pub fn force_from_robotq(q: &RobotQ<T>) -> Self {
37        if let Ok(sq) = Self::try_from(q) {
38            sq
39        } else {
40            let slice = q.as_slice().unwrap_or(&[]);
41            let mut arr = [T::zero(); N];
42            for i in 0..N {
43                arr[i] = slice.get(i).copied().unwrap_or_else(T::zero);
44            }
45            Self(arr)
46        }
47    }
48
49    pub fn norm(&self) -> T {
50        self.dot(self).sqrt()
51    }
52
53    pub fn dot(&self, other: &Self) -> T {
54        let mut sum = T::zero();
55        for i in 0..N {
56            sum = self.0[i].mul_add(other.0[i], sum);
57        }
58        sum
59    }
60
61    pub fn map(&self, f: impl Fn(T) -> T) -> Self {
62        let mut out = [T::zero(); N];
63        for i in 0..N {
64            out[i] = f(self.0[i]);
65        }
66        Self(out)
67    }
68
69    pub fn sum(&self) -> T {
70        let mut s = T::zero();
71        for i in 0..N {
72            s = s + self.0[i];
73        }
74        s
75    }
76
77    pub fn splat(val: T) -> Self {
78        Self([val; N])
79    }
80
81    pub fn from_fn(f: impl Fn(usize) -> T) -> Self {
82        let mut out = [T::zero(); N];
83        for i in 0..N {
84            out[i] = f(i);
85        }
86        Self(out)
87    }
88
89    pub fn norm_squared(&self) -> T {
90        self.dot(self)
91    }
92
93    pub fn normalize(&self) -> Self {
94        let n = self.norm();
95        debug_assert!(n > T::zero(), "cannot normalize zero-length SRobotQ");
96        *self / n
97    }
98
99    pub fn distance(&self, other: &Self) -> T {
100        (*self - *other).norm()
101    }
102
103    pub fn distance_squared(&self, other: &Self) -> T {
104        (*self - *other).norm_squared()
105    }
106
107    pub fn abs(&self) -> Self {
108        self.map(Float::abs)
109    }
110
111    pub fn clamp(&self, min: &Self, max: &Self) -> Self {
112        let mut out = [T::zero(); N];
113        for i in 0..N {
114            if self.0[i] < min.0[i] {
115                out[i] = min.0[i];
116            } else if self.0[i] > max.0[i] {
117                out[i] = max.0[i];
118            } else {
119                out[i] = self.0[i];
120            }
121        }
122        Self(out)
123    }
124
125    pub fn clamp_scalar(&self, min: T, max: T) -> Self {
126        self.map(|x| {
127            if x < min {
128                min
129            } else if x > max {
130                max
131            } else {
132                x
133            }
134        })
135    }
136
137    pub fn max_element(&self) -> T {
138        self.0
139            .iter()
140            .copied()
141            .fold(T::neg_infinity(), |a, b| if b > a { b } else { a })
142    }
143
144    pub fn min_element(&self) -> T {
145        self.0
146            .iter()
147            .copied()
148            .fold(T::infinity(), |a, b| if b < a { b } else { a })
149    }
150
151    pub fn linf_norm(&self) -> T {
152        self.abs().max_element()
153    }
154
155    pub fn elementwise_mul(&self, other: &Self) -> Self {
156        let mut out = [T::zero(); N];
157        for i in 0..N {
158            out[i] = self.0[i] * other.0[i];
159        }
160        Self(out)
161    }
162
163    pub fn elementwise_div(&self, other: &Self) -> Self {
164        let mut out = [T::zero(); N];
165        for i in 0..N {
166            out[i] = self.0[i] / other.0[i];
167        }
168        Self(out)
169    }
170
171    pub fn zip_map(&self, other: &Self, f: impl Fn(T, T) -> T) -> Self {
172        let mut out = [T::zero(); N];
173        for i in 0..N {
174            out[i] = f(self.0[i], other.0[i]);
175        }
176        Self(out)
177    }
178
179    pub fn sqrt(&self) -> Self {
180        self.map(Float::sqrt)
181    }
182
183    pub fn mul_add(&self, mul: &Self, add: &Self) -> Self {
184        let mut out = [T::zero(); N];
185        for i in 0..N {
186            out[i] = self.0[i].mul_add(mul.0[i], add.0[i]);
187        }
188        Self(out)
189    }
190
191    pub fn any_non_finite(&self) -> bool {
192        self.0.iter().any(|x| x.is_nan() || x.is_infinite())
193    }
194
195    pub fn any_gt(&self, other: &Self) -> bool {
196        self.0.iter().zip(other.0.iter()).any(|(a, b)| *a > *b)
197    }
198
199    pub fn any_lt(&self, other: &Self) -> bool {
200        self.0.iter().zip(other.0.iter()).any(|(a, b)| *a < *b)
201    }
202
203    pub fn is_close(&self, other: &Self, tol: T) -> bool {
204        (*self - *other).norm() < tol
205    }
206
207    pub fn interpolate(&self, other: &Self, t: T) -> Self {
208        *self + ((*other - *self) * t)
209    }
210}
211
212impl<const N: usize> SRobotQ<N, f32> {
213    pub const fn from_array_d(arr: [f64; N]) -> Self {
214        let mut out = [0.0; N];
215        let mut i = 0;
216        while i < N {
217            out[i] = arr[i] as f32;
218            i += 1;
219        }
220        Self(out)
221    }
222}
223
224impl<const N: usize, T: Float> std::ops::Index<usize> for SRobotQ<N, T> {
225    type Output = T;
226    #[inline]
227    fn index(&self, i: usize) -> &T {
228        &self.0[i]
229    }
230}
231
232impl<const N: usize, T: Float> std::ops::IndexMut<usize> for SRobotQ<N, T> {
233    #[inline]
234    fn index_mut(&mut self, i: usize) -> &mut T {
235        &mut self.0[i]
236    }
237}
238
239impl<const N: usize, T: Float> std::ops::Add for SRobotQ<N, T> {
240    type Output = Self;
241    #[inline]
242    fn add(self, rhs: Self) -> Self {
243        let mut out = [T::zero(); N];
244        for i in 0..N {
245            out[i] = self.0[i] + rhs.0[i];
246        }
247        Self(out)
248    }
249}
250
251impl<const N: usize, T: Float> std::ops::Sub for SRobotQ<N, T> {
252    type Output = Self;
253    #[inline]
254    fn sub(self, rhs: Self) -> Self {
255        let mut out = [T::zero(); N];
256        for i in 0..N {
257            out[i] = self.0[i] - rhs.0[i];
258        }
259        Self(out)
260    }
261}
262
263impl<const N: usize, T: Float> std::ops::Neg for SRobotQ<N, T> {
264    type Output = Self;
265    #[inline]
266    fn neg(self) -> Self {
267        let mut out = [T::zero(); N];
268        for i in 0..N {
269            out[i] = T::zero() - self.0[i];
270        }
271        Self(out)
272    }
273}
274
275impl<const N: usize, T: Float> std::ops::Mul<T> for SRobotQ<N, T> {
276    type Output = Self;
277    #[inline]
278    fn mul(self, rhs: T) -> Self {
279        let mut out = [T::zero(); N];
280        for i in 0..N {
281            out[i] = self.0[i] * rhs;
282        }
283        Self(out)
284    }
285}
286
287impl<const N: usize> std::ops::Mul<SRobotQ<N, f32>> for f32 {
288    type Output = SRobotQ<N, f32>;
289    #[inline]
290    fn mul(self, rhs: SRobotQ<N, f32>) -> SRobotQ<N, f32> {
291        rhs * self
292    }
293}
294
295impl<const N: usize> std::ops::Mul<SRobotQ<N, f64>> for f64 {
296    type Output = SRobotQ<N, f64>;
297    #[inline]
298    fn mul(self, rhs: SRobotQ<N, f64>) -> SRobotQ<N, f64> {
299        rhs * self
300    }
301}
302
303impl<const N: usize, T: Float> std::ops::Div<T> for SRobotQ<N, T> {
304    type Output = Self;
305    #[inline]
306    fn div(self, rhs: T) -> Self {
307        let mut out = [T::zero(); N];
308        for i in 0..N {
309            out[i] = self.0[i] / rhs;
310        }
311        Self(out)
312    }
313}
314
315impl<const N: usize, T: Float> std::ops::AddAssign for SRobotQ<N, T> {
316    #[inline]
317    fn add_assign(&mut self, rhs: Self) {
318        for i in 0..N {
319            self.0[i] = self.0[i] + rhs.0[i];
320        }
321    }
322}
323
324impl<const N: usize, T: Float> std::ops::SubAssign for SRobotQ<N, T> {
325    #[inline]
326    fn sub_assign(&mut self, rhs: Self) {
327        for i in 0..N {
328            self.0[i] = self.0[i] - rhs.0[i];
329        }
330    }
331}
332
333impl<const N: usize, T: Float> std::ops::MulAssign<T> for SRobotQ<N, T> {
334    #[inline]
335    fn mul_assign(&mut self, rhs: T) {
336        for i in 0..N {
337            self.0[i] = self.0[i] * rhs;
338        }
339    }
340}
341
342impl<const N: usize, T: Float> std::ops::DivAssign<T> for SRobotQ<N, T> {
343    #[inline]
344    fn div_assign(&mut self, rhs: T) {
345        for i in 0..N {
346            self.0[i] = self.0[i] / rhs;
347        }
348    }
349}
350
351macro_rules! impl_cross_float_ops {
352    ($lhs:ty, $rhs:ty) => {
353        impl<const N: usize> std::ops::Add<SRobotQ<N, $rhs>> for SRobotQ<N, $lhs> {
354            type Output = SRobotQ<N, $lhs>;
355            #[inline]
356            fn add(self, rhs: SRobotQ<N, $rhs>) -> SRobotQ<N, $lhs> {
357                let mut out = [0.0 as $lhs; N];
358                for i in 0..N {
359                    out[i] = self.0[i] + rhs.0[i] as $lhs;
360                }
361                SRobotQ(out)
362            }
363        }
364
365        impl<const N: usize> std::ops::Sub<SRobotQ<N, $rhs>> for SRobotQ<N, $lhs> {
366            type Output = SRobotQ<N, $lhs>;
367            #[inline]
368            fn sub(self, rhs: SRobotQ<N, $rhs>) -> SRobotQ<N, $lhs> {
369                let mut out = [0.0 as $lhs; N];
370                for i in 0..N {
371                    out[i] = self.0[i] - rhs.0[i] as $lhs;
372                }
373                SRobotQ(out)
374            }
375        }
376
377        impl<const N: usize> std::ops::AddAssign<SRobotQ<N, $rhs>> for SRobotQ<N, $lhs> {
378            #[inline]
379            fn add_assign(&mut self, rhs: SRobotQ<N, $rhs>) {
380                for i in 0..N {
381                    self.0[i] = self.0[i] + rhs.0[i] as $lhs;
382                }
383            }
384        }
385
386        impl<const N: usize> std::ops::SubAssign<SRobotQ<N, $rhs>> for SRobotQ<N, $lhs> {
387            #[inline]
388            fn sub_assign(&mut self, rhs: SRobotQ<N, $rhs>) {
389                for i in 0..N {
390                    self.0[i] = self.0[i] - rhs.0[i] as $lhs;
391                }
392            }
393        }
394
395        impl<const N: usize> std::ops::Mul<$rhs> for SRobotQ<N, $lhs> {
396            type Output = SRobotQ<N, $lhs>;
397            #[inline]
398            fn mul(self, rhs: $rhs) -> SRobotQ<N, $lhs> {
399                let rhs = rhs as $lhs;
400                let mut out = [0.0 as $lhs; N];
401                for i in 0..N {
402                    out[i] = self.0[i] * rhs;
403                }
404                SRobotQ(out)
405            }
406        }
407
408        impl<const N: usize> std::ops::Div<$rhs> for SRobotQ<N, $lhs> {
409            type Output = SRobotQ<N, $lhs>;
410            #[inline]
411            fn div(self, rhs: $rhs) -> SRobotQ<N, $lhs> {
412                let rhs = rhs as $lhs;
413                let mut out = [0.0 as $lhs; N];
414                for i in 0..N {
415                    out[i] = self.0[i] / rhs;
416                }
417                SRobotQ(out)
418            }
419        }
420
421        impl<const N: usize> std::ops::MulAssign<$rhs> for SRobotQ<N, $lhs> {
422            #[inline]
423            fn mul_assign(&mut self, rhs: $rhs) {
424                let rhs = rhs as $lhs;
425                for i in 0..N {
426                    self.0[i] = self.0[i] * rhs;
427                }
428            }
429        }
430
431        impl<const N: usize> std::ops::DivAssign<$rhs> for SRobotQ<N, $lhs> {
432            #[inline]
433            fn div_assign(&mut self, rhs: $rhs) {
434                let rhs = rhs as $lhs;
435                for i in 0..N {
436                    self.0[i] = self.0[i] / rhs;
437                }
438            }
439        }
440
441        impl<const N: usize> std::ops::Mul<SRobotQ<N, $lhs>> for $rhs {
442            type Output = SRobotQ<N, $lhs>;
443            #[inline]
444            fn mul(self, rhs: SRobotQ<N, $lhs>) -> SRobotQ<N, $lhs> {
445                rhs * (self as $lhs)
446            }
447        }
448    };
449}
450
451impl_cross_float_ops!(f32, f64);
452impl_cross_float_ops!(f64, f32);
453
454impl<const N: usize> std::ops::Add<SRobotQ<N, f32>> for &RobotQ {
455    type Output = SRobotQ<N, f32>;
456    #[inline]
457    fn add(self, rhs: SRobotQ<N, f32>) -> SRobotQ<N, f32> {
458        SRobotQ::<N, f32>::force_from_robotq(self) + rhs
459    }
460}
461
462impl<const N: usize> std::ops::Sub<SRobotQ<N, f32>> for &RobotQ {
463    type Output = SRobotQ<N, f32>;
464    #[inline]
465    fn sub(self, rhs: SRobotQ<N, f32>) -> SRobotQ<N, f32> {
466        SRobotQ::<N, f32>::force_from_robotq(self) - rhs
467    }
468}
469
470impl<const N: usize, T: Float> Default for SRobotQ<N, T> {
471    #[inline]
472    fn default() -> Self {
473        Self::zeros()
474    }
475}
476
477impl<const N: usize, T: Float> AsRef<[T; N]> for SRobotQ<N, T> {
478    #[inline]
479    fn as_ref(&self) -> &[T; N] {
480        &self.0
481    }
482}
483
484impl<const N: usize, T: Float> AsMut<[T; N]> for SRobotQ<N, T> {
485    #[inline]
486    fn as_mut(&mut self) -> &mut [T; N] {
487        &mut self.0
488    }
489}
490
491impl<const N: usize, T: Float> AsRef<[T]> for SRobotQ<N, T> {
492    #[inline]
493    fn as_ref(&self) -> &[T] {
494        &self.0
495    }
496}
497
498impl<const N: usize, T: Float> AsMut<[T]> for SRobotQ<N, T> {
499    #[inline]
500    fn as_mut(&mut self) -> &mut [T] {
501        &mut self.0
502    }
503}
504
505impl<const N: usize, T: Float> From<[T; N]> for SRobotQ<N, T> {
506    #[inline]
507    fn from(arr: [T; N]) -> Self {
508        Self(arr)
509    }
510}
511
512impl<const N: usize, T: Float> From<&[T; N]> for SRobotQ<N, T> {
513    #[inline]
514    fn from(arr: &[T; N]) -> Self {
515        Self(*arr)
516    }
517}
518
519impl<const N: usize> From<[f64; N]> for SRobotQ<N, f32> {
520    #[inline]
521    fn from(arr: [f64; N]) -> Self {
522        let mut out = [0.0f32; N];
523        for i in 0..N {
524            out[i] = arr[i] as f32;
525        }
526        Self(out)
527    }
528}
529
530impl<const N: usize> From<&[f64; N]> for SRobotQ<N, f32> {
531    #[inline]
532    fn from(arr: &[f64; N]) -> Self {
533        Self::from(*arr)
534    }
535}
536
537impl<const N: usize> From<[f32; N]> for SRobotQ<N, f64> {
538    #[inline]
539    fn from(arr: [f32; N]) -> Self {
540        let mut out = [0.0f64; N];
541        for i in 0..N {
542            out[i] = arr[i] as f64;
543        }
544        Self(out)
545    }
546}
547
548impl<const N: usize> From<&[f32; N]> for SRobotQ<N, f64> {
549    #[inline]
550    fn from(arr: &[f32; N]) -> Self {
551        Self::from(*arr)
552    }
553}
554
555impl<const N: usize, T: Float> From<SRobotQ<N, T>> for [T; N] {
556    #[inline]
557    fn from(q: SRobotQ<N, T>) -> [T; N] {
558        q.0
559    }
560}
561
562impl<const N: usize> From<SRobotQ<N, f32>> for [f64; N] {
563    #[inline]
564    fn from(q: SRobotQ<N, f32>) -> [f64; N] {
565        let mut out = [0.0f64; N];
566        for i in 0..N {
567            out[i] = q.0[i] as f64;
568        }
569        out
570    }
571}
572
573impl<const N: usize> From<SRobotQ<N, f64>> for [f32; N] {
574    #[inline]
575    fn from(q: SRobotQ<N, f64>) -> [f32; N] {
576        let mut out = [0.0f32; N];
577        for i in 0..N {
578            out[i] = q.0[i] as f32;
579        }
580        out
581    }
582}
583
584impl<const N: usize, T: Float> From<SRobotQ<N, T>> for Vec<T> {
585    #[inline]
586    fn from(q: SRobotQ<N, T>) -> Vec<T> {
587        q.0.to_vec()
588    }
589}
590
591impl<const N: usize> From<SRobotQ<N, f32>> for Vec<f64> {
592    #[inline]
593    fn from(q: SRobotQ<N, f32>) -> Vec<f64> {
594        q.0.iter().map(|&v| v as f64).collect()
595    }
596}
597
598impl<const N: usize> From<SRobotQ<N, f64>> for Vec<f32> {
599    #[inline]
600    fn from(q: SRobotQ<N, f64>) -> Vec<f32> {
601        q.0.iter().map(|&v| v as f32).collect()
602    }
603}
604
605impl<const N: usize, T: Float> From<SRobotQ<N, T>> for RobotQ<T> {
606    #[inline]
607    fn from(q: SRobotQ<N, T>) -> RobotQ<T> {
608        q.to_robotq()
609    }
610}
611
612impl<const N: usize, T: Float> From<&SRobotQ<N, T>> for SRobotQ<N, T> {
613    #[inline]
614    fn from(q: &SRobotQ<N, T>) -> Self {
615        *q
616    }
617}
618
619impl<const N: usize, T: Float> TryFrom<&[T]> for SRobotQ<N, T> {
620    type Error = DekeError;
621
622    #[inline]
623    fn try_from(slice: &[T]) -> Result<Self, Self::Error> {
624        if slice.len() != N {
625            return Err(DekeError::ShapeMismatch {
626                expected: N,
627                found: slice.len(),
628            });
629        }
630        let mut arr = [T::zero(); N];
631        arr.copy_from_slice(slice);
632        Ok(Self(arr))
633    }
634}
635
636impl<const N: usize, T: Float> TryFrom<Vec<T>> for SRobotQ<N, T> {
637    type Error = DekeError;
638
639    #[inline]
640    fn try_from(v: Vec<T>) -> Result<Self, Self::Error> {
641        Self::try_from(v.as_slice())
642    }
643}
644
645impl<const N: usize, T: Float> TryFrom<&Vec<T>> for SRobotQ<N, T> {
646    type Error = DekeError;
647
648    #[inline]
649    fn try_from(v: &Vec<T>) -> Result<Self, Self::Error> {
650        Self::try_from(v.as_slice())
651    }
652}
653
654impl<const N: usize> TryFrom<&[f64]> for SRobotQ<N, f32> {
655    type Error = DekeError;
656
657    #[inline]
658    fn try_from(slice: &[f64]) -> Result<Self, Self::Error> {
659        if slice.len() != N {
660            return Err(DekeError::ShapeMismatch {
661                expected: N,
662                found: slice.len(),
663            });
664        }
665        let mut arr = [0.0f32; N];
666        for i in 0..N {
667            arr[i] = slice[i] as f32;
668        }
669        Ok(Self(arr))
670    }
671}
672
673impl<const N: usize> TryFrom<Vec<f64>> for SRobotQ<N, f32> {
674    type Error = DekeError;
675
676    #[inline]
677    fn try_from(v: Vec<f64>) -> Result<Self, Self::Error> {
678        Self::try_from(v.as_slice())
679    }
680}
681
682impl<const N: usize> TryFrom<&Vec<f64>> for SRobotQ<N, f32> {
683    type Error = DekeError;
684
685    #[inline]
686    fn try_from(v: &Vec<f64>) -> Result<Self, Self::Error> {
687        Self::try_from(v.as_slice())
688    }
689}
690
691impl<const N: usize> TryFrom<&[f32]> for SRobotQ<N, f64> {
692    type Error = DekeError;
693
694    #[inline]
695    fn try_from(slice: &[f32]) -> Result<Self, Self::Error> {
696        if slice.len() != N {
697            return Err(DekeError::ShapeMismatch {
698                expected: N,
699                found: slice.len(),
700            });
701        }
702        let mut arr = [0.0f64; N];
703        for i in 0..N {
704            arr[i] = slice[i] as f64;
705        }
706        Ok(Self(arr))
707    }
708}
709
710impl<const N: usize> TryFrom<Vec<f32>> for SRobotQ<N, f64> {
711    type Error = DekeError;
712
713    #[inline]
714    fn try_from(v: Vec<f32>) -> Result<Self, Self::Error> {
715        Self::try_from(v.as_slice())
716    }
717}
718
719impl<const N: usize> TryFrom<&Vec<f32>> for SRobotQ<N, f64> {
720    type Error = DekeError;
721
722    #[inline]
723    fn try_from(v: &Vec<f32>) -> Result<Self, Self::Error> {
724        Self::try_from(v.as_slice())
725    }
726}
727
728impl<const N: usize, T: Float> TryFrom<&RobotQ<T>> for SRobotQ<N, T> {
729    type Error = DekeError;
730
731    #[inline]
732    fn try_from(q: &RobotQ<T>) -> Result<Self, Self::Error> {
733        let slice = q.as_slice().unwrap_or(&[]);
734        if slice.len() != N {
735            return Err(DekeError::ShapeMismatch {
736                expected: N,
737                found: slice.len(),
738            });
739        }
740        let mut arr = [T::zero(); N];
741        arr.copy_from_slice(slice);
742        Ok(Self(arr))
743    }
744}
745
746impl<const N: usize, T: Float + AbsDiffEq<Epsilon = T>> AbsDiffEq for SRobotQ<N, T> {
747    type Epsilon = T;
748
749    fn default_epsilon() -> T {
750        T::default_epsilon()
751    }
752
753    fn abs_diff_eq(&self, other: &Self, epsilon: T) -> bool {
754        self.0
755            .iter()
756            .zip(other.0.iter())
757            .all(|(a, b)| a.abs_diff_eq(b, epsilon))
758    }
759}
760
761impl<const N: usize, T: Float + RelativeEq + AbsDiffEq<Epsilon = T>> RelativeEq
762    for SRobotQ<N, T>
763{
764    fn default_max_relative() -> T {
765        T::default_max_relative()
766    }
767
768    fn relative_eq(&self, other: &Self, epsilon: T, max_relative: T) -> bool {
769        self.0
770            .iter()
771            .zip(other.0.iter())
772            .all(|(a, b)| a.relative_eq(b, epsilon, max_relative))
773    }
774}
775
776impl<const N: usize, T: Float + UlpsEq + AbsDiffEq<Epsilon = T>> UlpsEq for SRobotQ<N, T> {
777    fn default_max_ulps() -> u32 {
778        T::default_max_ulps()
779    }
780
781    fn ulps_eq(&self, other: &Self, epsilon: T, max_ulps: u32) -> bool {
782        self.0
783            .iter()
784            .zip(other.0.iter())
785            .all(|(a, b)| a.ulps_eq(b, epsilon, max_ulps))
786    }
787}
788
789impl<const N: usize, T: Float> TryFrom<RobotQ<T>> for SRobotQ<N, T> {
790    type Error = DekeError;
791
792    #[inline]
793    fn try_from(q: RobotQ<T>) -> Result<Self, Self::Error> {
794        let slice = q.as_slice().unwrap_or(&[]);
795        if slice.len() != N {
796            return Err(DekeError::ShapeMismatch {
797                expected: N,
798                found: slice.len(),
799            });
800        }
801        let mut arr = [T::zero(); N];
802        arr.copy_from_slice(slice);
803        Ok(Self(arr))
804    }
805}
806
807impl<const N: usize, T: Float> From<&SRobotQ<N, T>> for RobotQ<T> {
808    #[inline]
809    fn from(sq: &SRobotQ<N, T>) -> RobotQ<T> {
810        sq.to_robotq()
811    }
812}
813
814impl<const N: usize> From<SRobotQ<N, f32>> for RobotQ<f64> {
815    #[inline]
816    fn from(q: SRobotQ<N, f32>) -> RobotQ<f64> {
817        q.0.iter().map(|&v| v as f64).collect()
818    }
819}
820
821impl<const N: usize> From<SRobotQ<N, f64>> for RobotQ<f32> {
822    #[inline]
823    fn from(q: SRobotQ<N, f64>) -> RobotQ<f32> {
824        q.0.iter().map(|&v| v as f32).collect()
825    }
826}
827
828impl<const N: usize> From<&SRobotQ<N, f32>> for RobotQ<f64> {
829    #[inline]
830    fn from(q: &SRobotQ<N, f32>) -> RobotQ<f64> {
831        q.0.iter().map(|&v| v as f64).collect()
832    }
833}
834
835impl<const N: usize> From<&SRobotQ<N, f64>> for RobotQ<f32> {
836    #[inline]
837    fn from(q: &SRobotQ<N, f64>) -> RobotQ<f32> {
838        q.0.iter().map(|&v| v as f32).collect()
839    }
840}
841
842impl<const N: usize> TryFrom<&RobotQ<f64>> for SRobotQ<N, f32> {
843    type Error = DekeError;
844
845    #[inline]
846    fn try_from(q: &RobotQ<f64>) -> Result<Self, Self::Error> {
847        let slice = q.as_slice().unwrap_or(&[]);
848        if slice.len() != N {
849            return Err(DekeError::ShapeMismatch {
850                expected: N,
851                found: slice.len(),
852            });
853        }
854        let mut arr = [0.0f32; N];
855        for i in 0..N {
856            arr[i] = slice[i] as f32;
857        }
858        Ok(Self(arr))
859    }
860}
861
862impl<const N: usize> TryFrom<RobotQ<f64>> for SRobotQ<N, f32> {
863    type Error = DekeError;
864
865    #[inline]
866    fn try_from(q: RobotQ<f64>) -> Result<Self, Self::Error> {
867        Self::try_from(&q)
868    }
869}
870
871impl<const N: usize> TryFrom<&RobotQ<f32>> for SRobotQ<N, f64> {
872    type Error = DekeError;
873
874    #[inline]
875    fn try_from(q: &RobotQ<f32>) -> Result<Self, Self::Error> {
876        let slice = q.as_slice().unwrap_or(&[]);
877        if slice.len() != N {
878            return Err(DekeError::ShapeMismatch {
879                expected: N,
880                found: slice.len(),
881            });
882        }
883        let mut arr = [0.0f64; N];
884        for i in 0..N {
885            arr[i] = slice[i] as f64;
886        }
887        Ok(Self(arr))
888    }
889}
890
891impl<const N: usize> TryFrom<RobotQ<f32>> for SRobotQ<N, f64> {
892    type Error = DekeError;
893
894    #[inline]
895    fn try_from(q: RobotQ<f32>) -> Result<Self, Self::Error> {
896        Self::try_from(&q)
897    }
898}
899
900impl<const N: usize> From<SRobotQ<N, f32>> for SRobotQ<N, f64> {
901    #[inline]
902    fn from(q: SRobotQ<N, f32>) -> Self {
903        let mut out = [0.0f64; N];
904        for i in 0..N {
905            out[i] = q.0[i] as f64;
906        }
907        Self(out)
908    }
909}
910
911impl<const N: usize> From<SRobotQ<N, f64>> for SRobotQ<N, f32> {
912    #[inline]
913    fn from(q: SRobotQ<N, f64>) -> Self {
914        let mut out = [0.0f32; N];
915        for i in 0..N {
916            out[i] = q.0[i] as f32;
917        }
918        Self(out)
919    }
920}
921
922pub fn robotq<T: Float, U: Float>(vals: impl IntoIterator<Item = T>) -> RobotQ<U> {
923    use num_traits::NumCast;
924    vals.into_iter().map(|v| NumCast::from(v).unwrap_or(U::zero())).collect()
925}
926
927
928pub trait SRobotQLike<const N: usize, E: Into<DekeError>>: TryInto<SRobotQ<N, f32>, Error = E> + TryInto<SRobotQ<N, f64>, Error = E> {
929    fn to_srobotq(self) -> Result<SRobotQ<N, f32>, E>;
930    fn to_srobotq_f64(self) -> Result<SRobotQ<N, f64>, E>;
931}
932
933impl<const N: usize, T, E: Into<DekeError>> SRobotQLike<N, E> for T
934where
935    T: TryInto<SRobotQ<N, f32>, Error = E> + TryInto<SRobotQ<N, f64>, Error = E>,
936{
937    #[inline]
938    fn to_srobotq(self) -> Result<SRobotQ<N, f32>, E> {
939        self.try_into()
940    }
941
942    #[inline]
943    fn to_srobotq_f64(self) -> Result<SRobotQ<N, f64>, E> {
944        self.try_into()
945    }
946}
947
948#[allow(dead_code)]
949const _: () = {
950    fn assert_srobotq_like<const N: usize, E: Into<DekeError>, T: SRobotQLike<N, E>>() {}
951
952    fn assert_all<const N: usize>() {
953        assert_srobotq_like::<N, Infallible, SRobotQ<N, f32>>();
954        assert_srobotq_like::<N, Infallible, SRobotQ<N, f64>>();
955        assert_srobotq_like::<N, DekeError, RobotQ<f32>>();
956        assert_srobotq_like::<N, DekeError, RobotQ<f64>>();
957        assert_srobotq_like::<N, DekeError, Vec<f32>>();
958        assert_srobotq_like::<N, DekeError, Vec<f64>>();
959        assert_srobotq_like::<N, DekeError, &[f32]>();
960        assert_srobotq_like::<N, DekeError, &[f64]>();
961    }
962};
deke_types/q.rs

deke_types/
q.rs
