--- /dev/null
+/*
+ * 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
+ }
+}
projects / gps-watch.git / commitdiff