dsmc 0.1.6

Digitalservo Motion Control Library
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119

//! Arithmetic on `TransferFunction`: `G1 * G2`, `G1 + G2`, and constant gains
//! (`G * k`, `k * G`, `G / k`, `G *= k`, `G /= k`).
//! Both operands must share the same domain `D`, so `s` and `z` cannot be mixed.

use super::TransferFunction;
use num_traits::Float;
use std::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign};

// ---- Transfer function × transfer function -------------------------------------------------

impl<T: Float + AddAssign, D> Mul for &TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn mul(self, rhs: &TransferFunction<T, D>) -> TransferFunction<T, D> {
        TransferFunction::from_polynomials(
            &self.numerator * &rhs.numerator,
            &self.denominator * &rhs.denominator,
        )
        .reduced()
    }
}

impl<T: Float + AddAssign, D> Mul for TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn mul(self, rhs: TransferFunction<T, D>) -> TransferFunction<T, D> {
        &self * &rhs
    }
}

impl<T: Float + AddAssign, D> Add for &TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn add(self, rhs: &TransferFunction<T, D>) -> TransferFunction<T, D> {
        // n1/d1 + n2/d2 = (n1*d2 + n2*d1) / (d1*d2); `reduced()` then cancels any factor
        // shared by d1 and d2 (and any other common numerator/denominator roots), which is
        // equivalent to reducing to a common denominator first.
        let n1d2 = &self.numerator * &rhs.denominator;
        let n2d1 = &self.denominator * &rhs.numerator;
        let numerator = &n1d2 + &n2d1;
        let denominator = &self.denominator * &rhs.denominator;
        TransferFunction::from_polynomials(numerator, denominator).reduced()
    }
}

impl<T: Float + AddAssign, D> Add for TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn add(self, rhs: TransferFunction<T, D>) -> TransferFunction<T, D> {
        &self + &rhs
    }
}

// ---- Constant gain -------------------------------------------------------------------------
// Only the numerator is scaled. Poles/zeros are unchanged, so no `reduced()`.
// `*=` / `/=` do the work; `*` / `/` are built on them. (`&G` is copied with `from_polynomials`
// rather than `clone()`, since the derived `Clone` would require `D: Clone`.)

impl<T: Float, D> MulAssign<T> for TransferFunction<T, D> {
    fn mul_assign(&mut self, k: T) {
        self.numerator.iter_mut().for_each(|c| *c = *c * k);
    }
}

impl<T: Float, D> DivAssign<T> for TransferFunction<T, D> {
    fn div_assign(&mut self, k: T) {
        self.numerator.iter_mut().for_each(|c| *c = *c / k);
    }
}

impl<T: Float, D> Mul<T> for TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn mul(mut self, k: T) -> TransferFunction<T, D> {
        self *= k;
        self
    }
}

impl<T: Float, D> Mul<T> for &TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn mul(self, k: T) -> TransferFunction<T, D> {
        TransferFunction::from_polynomials(self.numerator.clone(), self.denominator.clone()) * k
    }
}

impl<T: Float, D> Div<T> for TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn div(mut self, k: T) -> TransferFunction<T, D> {
        self /= k;
        self
    }
}

impl<T: Float, D> Div<T> for &TransferFunction<T, D> {
    type Output = TransferFunction<T, D>;
    fn div(self, k: T) -> TransferFunction<T, D> {
        TransferFunction::from_polynomials(self.numerator.clone(), self.denominator.clone()) / k
    }
}

// `k * G`: the orphan rule forbids `impl<T> Mul<TransferFunction<T, D>> for T`, so the
// left-hand scalar is implemented per concrete float type. Only f64: with both f32 and f64,
// `10.0 * &g` fails to infer whenever `g`'s `T` is still an unresolved `{float}` (e.g. built by
// `tf!` or from literal slices). For f32, use `g * k`.
macro_rules! impl_scalar_lhs_mul {
    ($($t:ty),*) => {$(
        impl<D> Mul<TransferFunction<$t, D>> for $t {
            type Output = TransferFunction<$t, D>;
            fn mul(self, tf: TransferFunction<$t, D>) -> TransferFunction<$t, D> {
                tf * self
            }
        }

        impl<D> Mul<&TransferFunction<$t, D>> for $t {
            type Output = TransferFunction<$t, D>;
            fn mul(self, tf: &TransferFunction<$t, D>) -> TransferFunction<$t, D> {
                tf * self
            }
        }
    )*};
}

impl_scalar_lhs_mul!(f64);