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    /// Maximal-margin value used when a validator is disabled (no
97    /// constraint applied). Mirrors the `as_f64() == INFINITY` convention
98    /// for the unit return type.
99    fn passing() -> Self;
100}
101
102impl sealed::Sealed for () {}
103impl ValidatorRet for () {
104    #[inline]
105    fn as_f64(&self) -> f64 {
106        f64::INFINITY
107    }
108    #[inline]
109    fn passing() -> Self {}
110}
111
112impl sealed::Sealed for f32 {}
113impl ValidatorRet for f32 {
114    #[inline]
115    fn as_f64(&self) -> f64 {
116        *self as f64
117    }
118    #[inline]
119    fn passing() -> Self {
120        f32::INFINITY
121    }
122}
123
124impl sealed::Sealed for f64 {}
125impl ValidatorRet for f64 {
126    #[inline]
127    fn as_f64(&self) -> f64 {
128        *self
129    }
130    #[inline]
131    fn passing() -> Self {
132        f64::INFINITY
133    }
134}
135
136pub trait Validator<const N: usize, R: ValidatorRet = (), F: FKScalar = f32>: Sized + Clone + Debug + Send + Sync + 'static {
137    type Context<'ctx>: ValidatorContext;
138    const VALIDATE_MOTION_IS_CONTINUOUS: bool = false;
139
140    fn validate<'ctx, E: Into<DekeError>, A: SRobotQLike<N, E, F>>(
141        &self,
142        q: A,
143        ctx: &Self::Context<'ctx>,
144    ) -> DekeResult<R>;
145    fn validate_motion<'ctx>(
146        &self,
147        qs: &[SRobotQ<N, F>],
148        ctx: &Self::Context<'ctx>,
149    ) -> DekeResult<R>;
150}
151
152#[derive(Debug, Clone)]
153pub struct ValidatorAnd<A, B>(pub A, pub B);
154
155#[derive(Debug, Clone)]
156pub struct ValidatorOr<A, B>(pub A, pub B);
157
158#[derive(Debug, Clone)]
159pub struct ValidatorNot<A>(pub A);
160
161impl<A, B> ValidatorAnd<A, B> {
162    /// Construct an AND combinator after a compile-time assertion that
163    /// `A` and `B` share the `Validator<N, R, F>` signature passed via
164    /// turbofish or inferred at the call site.
165    ///
166    /// Direct tuple-struct construction (`ValidatorAnd(a, b)`) skips this
167    /// check and is what the [`combine_validators!`] macro emits — both
168    /// forms produce the same value, and the trait impl below only fires
169    /// for `(N, R, F)` triples both members support, so callers that
170    /// dispatch through the trait are safe either way.
171    ///
172    /// [`combine_validators!`]: deke-cricket
173    pub fn new<const N: usize, R: ValidatorRet, F: FKScalar>(a: A, b: B) -> Self
174    where
175        A: Validator<N, R, F>,
176        B: Validator<N, R, F>,
177    {
178        Self(a, b)
179    }
180}
181
182impl<A, B> ValidatorOr<A, B> {
183    /// See [`ValidatorAnd::new`].
184    pub fn new<const N: usize, R: ValidatorRet, F: FKScalar>(a: A, b: B) -> Self
185    where
186        A: Validator<N, R, F>,
187        B: Validator<N, R, F>,
188    {
189        Self(a, b)
190    }
191}
192
193impl<A> ValidatorNot<A> {
194    /// Construct a NOT combinator after a compile-time assertion that `A`
195    /// implements `Validator<N, (), F>`. `Not` is restricted to the unit
196    /// return type because inverting a scalar score isn't well-defined.
197    pub fn new<const N: usize, F: FKScalar>(a: A) -> Self
198    where
199        A: Validator<N, (), F>,
200    {
201        Self(a)
202    }
203}
204
205/// Blanket impls below cover every `(N, R, F)` triple that **both** member
206/// validators implement: a single generic `impl` over `R` and `F` (and `N`
207/// since validators are const-generic over DOF) means monomorphization
208/// fires the impl for every shared signature without manual enumeration.
209impl<const N: usize, F: FKScalar, R: ValidatorRet, A, B> Validator<N, R, F>
210    for ValidatorAnd<A, B>
211where
212    A: Validator<N, R, F>,
213    B: Validator<N, R, F>,
214{
215    type Context<'ctx> = (A::Context<'ctx>, B::Context<'ctx>);
216
217    #[inline]
218    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, F>>(
219        &self,
220        q: Q,
221        ctx: &Self::Context<'ctx>,
222    ) -> DekeResult<R> {
223        let q = q.to_srobotq().map_err(Into::into)?;
224        self.0.validate(q, &ctx.0)?;
225        self.1.validate(q, &ctx.1)
226    }
227
228    #[inline]
229    fn validate_motion<'ctx>(
230        &self,
231        qs: &[SRobotQ<N, F>],
232        ctx: &Self::Context<'ctx>,
233    ) -> DekeResult<R> {
234        self.0.validate_motion(qs, &ctx.0)?;
235        self.1.validate_motion(qs, &ctx.1)
236    }
237}
238
239impl<const N: usize, F: FKScalar, R: ValidatorRet, A, B> Validator<N, R, F> for ValidatorOr<A, B>
240where
241    A: Validator<N, R, F>,
242    B: Validator<N, R, F>,
243{
244    type Context<'ctx> = (A::Context<'ctx>, B::Context<'ctx>);
245
246    #[inline]
247    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, F>>(
248        &self,
249        q: Q,
250        ctx: &Self::Context<'ctx>,
251    ) -> DekeResult<R> {
252        let q = q.to_srobotq().map_err(Into::into)?;
253        match self.0.validate(q, &ctx.0) {
254            Ok(r) => Ok(r),
255            Err(_) => self.1.validate(q, &ctx.1),
256        }
257    }
258
259    #[inline]
260    fn validate_motion<'ctx>(
261        &self,
262        qs: &[SRobotQ<N, F>],
263        ctx: &Self::Context<'ctx>,
264    ) -> DekeResult<R> {
265        match self.0.validate_motion(qs, &ctx.0) {
266            Ok(r) => Ok(r),
267            Err(_) => self.1.validate_motion(qs, &ctx.1),
268        }
269    }
270}
271
272/// `Not` is only meaningful for `R = ()` (a pass/fail validator). For
273/// scalar-returning validators the inversion of the return value isn't
274/// well-defined, so the impl is restricted to the unit case.
275impl<const N: usize, F: FKScalar, A> Validator<N, (), F> for ValidatorNot<A>
276where
277    A: Validator<N, (), F>,
278{
279    type Context<'ctx> = A::Context<'ctx>;
280
281    #[inline]
282    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, F>>(
283        &self,
284        q: Q,
285        ctx: &Self::Context<'ctx>,
286    ) -> DekeResult<()> {
287        let q = q.to_srobotq().map_err(Into::into)?;
288        match self.0.validate(q, ctx) {
289            Ok(()) => Err(DekeError::SuperError),
290            Err(_) => Ok(()),
291        }
292    }
293
294    #[inline]
295    fn validate_motion<'ctx>(
296        &self,
297        qs: &[SRobotQ<N, F>],
298        ctx: &Self::Context<'ctx>,
299    ) -> DekeResult<()> {
300        match self.0.validate_motion(qs, ctx) {
301            Ok(()) => Err(DekeError::SuperError),
302            Err(_) => Ok(()),
303        }
304    }
305}
306
307#[derive(Debug, Clone)]
308pub enum MaybeValidator<V> {
309    Active(V),
310    Disabled,
311}
312
313impl<const N: usize, F: FKScalar, R: ValidatorRet, V> Validator<N, R, F> for MaybeValidator<V>
314where
315    V: Validator<N, R, F>,
316{
317    type Context<'ctx> = V::Context<'ctx>;
318
319    #[inline]
320    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, F>>(
321        &self,
322        q: Q,
323        ctx: &Self::Context<'ctx>,
324    ) -> DekeResult<R> {
325        match self {
326            MaybeValidator::Active(v) => v.validate(q, ctx),
327            MaybeValidator::Disabled => Ok(R::passing()),
328        }
329    }
330
331    #[inline]
332    fn validate_motion<'ctx>(
333        &self,
334        qs: &[SRobotQ<N, F>],
335        ctx: &Self::Context<'ctx>,
336    ) -> DekeResult<R> {
337        match self {
338            MaybeValidator::Active(v) => v.validate_motion(qs, ctx),
339            MaybeValidator::Disabled => Ok(R::passing()),
340        }
341    }
342}
343
344#[derive(Clone)]
345pub struct JointValidator<const N: usize, F: FKScalar = f32> {
346    lower: SRobotQ<N, F>,
347    upper: SRobotQ<N, F>,
348    extras: Option<Arc<[Box<dyn Fn(&SRobotQ<N, F>) -> bool + Send + Sync>]>>,
349}
350
351impl<const N: usize, F: FKScalar> Debug for JointValidator<N, F> {
352    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
353        f.debug_struct("JointValidator")
354            .field("lower", &self.lower)
355            .field("upper", &self.upper)
356            .field(
357                "extras",
358                &format!(
359                    "[{} extra checks]",
360                    self.extras.as_ref().map(|e| e.len()).unwrap_or(0)
361                ),
362            )
363            .finish()
364    }
365}
366
367impl<const N: usize, F: FKScalar> JointValidator<N, F> {
368    pub fn new(lower: SRobotQ<N, F>, upper: SRobotQ<N, F>) -> Self {
369        Self {
370            lower,
371            upper,
372            extras: None,
373        }
374    }
375
376    pub fn new_with_extras(
377        lower: SRobotQ<N, F>,
378        upper: SRobotQ<N, F>,
379        extras: Vec<Box<dyn Fn(&SRobotQ<N, F>) -> bool + Send + Sync>>,
380    ) -> Self {
381        Self {
382            lower,
383            upper,
384            extras: Some(extras.into()),
385        }
386    }
387}
388
389impl<const N: usize, F: FKScalar> Validator<N, (), F> for JointValidator<N, F> {
390    type Context<'ctx> = ();
391
392    #[inline]
393    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, F>>(
394        &self,
395        q: Q,
396        _ctx: &Self::Context<'ctx>,
397    ) -> DekeResult<()> {
398        let q = q.to_srobotq().map_err(Into::into)?;
399        if q.any_lt(&self.lower) || q.any_gt(&self.upper) {
400            return Err(DekeError::ExceedJointLimits);
401        }
402        if let Some(extras) = &self.extras {
403            for check in extras.iter() {
404                if !check(&q) {
405                    return Err(DekeError::ExceedJointLimits);
406                }
407            }
408        }
409        Ok(())
410    }
411
412    #[inline]
413    fn validate_motion<'ctx>(
414        &self,
415        qs: &[SRobotQ<N, F>],
416        _ctx: &Self::Context<'ctx>,
417    ) -> DekeResult<()> {
418        for q in qs {
419            self.validate(*q, _ctx)?;
420        }
421        Ok(())
422    }
423}
424
425/// Cross-precision entry point: f32-storage `JointValidator` accepting f64
426/// inputs. The input is narrowed to f32 at the boundary so the same limits
427/// govern both precisions; comparison is done in storage precision.
428impl<const N: usize> Validator<N, (), f64> for JointValidator<N, f32> {
429    type Context<'ctx> = ();
430
431    #[inline]
432    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, f64>>(
433        &self,
434        q: Q,
435        ctx: &Self::Context<'ctx>,
436    ) -> DekeResult<()> {
437        let q64 = q.to_srobotq().map_err(Into::into)?;
438        let q32: SRobotQ<N, f32> = q64.into();
439        <Self as Validator<N, (), f32>>::validate(self, q32, ctx)
440    }
441
442    #[inline]
443    fn validate_motion<'ctx>(
444        &self,
445        qs: &[SRobotQ<N, f64>],
446        ctx: &Self::Context<'ctx>,
447    ) -> DekeResult<()> {
448        for q in qs {
449            let q32: SRobotQ<N, f32> = (*q).into();
450            <Self as Validator<N, (), f32>>::validate(self, q32, ctx)?;
451        }
452        Ok(())
453    }
454}
455
456/// Cross-precision entry point: f64-storage `JointValidator` accepting f32
457/// inputs. The f32 input is widened to f64 (lossless) before comparison.
458impl<const N: usize> Validator<N, (), f32> for JointValidator<N, f64> {
459    type Context<'ctx> = ();
460
461    #[inline]
462    fn validate<'ctx, E: Into<DekeError>, Q: SRobotQLike<N, E, f32>>(
463        &self,
464        q: Q,
465        ctx: &Self::Context<'ctx>,
466    ) -> DekeResult<()> {
467        let q32 = q.to_srobotq().map_err(Into::into)?;
468        let q64: SRobotQ<N, f64> = q32.into();
469        <Self as Validator<N, (), f64>>::validate(self, q64, ctx)
470    }
471
472    #[inline]
473    fn validate_motion<'ctx>(
474        &self,
475        qs: &[SRobotQ<N, f32>],
476        ctx: &Self::Context<'ctx>,
477    ) -> DekeResult<()> {
478        for q in qs {
479            let q64: SRobotQ<N, f64> = (*q).into();
480            <Self as Validator<N, (), f64>>::validate(self, q64, ctx)?;
481        }
482        Ok(())
483    }
484}
deke_types/validator.rs

deke_types/
validator.rs
