Added missing math extensions

firmware/jerk_control/src/math_ext.rs

@@ -0,0 +1,78 @@
+
+
+pub trait Roots {
+    fn sqrt(self) -> Self;
+    fn cbrt(self) -> Self;
+}
+
+#[cfg(ignore)]
+impl Roots for u64 {
+    fn sqrt(self) -> Self {
+	let mut estimate = self / 2;
+	for _ in 0..6 {
+	    estimate = (self / estimate + estimate) / 2;
+	}
+	estimate
+    }
+
+    fn cbrt(self) -> Self {
+    }
+}
+
+#[cfg(ignore)]
+impl Roots for u32 {
+   fn sqrt(self) -> Self {
+	let mut estimate = self / 2;
+	for _ in 0..5 {
+	    estimate = (self / estimate + estimate) / 2;
+	}
+	estimate
+    }
+
+    fn cbrt(self) -> Self {
+    }
+}
+
+
+fn sqrt_approx(value: f32) -> f32 {
+    f32::from_bits((value.to_bits() + 0x3f80_0000) >> 1)
+}
+
+pub fn cbrt_approx(value: f32) -> f32 {
+    let value = value.to_bits();
+
+	const EXP_BIAS: i32 = 0x3f80_0000;
+
+	f32::from_bits((value & 0x8000_0000) |
+					   (((value as i32 & 0x7fff_ffff) - EXP_BIAS) / 3 + EXP_BIAS) as u32)
+}
+
+impl Roots for f32 {
+    fn sqrt(self) -> Self {
+	let mut estimate = sqrt_approx(self);
+	for _ in 0..5 {
+	    estimate = (self / estimate + estimate) / 2.;
+	}
+	estimate
+    }
+
+    fn cbrt(self) -> Self {
+	let mut estimate = cbrt_approx(self); // TODO: improve this with bit twiddling
+
+	// Use halley's method to refine the approximation.
+	for _ in 0..3 {
+	    let e3 = estimate * estimate * estimate;
+	    estimate = estimate * (e3 + self + self) / ( e3 + e3 + self);
+	}
+	
+	estimate
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use super::Roots;
+
+
+    
+}