From: Tilman Sauerbeck <tilman@code-monkey.de>
Date: Sun, 19 Jan 2020 11:24:07 +0000 (+0100)
Subject: common: Add the fixed15_49 module.
X-Git-Url: http://git.code-monkey.de/?a=commitdiff_plain;h=671d6a8482ac742b897913ec6975910ed5fa273c;p=gps-watch.git

common: Add the fixed15_49 module.

Implements Q15.49 fixed point math.
---

diff --git a/SConscript.libcommon.rs b/SConscript.libcommon.rs
index afbf463..519c05c 100644
--- a/SConscript.libcommon.rs
+++ b/SConscript.libcommon.rs
@@ -28,6 +28,7 @@ source_files = [
     'src/common/yencode.rs',
     'src/common/varint.rs',
     'src/common/logger.rs',
+    'src/common/fixed15_49.rs',
 ]
 
 libcommon_rlib = env.Rustc('libcommon.rlib', source_files[0])
diff --git a/src/common/fixed15_49.rs b/src/common/fixed15_49.rs
new file mode 100644
index 0000000..6ce37fe
--- /dev/null
+++ b/src/common/fixed15_49.rs
@@ -0,0 +1,334 @@
+/*
+ * Algorithms taken from libfixmath which is
+ * Copyright (c) 2012 Petteri Aimonen <jpa @ kapsi.fi>
+ * and from fpm which is
+ * Copyright (c) 2019 Mike Lankamp
+ *
+ * Translation to Rust is
+ * Copyright (c) 2020 Tilman Sauerbeck (tilman at code-monkey de)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+use core::cmp::{PartialEq, PartialOrd};
+use core::ops::Neg;
+use core::ops::{AddAssign, SubAssign, MulAssign, DivAssign};
+use core::ops::{Add, Sub, Mul, Div};
+
+#[derive(Clone, Copy, Eq, Ord)]
+pub struct Fixed15_49 {
+    bits: i64,
+}
+
+const FRACTIONAL_BITS: i64 = 49;
+const FACTOR: i64 = 1 << FRACTIONAL_BITS;
+
+impl Fixed15_49 {
+    pub fn from_bits(i: i64) -> Self {
+        Self {
+            bits: i,
+        }
+    }
+    pub fn to_bits(&self) -> i64 { self.bits}
+
+    pub fn from_i64(n: i64) -> Self {
+        if let Some(x) = n.checked_mul(FACTOR) {
+            Self { bits: x }
+        } else {
+            panic!("overflow");
+        }
+    }
+
+    pub fn from_f32(f: f32) -> Self {
+        let f_mul = f * FACTOR as f32;
+
+        if f_mul > i64::max_value() as f32 {
+            panic!("");
+        }
+
+        Self {
+            bits: f_mul as i64,
+        }
+    }
+
+    pub fn to_i64(&self) -> i64 {
+        self.bits >> FRACTIONAL_BITS
+    }
+
+    pub fn to_f32(&self) -> f32 {
+        (self.bits as f32) / (FACTOR as f32)
+    }
+
+    pub fn is_negative(&self) -> bool {
+        self.bits < 0
+    }
+
+    pub fn abs(&self) -> Self {
+        if self.is_negative() {
+            -*self
+        } else {
+            *self
+        }
+    }
+
+    pub fn pi() -> Self {
+        Self::from_bits(0x6487ed5110b46)
+    }
+
+    pub fn two_pi() -> Self {
+        Self::from_bits(0xc90fdaa22168c)
+    }
+
+    pub fn pi_div_two() -> Self {
+        Self::from_bits(0x6487ed5110b46)
+    }
+
+    pub fn to_radians(&self) -> Self {
+        (*self * Self::from_bits(0x8efa351294f))
+    }
+
+    pub fn sqrt(&self) -> Self {
+        // Ported from Mike Lankamp's fpm.
+
+        if self.bits == 0 {
+            return *self;
+        }
+
+        let mut num = (self.bits as i128) << FRACTIONAL_BITS;
+        let mut result = 0i128;
+
+        let shift = ((64 - 1 - self.bits.leading_zeros()) +
+                     (FRACTIONAL_BITS as u32)) / 2 * 2;
+
+        let mut bit = 1i128 << shift;
+
+        while bit != 0 {
+            let t = result + bit;
+
+            result >>= 1;
+
+            if num >= t {
+                num -= t;
+                result += bit;
+            }
+
+            bit >>= 2;
+        }
+
+        if num > result {
+            result += 1;
+        }
+
+        Self::from_bits(result as i64)
+    }
+
+    pub fn fmod(&self, rhs: Self) -> Self {
+        Self::from_bits(self.bits % rhs.bits)
+    }
+
+    pub fn sin(&self) -> Self {
+        // Ported from Mike Lankamp's fpm.
+
+        // This sine uses a fifth-order curve-fitting approximation originally
+        // described by Jasper Vijn on coranac.com which has a worst-case
+        // relative error of 0.07% (over [-pi:pi]).
+
+        let one = Self::from_i64(1);
+        let two = Self::from_i64(2);
+        let three = Self::from_i64(3);
+        let four = Self::from_i64(4);
+        let five = Self::from_i64(5);
+
+        let mut x = *self;
+
+        // Turn x from [0..2*PI] domain into [0..4] domain
+        x = x.fmod(Self::two_pi()) / Self::pi_div_two();
+
+        // Take x modulo one rotation, so [-4..+4].
+        if x.is_negative() {
+            x = x + four;
+        }
+
+        let sign = if x > two {
+            // Reduce domain to [0..2].
+            x = x - two;
+
+            -1
+        } else {
+            1
+        };
+
+        if x > one {
+            // Reduce domain to [0..1].
+            x = two - x;
+        }
+
+        let x2 = x * x;
+
+        let t = Self::pi() - (
+            x2 * (Self::two_pi() - five - (x2 * (Self::pi() - three)))
+        ); 
+
+        let v = (x * t) / two;
+
+        if sign == -1 {
+            -v
+        } else {
+            v
+        }
+    }
+
+    pub fn cos(&self) -> Self {
+        (*self + Self::pi_div_two()).sin()
+    }
+}
+
+impl PartialEq for Fixed15_49 {
+    fn eq(&self, other: &Self) -> bool {
+        self.bits == other.bits
+    }
+}
+
+impl PartialOrd for Fixed15_49 {
+    fn partial_cmp(&self, other: &Self) -> Option<core::cmp::Ordering> {
+        self.bits.partial_cmp(&other.bits)
+    }
+}
+
+impl Neg for Fixed15_49 {
+    type Output = Self;
+
+    fn neg(self) -> Self {
+        Self::from_bits(-self.bits)
+    }
+}
+
+impl AddAssign for Fixed15_49 {
+    fn add_assign(&mut self, rhs: Self) {
+        self.bits = self.bits.saturating_add(rhs.bits);
+    }
+}
+
+impl SubAssign for Fixed15_49 {
+    fn sub_assign(&mut self, rhs: Self) {
+        self.bits = self.bits.saturating_sub(rhs.bits);
+    }
+}
+
+impl MulAssign for Fixed15_49 {
+    fn mul_assign(&mut self, rhs: Self) {
+        let product = (self.bits as i128) * (rhs.bits as i128);
+
+        self.bits = (product >> FRACTIONAL_BITS) as i64;
+    }
+}
+
+impl DivAssign for Fixed15_49 {
+    fn div_assign(&mut self, rhs: Self) {
+        // Ported from Petteri Aimonen's libfixmath.
+
+        if rhs.bits == 0 {
+            panic!("attempted division by zero");
+        }
+
+        let mut remainder = (
+            if self.bits >= 0 { self.bits } else { -self.bits }
+        ) as u64;
+
+        let mut divider = (
+            if rhs.bits >= 0 { rhs.bits } else { -rhs.bits }
+        ) as u64;
+
+        let mut quotient: u64 = 0;
+
+        let mut bit_pos: i64 = FRACTIONAL_BITS + 1;
+
+        while (divider & 0xf) == 0 && bit_pos >= 4 {
+            divider >>= 4;
+            bit_pos -= 4;
+        }
+
+        while remainder != 0 && bit_pos >= 0 {
+            let mut shift = remainder.leading_zeros() as i64;
+
+            if shift > bit_pos {
+                shift = bit_pos;
+            }
+
+            remainder <<= shift;
+            bit_pos -= shift;
+
+            let d = (remainder / divider) as u64;
+            remainder %= divider;
+            quotient += d << bit_pos;
+
+            remainder <<= 1;
+            bit_pos -= 1;
+        }
+
+        let new_bits = (quotient >> 1) as i64;
+
+        if self.is_negative() == rhs.is_negative() {
+            self.bits = new_bits;
+        } else {
+            self.bits = -new_bits;
+        }
+    }
+}
+
+impl Add for Fixed15_49 {
+    type Output = Self;
+
+    fn add(self, rhs: Self) -> Self {
+        let mut r = self;
+        r += rhs;
+        r
+    }
+}
+
+impl Sub for Fixed15_49 {
+    type Output = Self;
+
+    fn sub(self, rhs: Self) -> Self {
+        let mut r = self;
+        r -= rhs;
+        r
+    }
+}
+
+impl Mul for Fixed15_49 {
+    type Output = Self;
+
+    fn mul(self, rhs: Self) -> Self {
+        let mut r = self;
+        r *= rhs;
+        r
+    }
+}
+
+impl Div for Fixed15_49 {
+    type Output = Self;
+
+    fn div(self, rhs: Self) -> Self {
+        let mut r = self;
+        r /= rhs;
+        r
+    }
+}
diff --git a/src/common/lib.rs b/src/common/lib.rs
index e7fb361..67f2648 100644
--- a/src/common/lib.rs
+++ b/src/common/lib.rs
@@ -50,3 +50,4 @@ pub mod shell;
 pub mod yencode;
 pub mod varint;
 pub mod logger;
+pub mod fixed15_49;