imu_fusion/
fusion_vector_impl.rs

1use core::ops;
2#[cfg(feature = "fusion-use-normal-sqrt")]
3use libm::sqrtf;
4
5use crate::{fusion_fast_inverse_sqrt, FusionVector};
6
7impl FusionVector {
8    pub fn get(&self, x: &mut f32, y: &mut f32, z: &mut f32) {
9        *x = self.x;
10        *y = self.y;
11        *z = self.z;
12    }
13    pub fn new(x: f32, y: f32, z: f32) -> Self {
14        Self {
15            x,
16            y,
17            z,
18        }
19    }
20    pub fn zero() -> Self {
21        const VALUE: FusionVector =
22            FusionVector {
23                x: 0.0f32,
24                y: 0.0f32,
25                z: 0.0f32,
26            };
27        VALUE
28    }
29
30    pub fn ones() -> Self {
31        const VALUE: FusionVector =
32            FusionVector {
33                x: 1.0f32,
34                y: 1.0f32,
35                z: 1.0f32,
36            };
37        VALUE
38    }
39
40    pub fn is_zero(&self) -> bool {
41        self.x == 0.0f32 && self.y == 0.0f32 && self.z == 0.0f32
42    }
43    pub fn sum(&self) -> f32 {
44        self.x + self.y + self.z
45    }
46
47    pub fn cross_product(&self, rhs: &Self) -> Self {
48        Self {
49            x: self.y * rhs.z - self.z * rhs.y,
50            y: self.z * rhs.x - self.x * rhs.z,
51            z: self.x * rhs.y - self.y * rhs.x,
52        }
53    }
54
55    pub fn dot_product(&self, rhs: &Self) -> f32 {
56        self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
57    }
58
59    pub fn magnitude_squared(&self, rhs: &Self) -> f32 {
60        self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
61    }
62
63    pub fn magnitude(&self) -> f32 {
64        self.x * self.x + self.y * self.y + self.z * self.z
65    }
66
67    pub fn normalize(&self) -> Self {
68        #[cfg(feature = "fusion-use-normal-sqrt")]
69        {
70            *self * (1.0f32 / sqrtf(self.magnitude()))
71        }
72        #[cfg(not(feature = "fusion-use-normal-sqrt"))]
73        {
74            *self * fusion_fast_inverse_sqrt(self.magnitude())
75        }
76    }
77}
78
79impl ops::Add for FusionVector {
80    type Output = Self;
81
82    fn add(self, rhs: Self) -> Self::Output {
83        Self {
84            x: self.x + rhs.x,
85            y: self.y + rhs.y,
86            z: self.z + rhs.z,
87        }
88    }
89}
90
91impl ops::Sub for FusionVector {
92    type Output = Self;
93
94    fn sub(self, rhs: Self) -> Self::Output {
95        Self {
96            x: self.x - rhs.x,
97            y: self.y - rhs.y,
98            z: self.z - rhs.z,
99        }
100    }
101}
102
103impl ops::AddAssign for FusionVector {
104    fn add_assign(&mut self, rhs: Self) {
105        self.x += rhs.x;
106        self.y += rhs.y;
107        self.z += rhs.z;
108    }
109}
110
111impl ops::SubAssign for FusionVector {
112    fn sub_assign(&mut self, rhs: Self) {
113        self.x -= rhs.x;
114        self.y -= rhs.y;
115        self.z -= rhs.z;
116    }
117}
118
119impl ops::Mul for FusionVector {
120    type Output = Self;
121
122    fn mul(self, rhs: Self) -> Self::Output {
123        Self {
124            x: self.x * rhs.x,
125            y: self.y * rhs.y,
126            z: self.z * rhs.z,
127        }
128    }
129}
130
131impl ops::Mul<f32> for FusionVector {
132    type Output = Self;
133
134    fn mul(self, rhs: f32) -> Self::Output {
135        Self {
136            x: self.x * rhs,
137            y: self.y * rhs,
138            z: self.z * rhs,
139        }
140    }
141}
142
143impl ops::MulAssign for FusionVector {
144    fn mul_assign(&mut self, rhs: Self) {
145        self.x *= rhs.x;
146        self.y *= rhs.y;
147        self.z *= rhs.z;
148    }
149}
150
151#[test]
152fn add_test() {
153    let a = FusionVector {
154        x: 1.0f32,
155        y: 2.0f32,
156        z: 3.0f32,
157    };
158    let b = FusionVector {
159        x: 4.0f32,
160        y: 5.0f32,
161        z: 6.0f32,
162    };
163    let c = a + b;
164    assert_eq!(c.x, 5.0f32);
165    assert_eq!(c.y, 7.0f32);
166    assert_eq!(c.z, 9.0f32);
167}
imu_fusion/fusion_vector_impl.rs

imu_fusion/
fusion_vector_impl.rs
