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