deke_types/
validator.rs

1use std::{fmt::Debug, sync::Arc};
2
3use crate::{DekeError, DekeResult, FKScalar, SRobotQ, SRobotQLike};
4
5
6pub trait ValidatorContext: Sized {}
7
8#[doc(hidden)]
9pub trait Leaf: ValidatorContext {}
10
11impl ValidatorContext for () {}
12
13#[macro_export]
14macro_rules! validator_context_type_impl {
15    ($($ident:ident),*) => {
16        $(
17            impl $crate::ValidatorContext for $ident {}
18            impl $crate::Leaf for $ident {}
19        )*
20    };
21}
22
23impl<A: ValidatorContext, B: ValidatorContext> ValidatorContext for (A, B) {}
24
25pub trait FromFlattened<Flattened>: ValidatorContext {
26    fn nest(flattened: Flattened) -> Self;
27}
28
29macro_rules! validator_context_tuple_impl {
30    ($tup:tt) => {
31        validator_context_tuple_impl!(@rewrite $tup [emit_impl]);
32    };
33
34    (@rewrite ($l:tt, $r:tt) [$($cb:tt)*]) => {
35        validator_context_tuple_impl!(@rewrite $l [pair_right $r [$($cb)*]]);
36    };
37    (@rewrite $ident:ident [$($cb:tt)*]) => {
38        validator_context_tuple_impl!(@invoke [$($cb)*] [$ident] $ident);
39        validator_context_tuple_impl!(@invoke [$($cb)*] [] ());
40    };
41
42    (@invoke [pair_right $r:tt [$($cb:tt)*]] [$($kept_l:ident)*] $rew_l:tt) => {
43        validator_context_tuple_impl!(@rewrite $r [pair_combine [$($kept_l)*] $rew_l [$($cb)*]]);
44    };
45    (@invoke [pair_combine [$($kept_l:ident)*] $rew_l:tt [$($cb:tt)*]] [$($kept_r:ident)*] $rew_r:tt) => {
46        validator_context_tuple_impl!(@invoke [$($cb)*] [$($kept_l)* $($kept_r)*] ($rew_l, $rew_r));
47    };
48    (@invoke [emit_impl] [$($kept:ident)*] $shape:tt) => {
49        #[allow(non_snake_case)]
50        impl<$($kept: Leaf),*> FromFlattened<($($kept,)*)> for $shape {
51            #[inline]
52            fn nest(flattened: ($($kept,)*)) -> Self {
53                let ($($kept,)*) = flattened;
54                $shape
55            }
56        }
57    };
58}
59
60validator_context_tuple_impl! { (A, (B, C)) }
61validator_context_tuple_impl! { ((A, B), C) }
62
63validator_context_tuple_impl! { (A, (B, (C, D))) }
64validator_context_tuple_impl! { (A, ((B, C), D)) }
65validator_context_tuple_impl! { ((A, B), (C, D)) }
66validator_context_tuple_impl! { ((A, (B, C)), D) }
67validator_context_tuple_impl! { (((A, B), C), D) }
68
69validator_context_tuple_impl! { (A, (B, (C, (D, E)))) }
70validator_context_tuple_impl! { (A, (B, ((C, D), E))) }
71validator_context_tuple_impl! { (A, ((B, C), (D, E))) }
72validator_context_tuple_impl! { (A, ((B, (C, D)), E)) }
73validator_context_tuple_impl! { (A, (((B, C), D), E)) }
74validator_context_tuple_impl! { ((A, B), (C, (D, E))) }
75validator_context_tuple_impl! { ((A, B), ((C, D), E)) }
76validator_context_tuple_impl! { ((A, (B, C)), (D, E)) }
77validator_context_tuple_impl! { (((A, B), C), (D, E)) }
78validator_context_tuple_impl! { ((A, (B, (C, D))), E) }
79validator_context_tuple_impl! { ((A, ((B, C), D)), E) }
80validator_context_tuple_impl! { (((A, B), (C, D)), E) }
81validator_context_tuple_impl! { (((A, (B, C)), D), E) }
82validator_context_tuple_impl! { ((((A, B), C), D), E) }
83
84validator_context_tuple_impl! { ((A, B), ((C, D), (E, F))) }
85validator_context_tuple_impl! { (((A, B), (C, D)), (E, F)) }
86validator_context_tuple_impl! { ((A, (B, C)), ((D, E), F)) }
87validator_context_tuple_impl! { (((A, B), C), ((D, E), F)) }
88
89#[doc(hidden)]
90mod sealed {
91    pub trait Sealed {}
92}
93
94pub trait ValidatorRet: Sized + sealed::Sealed + Copy {
95    fn as_f64(&self) -> f64;
96}
97
98impl sealed::Sealed for () {}
99impl ValidatorRet for () {
100    #[inline]
101    fn as_f64(&self) -> f64 {
102        f64::INFINITY
103    }
104}
105
106impl sealed::Sealed for f32 {}
107impl ValidatorRet for f32 {
108    #[inline]
109    fn as_f64(&self) -> f64 {
110        *self as f64
111    }
112}
113
114impl sealed::Sealed for f64 {}
115impl ValidatorRet for f64 {
116    #[inline]
117    fn as_f64(&self) -> f64 {
118        *self
119    }
120}
121
122pub trait Validator<const N: usize, R: ValidatorRet = (), F: FKScalar = f32>: Sized + Clone + Debug + Send + Sync + 'static {
123    type Context<'ctx>: ValidatorContext;
124
125    fn validate<'ctx, E: Into<DekeError>, A: SRobotQLike<N, E, F>>(
126        &self,
127        q: A,
128        ctx: &Self::Context<'ctx>,
129    ) -> DekeResult<R>;
130    fn validate_motion<'ctx>(
131        &self,
132        qs: &[SRobotQ<N, F>],
133        ctx: &Self::Context<'ctx>,
134    ) -> DekeResult<R>;
135}
136
137#[derive(Debug, Clone)]
138pub struct ValidatorAnd<A, B>(pub A, pub B);
139
140#[derive(Debug, Clone)]
141pub struct ValidatorOr<A, B>(pub A, pub B);
142
143#[derive(Debug, Clone)]
144pub struct ValidatorNot<A>(pub A);
145
146impl<A, B> ValidatorAnd<A, B> {
147    /// Construct an AND combinator after a compile-time assertion that
148    /// `A` and `B` share the `Validator<N, R, F>` signature passed via
149    /// turbofish or inferred at the call site.
150    ///
151    /// Direct tuple-struct construction (`ValidatorAnd(a, b)`) skips this
152    /// check and is what the [`combine_validators!`] macro emits — both
153    /// forms produce the same value, and the trait impl below only fires
154    /// for `(N, R, F)` triples both members support, so callers that
155    /// dispatch through the trait are safe either way.
156    ///
157    /// [`combine_validators!`]: deke-cricket
158    pub fn new<const N: usize, R: ValidatorRet, F: FKScalar>(a: A, b: B) -> Self
159    where
160        A: Validator<N, R, F>,
161        B: Validator<N, R, F>,
162    {
163        Self(a, b)
164    }
165}
166
167impl<A, B> ValidatorOr<A, B> {
168    /// See [`ValidatorAnd::new`].
169    pub fn new<const N: usize, R: ValidatorRet, F: FKScalar>(a: A, b: B) -> Self
170    where
171        A: Validator<N, R, F>,
172        B: Validator<N, R, F>,
173    {
174        Self(a, b)
175    }
176}
177
178impl<A> ValidatorNot<A> {
179    /// Construct a NOT combinator after a compile-time assertion that `A`
180    /// implements `Validator<N, (), F>`. `Not` is restricted to the unit
181    /// return type because inverting a scalar score isn't well-defined.
182    pub fn new<const N: usize, F: FKScalar>(a: A) -> Self
183    where
184        A: Validator<N, (), F>,
185    {
186        Self(a)
187    }
188}
189
190/// Blanket impls below cover every `(N, R, F)` triple that **both** member
191/// validators implement: a single generic `impl` over `R` and `F` (and `N`
192/// since validators are const-generic over DOF) means monomorphization
193/// fires the impl for every shared signature without manual enumeration.
194impl<const N: usize, F: FKScalar, R: ValidatorRet, A, B> Validator<N, R, F>
195    for ValidatorAnd<A, B>
196where
197    A: Validator<N, R, F>,
198    B: Validator<N, R, F>,
199{
200    type Context<'ctx> = (A::Context<'ctx>, B::Context<'ctx>);
201
202    #[inline]
203    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, F>>(
204        &self,
205        q: Q,
206        ctx: &Self::Context<'ctx>,
207    ) -> DekeResult<R> {
208        let q = q.to_srobotq().map_err(Into::into)?;
209        self.0.validate(q, &ctx.0)?;
210        self.1.validate(q, &ctx.1)
211    }
212
213    #[inline]
214    fn validate_motion<'ctx>(
215        &self,
216        qs: &[SRobotQ<N, F>],
217        ctx: &Self::Context<'ctx>,
218    ) -> DekeResult<R> {
219        self.0.validate_motion(qs, &ctx.0)?;
220        self.1.validate_motion(qs, &ctx.1)
221    }
222}
223
224impl<const N: usize, F: FKScalar, R: ValidatorRet, A, B> Validator<N, R, F> for ValidatorOr<A, B>
225where
226    A: Validator<N, R, F>,
227    B: Validator<N, R, F>,
228{
229    type Context<'ctx> = (A::Context<'ctx>, B::Context<'ctx>);
230
231    #[inline]
232    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, F>>(
233        &self,
234        q: Q,
235        ctx: &Self::Context<'ctx>,
236    ) -> DekeResult<R> {
237        let q = q.to_srobotq().map_err(Into::into)?;
238        match self.0.validate(q, &ctx.0) {
239            Ok(r) => Ok(r),
240            Err(_) => self.1.validate(q, &ctx.1),
241        }
242    }
243
244    #[inline]
245    fn validate_motion<'ctx>(
246        &self,
247        qs: &[SRobotQ<N, F>],
248        ctx: &Self::Context<'ctx>,
249    ) -> DekeResult<R> {
250        match self.0.validate_motion(qs, &ctx.0) {
251            Ok(r) => Ok(r),
252            Err(_) => self.1.validate_motion(qs, &ctx.1),
253        }
254    }
255}
256
257/// `Not` is only meaningful for `R = ()` (a pass/fail validator). For
258/// scalar-returning validators the inversion of the return value isn't
259/// well-defined, so the impl is restricted to the unit case.
260impl<const N: usize, F: FKScalar, A> Validator<N, (), F> for ValidatorNot<A>
261where
262    A: Validator<N, (), F>,
263{
264    type Context<'ctx> = A::Context<'ctx>;
265
266    #[inline]
267    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, F>>(
268        &self,
269        q: Q,
270        ctx: &Self::Context<'ctx>,
271    ) -> DekeResult<()> {
272        let q = q.to_srobotq().map_err(Into::into)?;
273        match self.0.validate(q, ctx) {
274            Ok(()) => Err(DekeError::SuperError),
275            Err(_) => Ok(()),
276        }
277    }
278
279    #[inline]
280    fn validate_motion<'ctx>(
281        &self,
282        qs: &[SRobotQ<N, F>],
283        ctx: &Self::Context<'ctx>,
284    ) -> DekeResult<()> {
285        match self.0.validate_motion(qs, ctx) {
286            Ok(()) => Err(DekeError::SuperError),
287            Err(_) => Ok(()),
288        }
289    }
290}
291
292#[derive(Clone)]
293pub struct JointValidator<const N: usize, F: FKScalar = f32> {
294    lower: SRobotQ<N, F>,
295    upper: SRobotQ<N, F>,
296    extras: Option<Arc<[Box<dyn Fn(&SRobotQ<N, F>) -> bool + Send + Sync>]>>,
297}
298
299impl<const N: usize, F: FKScalar> Debug for JointValidator<N, F> {
300    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
301        f.debug_struct("JointValidator")
302            .field("lower", &self.lower)
303            .field("upper", &self.upper)
304            .field(
305                "extras",
306                &format!(
307                    "[{} extra checks]",
308                    self.extras.as_ref().map(|e| e.len()).unwrap_or(0)
309                ),
310            )
311            .finish()
312    }
313}
314
315impl<const N: usize, F: FKScalar> JointValidator<N, F> {
316    pub fn new(lower: SRobotQ<N, F>, upper: SRobotQ<N, F>) -> Self {
317        Self {
318            lower,
319            upper,
320            extras: None,
321        }
322    }
323
324    pub fn new_with_extras(
325        lower: SRobotQ<N, F>,
326        upper: SRobotQ<N, F>,
327        extras: Vec<Box<dyn Fn(&SRobotQ<N, F>) -> bool + Send + Sync>>,
328    ) -> Self {
329        Self {
330            lower,
331            upper,
332            extras: Some(extras.into()),
333        }
334    }
335}
336
337impl<const N: usize, F: FKScalar> Validator<N, (), F> for JointValidator<N, F> {
338    type Context<'ctx> = ();
339
340    #[inline]
341    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, F>>(
342        &self,
343        q: Q,
344        _ctx: &Self::Context<'ctx>,
345    ) -> DekeResult<()> {
346        let q = q.to_srobotq().map_err(Into::into)?;
347        if q.any_lt(&self.lower) || q.any_gt(&self.upper) {
348            return Err(DekeError::ExceedJointLimits);
349        }
350        if let Some(extras) = &self.extras {
351            for check in extras.iter() {
352                if !check(&q) {
353                    return Err(DekeError::ExceedJointLimits);
354                }
355            }
356        }
357        Ok(())
358    }
359
360    #[inline]
361    fn validate_motion<'ctx>(
362        &self,
363        qs: &[SRobotQ<N, F>],
364        _ctx: &Self::Context<'ctx>,
365    ) -> DekeResult<()> {
366        for q in qs {
367            self.validate(*q, _ctx)?;
368        }
369        Ok(())
370    }
371}
372
373/// Cross-precision entry point: f32-storage `JointValidator` accepting f64
374/// inputs. The input is narrowed to f32 at the boundary so the same limits
375/// govern both precisions; comparison is done in storage precision.
376impl<const N: usize> Validator<N, (), f64> for JointValidator<N, f32> {
377    type Context<'ctx> = ();
378
379    #[inline]
380    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, f64>>(
381        &self,
382        q: Q,
383        ctx: &Self::Context<'ctx>,
384    ) -> DekeResult<()> {
385        let q64 = q.to_srobotq().map_err(Into::into)?;
386        let q32: SRobotQ<N, f32> = q64.into();
387        <Self as Validator<N, (), f32>>::validate(self, q32, ctx)
388    }
389
390    #[inline]
391    fn validate_motion<'ctx>(
392        &self,
393        qs: &[SRobotQ<N, f64>],
394        ctx: &Self::Context<'ctx>,
395    ) -> DekeResult<()> {
396        for q in qs {
397            let q32: SRobotQ<N, f32> = (*q).into();
398            <Self as Validator<N, (), f32>>::validate(self, q32, ctx)?;
399        }
400        Ok(())
401    }
402}
403
404/// Cross-precision entry point: f64-storage `JointValidator` accepting f32
405/// inputs. The f32 input is widened to f64 (lossless) before comparison.
406impl<const N: usize> Validator<N, (), f32> for JointValidator<N, f64> {
407    type Context<'ctx> = ();
408
409    #[inline]
410    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, f32>>(
411        &self,
412        q: Q,
413        ctx: &Self::Context<'ctx>,
414    ) -> DekeResult<()> {
415        let q32 = q.to_srobotq().map_err(Into::into)?;
416        let q64: SRobotQ<N, f64> = q32.into();
417        <Self as Validator<N, (), f64>>::validate(self, q64, ctx)
418    }
419
420    #[inline]
421    fn validate_motion<'ctx>(
422        &self,
423        qs: &[SRobotQ<N, f32>],
424        ctx: &Self::Context<'ctx>,
425    ) -> DekeResult<()> {
426        for q in qs {
427            let q64: SRobotQ<N, f64> = (*q).into();
428            <Self as Validator<N, (), f64>>::validate(self, q64, ctx)?;
429        }
430        Ok(())
431    }
432}
deke_types/validator.rs

deke_types/
validator.rs
