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
7 * Translation to Rust is
8 * Copyright (c) 2020 Tilman Sauerbeck (tilman at code-monkey de)
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:
18 * The above copyright notice and this permission notice shall be
19 * included in all copies or substantial portions of the Software.
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.
30 use core::cmp::{PartialEq, PartialOrd};
32 use core::ops::{AddAssign, SubAssign, MulAssign, DivAssign};
33 use core::ops::{Add, Sub, Mul, Div};
35 #[derive(Clone, Copy, Eq, Ord)]
36 pub struct Fixed15_49 {
40 const FRACTIONAL_BITS: i64 = 49;
41 const FACTOR: i64 = 1 << FRACTIONAL_BITS;
44 pub fn from_bits(i: i64) -> Self {
49 pub fn to_bits(&self) -> i64 { self.bits}
51 pub fn from_i64(n: i64) -> Self {
52 if let Some(x) = n.checked_mul(FACTOR) {
59 pub fn from_f32(f: f32) -> Self {
60 let f_mul = f * FACTOR as f32;
62 if f_mul > i64::max_value() as f32 {
71 pub fn to_i64(&self) -> i64 {
72 self.bits >> FRACTIONAL_BITS
75 pub fn to_f32(&self) -> f32 {
76 (self.bits as f32) / (FACTOR as f32)
79 pub fn is_negative(&self) -> bool {
83 pub fn abs(&self) -> Self {
84 if self.is_negative() {
92 Self::from_bits(0x6487ed5110b46)
95 pub fn two_pi() -> Self {
96 Self::from_bits(0xc90fdaa22168c)
99 pub fn pi_div_two() -> Self {
100 Self::from_bits(0x3243f6a8885a3)
103 pub fn to_radians(&self) -> Self {
104 (*self * Self::from_bits(0x8efa351294f))
107 pub fn sqrt(&self) -> Self {
108 // Ported from Mike Lankamp's fpm.
114 let mut num = (self.bits as i128) << FRACTIONAL_BITS;
115 let mut result = 0i128;
117 let shift = ((64 - 1 - self.bits.leading_zeros()) +
118 (FRACTIONAL_BITS as u32)) / 2 * 2;
120 let mut bit = 1i128 << shift;
123 let t = result + bit;
139 Self::from_bits(result as i64)
142 pub fn fmod(&self, rhs: Self) -> Self {
143 Self::from_bits(self.bits % rhs.bits)
146 pub fn sin(&self) -> Self {
147 // Ported from Mike Lankamp's fpm.
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]).
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);
161 // Turn x from [0..2*PI] domain into [0..4] domain
162 x = x.fmod(Self::two_pi()) / Self::pi_div_two();
164 // Take x modulo one rotation, so [-4..+4].
169 let sign = if x > two {
170 // Reduce domain to [0..2].
179 // Reduce domain to [0..1].
185 let t = Self::pi() - (
186 x2 * (Self::two_pi() - five - (x2 * (Self::pi() - three)))
189 let v = (x * t) / two;
198 pub fn cos(&self) -> Self {
199 (*self + Self::pi_div_two()).sin()
203 impl PartialEq for Fixed15_49 {
204 fn eq(&self, other: &Self) -> bool {
205 self.bits == other.bits
209 impl PartialOrd for Fixed15_49 {
210 fn partial_cmp(&self, other: &Self) -> Option<core::cmp::Ordering> {
211 self.bits.partial_cmp(&other.bits)
215 impl Neg for Fixed15_49 {
218 fn neg(self) -> Self {
219 Self::from_bits(-self.bits)
223 impl AddAssign for Fixed15_49 {
224 fn add_assign(&mut self, rhs: Self) {
225 self.bits = self.bits.saturating_add(rhs.bits);
229 impl SubAssign for Fixed15_49 {
230 fn sub_assign(&mut self, rhs: Self) {
231 self.bits = self.bits.saturating_sub(rhs.bits);
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);
239 self.bits = (product >> FRACTIONAL_BITS) as i64;
243 impl DivAssign for Fixed15_49 {
244 fn div_assign(&mut self, rhs: Self) {
245 // Ported from Petteri Aimonen's libfixmath.
248 panic!("attempted division by zero");
251 let mut remainder = (
252 if self.bits >= 0 { self.bits } else { -self.bits }
256 if rhs.bits >= 0 { rhs.bits } else { -rhs.bits }
259 let mut quotient: u64 = 0;
261 let mut bit_pos: i64 = FRACTIONAL_BITS + 1;
263 while (divider & 0xf) == 0 && bit_pos >= 4 {
268 while remainder != 0 && bit_pos >= 0 {
269 let mut shift = remainder.leading_zeros() as i64;
278 let d = (remainder / divider) as u64;
279 remainder %= divider;
280 quotient += d << bit_pos;
286 let new_bits = (quotient >> 1) as i64;
288 if self.is_negative() == rhs.is_negative() {
289 self.bits = new_bits;
291 self.bits = -new_bits;
296 impl Add for Fixed15_49 {
299 fn add(self, rhs: Self) -> Self {
306 impl Sub for Fixed15_49 {
309 fn sub(self, rhs: Self) -> Self {
316 impl Mul for Fixed15_49 {
319 fn mul(self, rhs: Self) -> Self {
326 impl Div for Fixed15_49 {
329 fn div(self, rhs: Self) -> Self {