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