f3l_core 0.3.0

3D Point Cloud 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

use crate::BasicFloat;

/// Compute `Row echelon form` for any  matrix.
pub fn rref<T: BasicFloat, const R: usize, const C: usize, M: Into<[[T; C]; R]>>(matrix: M) -> M
where
    [[T; C]; R]: Into<M>,
{
    let mut matrix: [[T; C]; R] = matrix.into();
    let mut lead = 0_usize;

    (0..R).for_each(|r| {
        if lead >= C {
            return;
        }
        let mut i = r;
        while matrix[i][lead] == T::zero() {
            i += 1;
            if i == R {
                i = r;
                lead += 1;
                if lead == C {
                    return;
                }
            }
        }
        (matrix[i], matrix[r]) = (matrix[r], matrix[i]);
        let lv = matrix[r][lead];
        (0..C).for_each(|i| {
            matrix[r][i] /= lv;
        });

        (0..R).filter(|&i| i != r).for_each(|i| {
            let lv = matrix[i][lead];
            (0..C).for_each(|ii| {
                matrix[i][ii] -= lv * matrix[r][ii];
            });
        });
        lead += 1;
    });
    matrix.into()
}

#[cfg(test)]
mod test_rref {
    use super::*;

    #[test]
    fn mat33() {
        let matrix = [
            [1f32, 2., 0., 1., 0., 0.],
            [0., 0., 0., 3., 0., 0.],
            [0., 0., 1., 0., 1., 0.],
        ];
        let src = [
            [1f32, 2.0, 0.0, 0.0, 0.0, 0.0],
            [0.0, 0.0, 1.0, 0.0, 1.0, 0.0],
            [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
        ];

        let target = rref(matrix);
        assert_eq!(src, target);
    }
}