multicalc 0.9.0

Math for real-time embedded systems, in stable no_std Rust: state estimation, control, kinematics, Lie groups, autodiff, and linear algebra — from 64-bit servers to bare-metal microcontrollers
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
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175

use multicalc::linear_algebra::{Matrix, Vector};

// ----- construction & access -----

#[test]
fn construct_and_access() {
    let vector = Vector::new([1.0, 2.0, 3.0]);
    assert_eq!(vector.get(0), Some(&1.0));
    assert_eq!(vector.into_array(), [1.0, 2.0, 3.0]);

    let mut mutable = Vector::from([4.0, 5.0]);
    if let Some(entry) = mutable.get_mut(1) {
        *entry = 9.0;
    }
    assert_eq!(mutable, Vector::new([4.0, 9.0]));

    let zeros: Vector<3> = Vector::zeros();
    assert_eq!(zeros, Vector::new([0.0, 0.0, 0.0]));

    assert_eq!(
        Vector::<4>::from_fn(|index| index as f64),
        Vector::new([0.0, 1.0, 2.0, 3.0])
    );

    let mut matrix = Matrix::new([[1.0, 2.0], [3.0, 4.0]]);
    assert_eq!(matrix.get(1, 0), Some(&3.0));
    if let Some(entry) = matrix.get_mut(0, 1) {
        *entry = 7.0;
    }
    assert_eq!(matrix.get(0, 1), Some(&7.0));

    let identity: Matrix<3, 3> = Matrix::identity();
    assert_eq!(
        identity.into_array(),
        [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
    );

    assert_eq!(
        Matrix::<2, 2>::from_fn(|row, column| (row * 2 + column) as f64),
        Matrix::new([[0.0, 1.0], [2.0, 3.0]])
    );
}

#[test]
fn try_from_slice_length() {
    assert_eq!(
        Vector::<3>::try_from_slice(&[1.0, 2.0, 3.0]),
        Some(Vector::new([1.0, 2.0, 3.0]))
    );
    assert!(Vector::<3>::try_from_slice(&[1.0, 2.0]).is_none());

    assert_eq!(
        Matrix::<2, 2>::try_from_row_slice(&[1.0, 2.0, 3.0, 4.0]),
        Some(Matrix::new([[1.0, 2.0], [3.0, 4.0]]))
    );
    assert!(Matrix::<2, 2>::try_from_row_slice(&[1.0, 2.0, 3.0]).is_none());
}

#[test]
fn get_checked_access() {
    let vector = Vector::new([1.0, 2.0, 3.0]);
    assert_eq!(vector.get(0), Some(&1.0));
    assert_eq!(vector.get(3), None);

    let mut mutable = Vector::new([4.0, 5.0]);
    if let Some(entry) = mutable.get_mut(1) {
        *entry = 9.0;
    }
    assert_eq!(mutable.get(1), Some(&9.0));

    let mut matrix = Matrix::new([[1.0, 2.0], [3.0, 4.0]]);
    assert_eq!(matrix.get(1, 0), Some(&3.0));
    assert_eq!(matrix.get(2, 0), None);
    assert_eq!(matrix.get(0, 2), None);
    if let Some(entry) = matrix.get_mut(0, 1) {
        *entry = 7.0;
    }
    assert_eq!(matrix.get(0, 1), Some(&7.0));

    matrix.as_mut_slice_rows()[1][1] = 8.0;
    assert_eq!(matrix.get(1, 1), Some(&8.0));
}

// ----- vector arithmetic -----

#[test]
fn vector_arithmetic() {
    let left = Vector::new([1.0, 2.0, 3.0]);
    let right = Vector::new([4.0, 5.0, 6.0]);

    assert_eq!(left + right, Vector::new([5.0, 7.0, 9.0]));
    assert_eq!(right - left, Vector::new([3.0, 3.0, 3.0]));
    assert_eq!(-left, Vector::new([-1.0, -2.0, -3.0]));
    assert_eq!(left * 2.0, left.scale(2.0));
    assert_eq!(left.scale(2.0), Vector::new([2.0, 4.0, 6.0]));

    let mut accumulated = left;
    accumulated += right;
    assert_eq!(accumulated, left + right);
    accumulated -= right;
    assert_eq!(accumulated, left);
}

#[test]
fn vector_dot_and_norm() {
    let left: Vector<3> = Vector::new([1.0, 2.0, 3.0]);
    let right: Vector<3> = Vector::new([4.0, 5.0, 6.0]);
    assert_eq!(left.dot(right), 32.0);
    assert!((left.dot(right) - right.dot(left)).abs() < 1e-12); // symmetry
    assert_eq!(Vector::new([1.0, 0.0]).dot(Vector::new([0.0, 1.0])), 0.0); // orthogonal

    let empty: Vector<0> = Vector::zeros();
    assert_eq!(empty.dot(empty), 0.0);

    let vector: Vector<2> = Vector::new([3.0, 4.0]);
    assert_eq!(vector.norm(), 5.0);
    assert!((vector.norm_squared() - vector.norm() * vector.norm()).abs() < 1e-12);

    let zeros: Vector<3> = Vector::zeros();
    assert_eq!(zeros.norm(), 0.0);
    assert!(Vector::new([f64::INFINITY, 0.0]).norm().is_infinite());
}

#[test]
fn vector_is_finite() {
    assert!(Vector::new([1.0, -2.0, 3.0]).is_finite());
    assert!(Vector::<0>::zeros().is_finite()); // vacuously true
    assert!(!Vector::new([1.0, f64::NAN]).is_finite());
    assert!(!Vector::new([f64::INFINITY, 0.0]).is_finite());
    assert!(!Vector::new([0.0, f64::NEG_INFINITY]).is_finite());
}

// ----- cross products & scalar triple -----

#[test]
fn vector_cross_3d() {
    let unit_x = Vector::new([1.0, 0.0, 0.0]);
    let unit_y = Vector::new([0.0, 1.0, 0.0]);
    let unit_z = Vector::new([0.0, 0.0, 1.0]);
    assert_eq!(unit_x.cross(unit_y), unit_z);
    assert_eq!(unit_y.cross(unit_z), unit_x);
    assert_eq!(unit_z.cross(unit_x), unit_y);
    assert_eq!(unit_x.cross(unit_y), -(unit_y.cross(unit_x))); // anti-commutativity

    let left = Vector::new([1.0, 2.0, 3.0]);
    let right = Vector::new([4.0, 5.0, 6.0]);
    let cross_product = left.cross(right);
    assert_eq!(left.dot(cross_product), 0.0); // orthogonal to both inputs
    assert_eq!(right.dot(cross_product), 0.0);
}

#[test]
fn vector_cross_2d_and_scalar_triple() {
    assert_eq!(Vector::new([1.0, 0.0]).cross(Vector::new([0.0, 1.0])), 1.0);
    let left: Vector<2> = Vector::new([2.0, 3.0]);
    let right: Vector<2> = Vector::new([5.0, 7.0]);
    assert!((left.cross(right) + right.cross(left)).abs() < 1e-12); // anti-commutativity
    assert_eq!(left.cross(left), 0.0); // parallel

    let unit_x = Vector::new([1.0, 0.0, 0.0]);
    let unit_y = Vector::new([0.0, 1.0, 0.0]);
    let unit_z = Vector::new([0.0, 0.0, 1.0]);
    assert_eq!(unit_x.scalar_triple(unit_y, unit_z), 1.0);

    let first: Vector<3> = Vector::new([1.0, 2.0, 3.0]);
    let second: Vector<3> = Vector::new([0.0, 1.0, 4.0]);
    let third: Vector<3> = Vector::new([5.0, 6.0, 0.0]);
    // cyclic
    assert!(
        (first.scalar_triple(second, third) - second.scalar_triple(third, first)).abs() < 1e-12
    );
    let rows_as_matrix = Matrix::new([first.into_array(), second.into_array(), third.into_array()]);
    // equals the determinant of the matrix whose rows are the three vectors
    assert!((first.scalar_triple(second, third) - rows_as_matrix.determinant()).abs() < 1e-12);
}