vqm 0.1.9

A vector, quaternion, and matrix library targeted at embedded systems and robotics.
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
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145

#![allow(clippy::inline_always)]
#![allow(unused)]

use crate::Matrix9x9;

// **** Math ****

/// Math functions for Matrix9x9.<br><br>
pub trait Matrix9x9Math: Sized {
    fn m9x9_neg(this: Matrix9x9<Self>) -> Matrix9x9<Self>;
    fn m9x9_abs(this: Matrix9x9<Self>) -> Matrix9x9<Self>;
    fn m9x9_add(this: Matrix9x9<Self>, this: Matrix9x9<Self>) -> Matrix9x9<Self>;
    fn m9x9_mul_scalar(this: Matrix9x9<Self>, other: Self) -> Matrix9x9<Self>;
    fn m9x9_div_scalar(this: Matrix9x9<Self>, other: Self) -> Matrix9x9<Self>;
    fn m9x9_mul_add(this: Matrix9x9<Self>, k: Self, other: Matrix9x9<Self>) -> Matrix9x9<Self>;
    fn m9x9_trace(this: Matrix9x9<Self>) -> Self;
    fn m9x9_trace_sum_squares(this: Matrix9x9<Self>) -> Self;
    fn m9x9_sum(this: Matrix9x9<Self>) -> Self;
    fn m9x9_mean(this: Matrix9x9<Self>) -> Self;
    fn m9x9_product(this: Matrix9x9<Self>) -> Self;
}

impl Matrix9x9Math for f32 {
    #[inline(always)]
    fn m9x9_neg(this: Matrix9x9<Self>) -> Matrix9x9<Self> {
        let ret = core::array::from_fn(|ii| -this.a[ii]);
        Matrix9x9::from(ret)
    }

    #[inline(always)]
    fn m9x9_abs(this: Matrix9x9<Self>) -> Matrix9x9<Self> {
        let ret = core::array::from_fn(|ii| this.a[ii].abs());
        Matrix9x9::from(ret)
    }

    #[inline(always)]
    fn m9x9_add(this: Matrix9x9<Self>, other: Matrix9x9<Self>) -> Matrix9x9<Self> {
        let ret = core::array::from_fn(|ii| this.a[ii] + other.a[ii]);
        Matrix9x9::from(ret)
    }

    #[inline(always)]
    fn m9x9_mul_scalar(this: Matrix9x9<Self>, other: Self) -> Matrix9x9<Self> {
        let ret = core::array::from_fn(|ii| this.a[ii] * other);
        Matrix9x9::from(ret)
    }

    #[inline(always)]
    fn m9x9_div_scalar(this: Matrix9x9<Self>, other: Self) -> Matrix9x9<Self> {
        Self::m9x9_mul_scalar(this, 1.0 / other)
    }

    #[inline(always)]
    fn m9x9_mul_add(this: Matrix9x9<Self>, k: Self, other: Matrix9x9<Self>) -> Matrix9x9<Self> {
        Self::m9x9_add(Self::m9x9_mul_scalar(this, k), other)
    }

    #[inline(always)]
    fn m9x9_trace(this: Matrix9x9<Self>) -> Self {
        this.a[0] + this.a[5] + this.a[10] + this.a[15]
    }

    #[inline(always)]
    fn m9x9_trace_sum_squares(this: Matrix9x9<Self>) -> Self {
        this.a[0] * this.a[0] + this.a[5] * this.a[5] + this.a[10] * this.a[10] + this.a[15] * this.a[15]
    }

    #[inline(always)]
    fn m9x9_sum(this: Matrix9x9<Self>) -> Self {
        this.a.iter().sum()
    }

    #[inline(always)]
    fn m9x9_mean(this: Matrix9x9<Self>) -> Self {
        Self::m9x9_sum(this) / 16.0
    }

    #[inline(always)]
    fn m9x9_product(this: Matrix9x9<Self>) -> Self {
        this.a.iter().product()
    }
}

// **** f64 ****

impl Matrix9x9Math for f64 {
    #[inline(always)]
    fn m9x9_neg(this: Matrix9x9<Self>) -> Matrix9x9<Self> {
        let ret = core::array::from_fn(|ii| -this.a[ii]);
        Matrix9x9::from(ret)
    }

    #[inline(always)]
    fn m9x9_abs(this: Matrix9x9<Self>) -> Matrix9x9<Self> {
        let ret = core::array::from_fn(|ii| this.a[ii].abs());
        Matrix9x9::from(ret)
    }

    #[inline(always)]
    fn m9x9_add(this: Matrix9x9<Self>, other: Matrix9x9<Self>) -> Matrix9x9<Self> {
        let ret = core::array::from_fn(|ii| this.a[ii] + other.a[ii]);
        Matrix9x9::from(ret)
    }

    #[inline(always)]
    fn m9x9_mul_scalar(this: Matrix9x9<Self>, other: Self) -> Matrix9x9<Self> {
        let ret = core::array::from_fn(|ii| this.a[ii] * other);
        Matrix9x9::from(ret)
    }

    #[inline(always)]
    fn m9x9_div_scalar(this: Matrix9x9<Self>, other: Self) -> Matrix9x9<Self> {
        Self::m9x9_mul_scalar(this, 1.0 / other)
    }

    #[inline(always)]
    fn m9x9_mul_add(this: Matrix9x9<Self>, k: Self, other: Matrix9x9<Self>) -> Matrix9x9<Self> {
        Self::m9x9_add(Self::m9x9_mul_scalar(this, k), other)
    }

    #[inline(always)]
    fn m9x9_trace(this: Matrix9x9<Self>) -> Self {
        this.a[0] + this.a[5] + this.a[10] + this.a[15]
    }

    #[inline(always)]
    fn m9x9_trace_sum_squares(this: Matrix9x9<Self>) -> Self {
        { this.a[0] * this.a[0] + this.a[5] * this.a[5] + this.a[10] * this.a[10] + this.a[15] * this.a[15] }
    }

    #[inline(always)]
    fn m9x9_sum(this: Matrix9x9<Self>) -> Self {
        this.a.iter().sum()
    }

    #[inline(always)]
    fn m9x9_mean(this: Matrix9x9<Self>) -> Self {
        Self::m9x9_sum(this) / 16.0
    }

    #[inline(always)]
    fn m9x9_product(this: Matrix9x9<Self>) -> Self {
        this.a.iter().product()
    }
}