imu_fusion/
fusion_matrix_impl.rs

1use core::ops;
2use crate::{FusionMatrix, FusionQuaternion, FusionVector};
3
4impl FusionMatrix {
5    pub fn new(xx: f32, xy: f32, xz: f32, yx: f32, yy: f32, yz: f32, zx: f32, zy: f32, zz: f32) -> Self {
6        Self {
7            xx,
8            xy,
9            xz,
10            yx,
11            yy,
12            yz,
13            zx,
14            zy,
15            zz,
16        }
17    }
18
19
20    pub fn identity() -> Self {
21        const VALUE: FusionMatrix =
22            FusionMatrix {
23                xx: 1.0f32,
24                xy: 0.0f32,
25                xz: 0.0f32,
26                yx: 0.0f32,
27                yy: 1.0f32,
28                yz: 0.0f32,
29                zx: 0.0f32,
30                zy: 0.0f32,
31                zz: 1.0f32,
32            };
33        VALUE
34    }
35}
36
37impl ops::Mul<FusionVector> for FusionMatrix {
38    type Output = FusionVector;
39    fn mul(self, rhs: FusionVector) -> Self::Output {
40        FusionVector {
41            x: self.xx * rhs.x + self.xy * rhs.y + self.xz * rhs.z,
42            y: self.yx * rhs.x + self.yy * rhs.y + self.yz * rhs.z,
43            z: self.zx * rhs.x + self.zy * rhs.y + self.zz * rhs.z,
44        }
45    }
46}
47
48impl From<FusionQuaternion> for FusionMatrix {
49    fn from(q: FusionQuaternion) -> Self {
50        let qwqw = q.w * q.w;
51        let qwqx = q.w * q.x;
52        let qwqy = q.w * q.y;
53        let qwqz = q.w * q.z;
54        let qxqx = q.x * q.x;
55        let qxqy = q.x * q.y;
56        let qxqz = q.x * q.z;
57        let qyqy = q.y * q.y;
58        let qyqz = q.y * q.z;
59        let qzqz = q.z * q.z;
60        Self {
61            xx: 2.0f32 * (qwqw - 0.5f32 + qxqx),
62            xy: 2.0f32 * (qxqy - qwqz),
63            xz: 2.0f32 * (qxqz + qwqy),
64            yx: 2.0f32 * (qxqy + qwqz),
65            yy: 2.0f32 * (qwqw - 0.5f32 + qyqy),
66            yz: 2.0f32 * (qyqz - qwqx),
67            zx: 2.0f32 * (qxqz - qwqy),
68            zy: 2.0f32 * (qyqz + qwqx),
69            zz: 2.0f32 * (qwqw - 0.5f32 + qzqz),
70        }
71    }
72}
imu_fusion/fusion_matrix_impl.rs

imu_fusion/
fusion_matrix_impl.rs
