imu_fusion/
fusion_quaternion_impl.rs

1use core::ops;
2#[allow(unused_imports)]
3use libm::{asinf, atan2f, sqrtf};
4use crate::{Angle, asin_safe, fusion_fast_inverse_sqrt, fusion_radians_to_degrees, FusionEuler, FusionMatrix, FusionQuaternion, FusionVector};
5
6impl FusionQuaternion {
7    pub fn identity() -> Self {
8        const VALUE: FusionQuaternion =
9            FusionQuaternion {
10                w: 1.0f32,
11                x: 0.0f32,
12                y: 0.0f32,
13                z: 0.0f32,
14            };
15        VALUE
16    }
17
18    pub fn normalize(&self) -> Self {
19        #[cfg(feature = "fusion-use-normal-sqrt")]
20        {
21            *self * 1.0f32 / sqrtf(self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z)
22        }
23        #[cfg(not(feature = "fusion-use-normal-sqrt"))]
24        {
25            *self * fusion_fast_inverse_sqrt(self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z)
26        }
27    }
28
29    pub fn euler(self) -> FusionEuler {
30        // calculate common terms to avoid repeated operations
31        let q = self;
32        let half_minus_qy_squared = 0.5f32 - q.y * q.y;
33        let roll = fusion_radians_to_degrees(atan2f(q.w * q.x + q.y * q.z, half_minus_qy_squared - q.x * q.x));
34        let pitch = fusion_radians_to_degrees(asin_safe(2.0f32 * (q.w * q.y - q.z * q.x)));
35        let yaw = fusion_radians_to_degrees(atan2f(q.w * q.z + q.x * q.y, half_minus_qy_squared - q.z * q.z));
36
37        FusionEuler {
38            angle: Angle {
39                roll,
40                pitch,
41                yaw,
42            }
43        }
44    }
45
46    pub fn rotation(self) -> FusionMatrix {
47        // calculate common terms to avoid repeated operations
48        let qwqw = self.w * self.w;
49        let qwqx = self.w * self.x;
50        let qwqy = self.w * self.y;
51        let qwqz = self.w * self.z;
52        let qxqx = self.x * self.x;
53        let qxqy = self.x * self.y;
54        let qxqz = self.x * self.z;
55        let qyqy = self.y * self.y;
56        let qyqz = self.y * self.z;
57        let qzqz = self.z * self.z;
58        FusionMatrix {
59            xx: 2.0f32 * (qwqw - 0.5f32 + qxqx),
60            xy: 2.0f32 * (qxqy - qwqz),
61            xz: 2.0f32 * (qxqz + qwqy),
62            yx: 2.0f32 * (qxqy + qwqz),
63            yy: 2.0f32 * (qwqw - 0.5f32 + qyqy),
64            yz: 2.0f32 * (qyqz - qwqx),
65            zx: 2.0f32 * (qxqz - qwqy),
66            zy: 2.0f32 * (qyqz + qwqx),
67            zz: 2.0f32 * (qwqw - 0.5f32 + qzqz),
68
69        }
70    }
71}
72
73impl ops::Add for FusionQuaternion {
74    type Output = Self;
75
76    fn add(self, rhs: Self) -> Self::Output {
77        Self {
78            w: self.w + rhs.w,
79            x: self.x + rhs.x,
80            y: self.y + rhs.y,
81            z: self.z + rhs.z,
82        }
83    }
84}
85
86impl ops::Mul for FusionQuaternion {
87    type Output = Self;
88
89    fn mul(self, rhs: Self) -> Self::Output {
90        Self {
91            w: self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z,
92            x: self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y,
93            y: self.w * rhs.y - self.x * rhs.z + self.y * rhs.w + self.z * rhs.x,
94            z: self.w * rhs.z + self.x * rhs.y - self.y * rhs.x + self.z * rhs.w,
95        }
96    }
97}
98
99impl ops::Mul<FusionVector> for FusionQuaternion {
100    type Output = Self;
101    fn mul(self, rhs: FusionVector) -> Self::Output {
102        Self {
103            w: -self.x * rhs.x - self.y * rhs.y - self.z * rhs.z,
104            x: self.w * rhs.x + self.y * rhs.z - self.z * rhs.y,
105            y: self.w * rhs.y - self.x * rhs.z + self.z * rhs.x,
106            z: self.w * rhs.z + self.x * rhs.y - self.y * rhs.x,
107        }
108    }
109}
110
111impl ops::Mul<f32> for FusionQuaternion {
112    type Output = Self;
113    fn mul(self, rhs: f32) -> Self::Output {
114        Self {
115            w: self.w * rhs,
116            x: self.x * rhs,
117            y: self.y * rhs,
118            z: self.z * rhs,
119        }
120    }
121}
122
imu_fusion/fusion_quaternion_impl.rs

imu_fusion/
fusion_quaternion_impl.rs
