From 671d6a8482ac742b897913ec6975910ed5fa273c Mon Sep 17 00:00:00 2001 From: Tilman Sauerbeck Date: Sun, 19 Jan 2020 12:24:07 +0100 Subject: [PATCH] common: Add the fixed15_49 module. Implements Q15.49 fixed point math. --- SConscript.libcommon.rs | 1 + src/common/fixed15_49.rs | 334 +++++++++++++++++++++++++++++++++++++++ src/common/lib.rs | 1 + 3 files changed, 336 insertions(+) create mode 100644 src/common/fixed15_49.rs 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 + * 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 { + 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; -- 2.30.2