src/common/fixed15_49.rs

   1 /*
   2  * Algorithms taken from libfixmath which is
   3  * Copyright (c) 2012 Petteri Aimonen <jpa @ kapsi.fi>
   4  * and from fpm which is
   5  * Copyright (c) 2019 Mike Lankamp
   6  *
   7  * Translation to Rust is
   8  * Copyright (c) 2020 Tilman Sauerbeck (tilman at code-monkey de)
   9  *
  10  * Permission is hereby granted, free of charge, to any person obtaining
  11  * a copy of this software and associated documentation files (the
  12  * "Software"), to deal in the Software without restriction, including
  13  * without limitation the rights to use, copy, modify, merge, publish,
  14  * distribute, sublicense, and/or sell copies of the Software, and to
  15  * permit persons to whom the Software is furnished to do so, subject to
  16  * the following conditions:
  17  *
  18  * The above copyright notice and this permission notice shall be
  19  * included in all copies or substantial portions of the Software.
  20  *
  21  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  22  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
  23  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  24  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
  25  * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
  26  * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
  27  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  28  */
  29
  30 use core::cmp::{PartialEq, PartialOrd};
  31 use core::ops::Neg;
  32 use core::ops::{AddAssign, SubAssign, MulAssign, DivAssign};
  33 use core::ops::{Add, Sub, Mul, Div};
  34
  35 #[derive(Clone, Copy, Eq, Ord)]
  36 pub struct Fixed15_49 {
  37     bits: i64,
  38 }
  39
  40 const FRACTIONAL_BITS: i64 = 49;
  41 const FACTOR: i64 = 1 << FRACTIONAL_BITS;
  42
  43 impl Fixed15_49 {
  44     pub fn from_bits(i: i64) -> Self {
  45         Self {
  46             bits: i,
  47         }
  48     }
  49     pub fn to_bits(&self) -> i64 { self.bits}
  50
  51     pub fn from_i64(n: i64) -> Self {
  52         if let Some(x) = n.checked_mul(FACTOR) {
  53             Self { bits: x }
  54         } else {
  55             panic!("overflow");
  56         }
  57     }
  58
  59     pub fn from_f32(f: f32) -> Self {
  60         let f_mul = f * FACTOR as f32;
  61
  62         if f_mul > i64::max_value() as f32 {
  63             panic!("");
  64         }
  65
  66         Self {
  67             bits: f_mul as i64,
  68         }
  69     }
  70
  71     pub fn to_i64(&self) -> i64 {
  72         self.bits >> FRACTIONAL_BITS
  73     }
  74
  75     pub fn to_f32(&self) -> f32 {
  76         (self.bits as f32) / (FACTOR as f32)
  77     }
  78
  79     pub fn is_negative(&self) -> bool {
  80         self.bits < 0
  81     }
  82
  83     pub fn abs(&self) -> Self {
  84         if self.is_negative() {
  85             -*self
  86         } else {
  87             *self
  88         }
  89     }
  90
  91     pub fn pi() -> Self {
  92         Self::from_bits(0x6487ed5110b46)
  93     }
  94
  95     pub fn two_pi() -> Self {
  96         Self::from_bits(0xc90fdaa22168c)
  97     }
  98
  99     pub fn pi_div_two() -> Self {
 100         Self::from_bits(0x6487ed5110b46)
 101     }
 102
 103     pub fn to_radians(&self) -> Self {
 104         (*self * Self::from_bits(0x8efa351294f))
 105     }
 106
 107     pub fn sqrt(&self) -> Self {
 108         // Ported from Mike Lankamp's fpm.
 109
 110         if self.bits == 0 {
 111             return *self;
 112         }
 113
 114         let mut num = (self.bits as i128) << FRACTIONAL_BITS;
 115         let mut result = 0i128;
 116
 117         let shift = ((64 - 1 - self.bits.leading_zeros()) +
 118                      (FRACTIONAL_BITS as u32)) / 2 * 2;
 119
 120         let mut bit = 1i128 << shift;
 121
 122         while bit != 0 {
 123             let t = result + bit;
 124
 125             result >>= 1;
 126
 127             if num >= t {
 128                 num -= t;
 129                 result += bit;
 130             }
 131
 132             bit >>= 2;
 133         }
 134
 135         if num > result {
 136             result += 1;
 137         }
 138
 139         Self::from_bits(result as i64)
 140     }
 141
 142     pub fn fmod(&self, rhs: Self) -> Self {
 143         Self::from_bits(self.bits % rhs.bits)
 144     }
 145
 146     pub fn sin(&self) -> Self {
 147         // Ported from Mike Lankamp's fpm.
 148
 149         // This sine uses a fifth-order curve-fitting approximation originally
 150         // described by Jasper Vijn on coranac.com which has a worst-case
 151         // relative error of 0.07% (over [-pi:pi]).
 152
 153         let one = Self::from_i64(1);
 154         let two = Self::from_i64(2);
 155         let three = Self::from_i64(3);
 156         let four = Self::from_i64(4);
 157         let five = Self::from_i64(5);
 158
 159         let mut x = *self;
 160
 161         // Turn x from [0..2*PI] domain into [0..4] domain
 162         x = x.fmod(Self::two_pi()) / Self::pi_div_two();
 163
 164         // Take x modulo one rotation, so [-4..+4].
 165         if x.is_negative() {
 166             x = x + four;
 167         }
 168
 169         let sign = if x > two {
 170             // Reduce domain to [0..2].
 171             x = x - two;
 172
 173             -1
 174         } else {
 175             1
 176         };
 177
 178         if x > one {
 179             // Reduce domain to [0..1].
 180             x = two - x;
 181         }
 182
 183         let x2 = x * x;
 184
 185         let t = Self::pi() - (
 186             x2 * (Self::two_pi() - five - (x2 * (Self::pi() - three)))
 187         );
 188
 189         let v = (x * t) / two;
 190
 191         if sign == -1 {
 192             -v
 193         } else {
 194             v
 195         }
 196     }
 197
 198     pub fn cos(&self) -> Self {
 199         (*self + Self::pi_div_two()).sin()
 200     }
 201 }
 202
 203 impl PartialEq for Fixed15_49 {
 204     fn eq(&self, other: &Self) -> bool {
 205         self.bits == other.bits
 206     }
 207 }
 208
 209 impl PartialOrd for Fixed15_49 {
 210     fn partial_cmp(&self, other: &Self) -> Option<core::cmp::Ordering> {
 211         self.bits.partial_cmp(&other.bits)
 212     }
 213 }
 214
 215 impl Neg for Fixed15_49 {
 216     type Output = Self;
 217
 218     fn neg(self) -> Self {
 219         Self::from_bits(-self.bits)
 220     }
 221 }
 222
 223 impl AddAssign for Fixed15_49 {
 224     fn add_assign(&mut self, rhs: Self) {
 225         self.bits = self.bits.saturating_add(rhs.bits);
 226     }
 227 }
 228
 229 impl SubAssign for Fixed15_49 {
 230     fn sub_assign(&mut self, rhs: Self) {
 231         self.bits = self.bits.saturating_sub(rhs.bits);
 232     }
 233 }
 234
 235 impl MulAssign for Fixed15_49 {
 236     fn mul_assign(&mut self, rhs: Self) {
 237         let product = (self.bits as i128) * (rhs.bits as i128);
 238
 239         self.bits = (product >> FRACTIONAL_BITS) as i64;
 240     }
 241 }
 242
 243 impl DivAssign for Fixed15_49 {
 244     fn div_assign(&mut self, rhs: Self) {
 245         // Ported from Petteri Aimonen's libfixmath.
 246
 247         if rhs.bits == 0 {
 248             panic!("attempted division by zero");
 249         }
 250
 251         let mut remainder = (
 252             if self.bits >= 0 { self.bits } else { -self.bits }
 253         ) as u64;
 254
 255         let mut divider = (
 256             if rhs.bits >= 0 { rhs.bits } else { -rhs.bits }
 257         ) as u64;
 258
 259         let mut quotient: u64 = 0;
 260
 261         let mut bit_pos: i64 = FRACTIONAL_BITS + 1;
 262
 263         while (divider & 0xf) == 0 && bit_pos >= 4 {
 264             divider >>= 4;
 265             bit_pos -= 4;
 266         }
 267
 268         while remainder != 0 && bit_pos >= 0 {
 269             let mut shift = remainder.leading_zeros() as i64;
 270
 271             if shift > bit_pos {
 272                 shift = bit_pos;
 273             }
 274
 275             remainder <<= shift;
 276             bit_pos -= shift;
 277
 278             let d = (remainder / divider) as u64;
 279             remainder %= divider;
 280             quotient += d << bit_pos;
 281
 282             remainder <<= 1;
 283             bit_pos -= 1;
 284         }
 285
 286         let new_bits = (quotient >> 1) as i64;
 287
 288         if self.is_negative() == rhs.is_negative() {
 289             self.bits = new_bits;
 290         } else {
 291             self.bits = -new_bits;
 292         }
 293     }
 294 }
 295
 296 impl Add for Fixed15_49 {
 297     type Output = Self;
 298
 299     fn add(self, rhs: Self) -> Self {
 300         let mut r = self;
 301         r += rhs;
 302         r
 303     }
 304 }
 305
 306 impl Sub for Fixed15_49 {
 307     type Output = Self;
 308
 309     fn sub(self, rhs: Self) -> Self {
 310         let mut r = self;
 311         r -= rhs;
 312         r
 313     }
 314 }
 315
 316 impl Mul for Fixed15_49 {
 317     type Output = Self;
 318
 319     fn mul(self, rhs: Self) -> Self {
 320         let mut r = self;
 321         r *= rhs;
 322         r
 323     }
 324 }
 325
 326 impl Div for Fixed15_49 {
 327     type Output = Self;
 328
 329     fn div(self, rhs: Self) -> Self {
 330         let mut r = self;
 331         r /= rhs;
 332         r
 333     }
 334 }