rust/fpnum/src/lib.rs
author unC0Rr
Thu, 19 Dec 2024 14:18:55 +0100
branchtransitional_engine
changeset 16081 6633961698ad
parent 16079 624b74443b53
permissions -rw-r--r--
Make wider range of numbers representable with FPNum on the cost of smaller precision
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
16079
624b74443b53 Make FPNum use saturating calculations
unC0Rr
parents: 15860
diff changeset
     1
use std::{cmp, ops};
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
     2
use std::marker::PhantomData;
16079
624b74443b53 Make FPNum use saturating calculations
unC0Rr
parents: 15860
diff changeset
     3
use saturate::SaturatingInto;
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
     4
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
     5
const POSITIVE_MASK: u64 = 0x0000_0000_0000_0000;
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
     6
const NEGATIVE_MASK: u64 = 0xFFFF_FFFF_FFFF_FFFF;
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
     7
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
     8
#[inline]
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
     9
fn bool_mask(is_negative: bool) -> u64 {
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    10
    if is_negative {
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    11
        NEGATIVE_MASK
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    12
    } else {
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    13
        POSITIVE_MASK
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    14
    }
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    15
}
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    16
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    17
struct FracBits<const N: u8>;
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    18
#[derive(Clone, Debug, Copy)]
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    19
pub struct FixedPoint<const FRAC_BITS: u8> {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    20
    sign_mask: u64,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    21
    value: u64,
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    22
    _marker: PhantomData<FracBits<FRAC_BITS>>,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    23
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    24
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    25
pub type FPNum = FixedPoint<20>;
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    26
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    27
impl<const FRAC_BITS: u8> FixedPoint<FRAC_BITS> {
13948
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
    28
    #[inline]
13949
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
    29
    pub fn new(numerator: i32, denominator: u32) -> Self {
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    30
        Self::from(numerator) / denominator
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    31
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    32
13909
cd39e87d7a80 inline more operators in fpnum
unc0rr
parents: 13908
diff changeset
    33
    #[inline]
13949
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
    34
    pub fn signum(&self) -> i8 {
15234
517f3a1dd5c2 reduce convertions in signum
alfadur
parents: 15233
diff changeset
    35
        (1u64 ^ self.sign_mask).wrapping_sub(self.sign_mask) as i8
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    36
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    37
13909
cd39e87d7a80 inline more operators in fpnum
unc0rr
parents: 13908
diff changeset
    38
    #[inline]
14648
2e2b31cf0871 make stuff const
alfadur
parents: 14228
diff changeset
    39
    pub const fn is_negative(&self) -> bool {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    40
        self.sign_mask != POSITIVE_MASK
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    41
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    42
13909
cd39e87d7a80 inline more operators in fpnum
unc0rr
parents: 13908
diff changeset
    43
    #[inline]
14648
2e2b31cf0871 make stuff const
alfadur
parents: 14228
diff changeset
    44
    pub const fn is_positive(&self) -> bool {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    45
        self.sign_mask == POSITIVE_MASK
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    46
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    47
13909
cd39e87d7a80 inline more operators in fpnum
unc0rr
parents: 13908
diff changeset
    48
    #[inline]
14648
2e2b31cf0871 make stuff const
alfadur
parents: 14228
diff changeset
    49
    pub const fn is_zero(&self) -> bool {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    50
        self.value == 0
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    51
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    52
13909
cd39e87d7a80 inline more operators in fpnum
unc0rr
parents: 13908
diff changeset
    53
    #[inline]
14648
2e2b31cf0871 make stuff const
alfadur
parents: 14228
diff changeset
    54
    pub const fn abs(&self) -> Self {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    55
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    56
            sign_mask: POSITIVE_MASK,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    57
            value: self.value,
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    58
            _marker: self._marker,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    59
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    60
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    61
13909
cd39e87d7a80 inline more operators in fpnum
unc0rr
parents: 13908
diff changeset
    62
    #[inline]
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    63
    pub fn round(&self) -> i64 {
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    64
        ((self.value >> FRAC_BITS) as i64 ^ self.sign_mask as i64).wrapping_sub(self.sign_mask as i64)
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    65
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    66
13909
cd39e87d7a80 inline more operators in fpnum
unc0rr
parents: 13908
diff changeset
    67
    #[inline]
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    68
    pub const fn abs_round(&self) -> u64 {
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    69
        self.value >> FRAC_BITS
14102
5d42204ac35e Start convertion of FindPoint()
unC0Rr
parents: 13960
diff changeset
    70
    }
5d42204ac35e Start convertion of FindPoint()
unC0Rr
parents: 13960
diff changeset
    71
5d42204ac35e Start convertion of FindPoint()
unC0Rr
parents: 13960
diff changeset
    72
    #[inline]
13949
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
    73
    pub fn sqr(&self) -> Self {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    74
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    75
            sign_mask: 0,
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    76
            value: ((self.value as u128).pow(2) >> FRAC_BITS).saturating_into(),
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    77
            _marker: self._marker
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    78
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    79
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    80
15239
b2c086629fb8 optimize 64-bit sqrt some more
alfadur
parents: 15235
diff changeset
    81
    #[inline]
13949
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
    82
    pub fn sqrt(&self) -> Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    83
        debug_assert!(self.is_positive());
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    84
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    85
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    86
            sign_mask: POSITIVE_MASK,
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    87
            value: integral_sqrt(self.value) << (FRAC_BITS / 2),
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    88
            _marker: self._marker
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    89
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
    90
    }
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
    91
13948
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
    92
    #[inline]
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    93
    pub fn with_sign(&self, is_negative: bool) -> Self {
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
    94
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
    95
            sign_mask: bool_mask(is_negative),
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
    96
            ..*self
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
    97
        }
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
    98
    }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
    99
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   100
    #[inline]
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   101
    pub const fn with_sign_as(self, other: Self) -> Self {
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   102
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   103
            sign_mask: other.sign_mask,
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   104
            ..self
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   105
        }
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   106
    }
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   107
/*
13948
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   108
    #[inline]
14648
2e2b31cf0871 make stuff const
alfadur
parents: 14228
diff changeset
   109
    pub const fn point(self) -> FPPoint {
13948
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   110
        FPPoint::new(self, self)
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   111
    }
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   112
*/
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   113
    #[inline]
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   114
    const fn temp_i128(self) -> i128 {
15233
293250953317 fix u32 conversion sign
alfadur
parents: 15232
diff changeset
   115
        ((self.value ^ self.sign_mask) as i128).wrapping_sub(self.sign_mask as i128)
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   116
    }
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   117
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   118
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   119
impl<const FRAC_BITS: u8> From<i32> for FixedPoint<FRAC_BITS> {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   120
    #[inline]
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   121
    fn from(n: i32) -> Self {
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   122
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   123
            sign_mask: bool_mask(n < 0),
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   124
            value: (n.abs() as u64) << FRAC_BITS,
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   125
            _marker: PhantomData,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   126
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   127
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   128
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   129
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   130
impl<const FRAC_BITS: u8> From<u32> for FixedPoint<FRAC_BITS> {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   131
    #[inline]
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   132
    fn from(n: u32) -> Self {
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   133
        Self {
15233
293250953317 fix u32 conversion sign
alfadur
parents: 15232
diff changeset
   134
            sign_mask: POSITIVE_MASK,
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   135
            value: (n as u64) << FRAC_BITS,
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   136
            _marker: PhantomData,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   137
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   138
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   139
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   140
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   141
impl<const FRAC_BITS: u8> From<FixedPoint<FRAC_BITS>> for f64 {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   142
    #[inline]
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   143
    fn from(n: FixedPoint<FRAC_BITS>) -> Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   144
        if n.is_negative() {
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   145
            n.value as f64 / -(1i64 << FRAC_BITS) as f64
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   146
        } else {
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   147
            n.value as f64 / (1i64 << FRAC_BITS) as f64
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   148
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   149
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   150
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   151
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   152
impl<const FRAC_BITS: u8> PartialEq for FixedPoint<FRAC_BITS> {
13909
cd39e87d7a80 inline more operators in fpnum
unc0rr
parents: 13908
diff changeset
   153
    #[inline]
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   154
    fn eq(&self, other: &Self) -> bool {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   155
        self.value == other.value && (self.sign_mask == other.sign_mask || self.value == 0)
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   156
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   157
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   158
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   159
impl<const FRAC_BITS: u8> Eq for FixedPoint<FRAC_BITS> {}
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   160
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   161
impl<const FRAC_BITS: u8> PartialOrd for FixedPoint<FRAC_BITS> {
13909
cd39e87d7a80 inline more operators in fpnum
unc0rr
parents: 13908
diff changeset
   162
    #[inline]
16079
624b74443b53 Make FPNum use saturating calculations
unC0Rr
parents: 15860
diff changeset
   163
    fn partial_cmp(&self, rhs: &Self) -> Option<cmp::Ordering> {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   164
        Some(self.cmp(rhs))
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   165
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   166
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   167
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   168
impl<const FRAC_BITS: u8> Ord for FixedPoint<FRAC_BITS> {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   169
    #[inline]
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   170
    fn cmp(&self, rhs: &Self) -> cmp::Ordering {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   171
        self.temp_i128().cmp(&(rhs.temp_i128()))
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   172
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   173
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   174
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   175
impl<const FRAC_BITS: u8> ops::Add for FixedPoint<FRAC_BITS> {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   176
    type Output = Self;
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   177
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   178
    #[inline]
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   179
    fn add(self, rhs: Self) -> Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   180
        let tmp = self.temp_i128() + rhs.temp_i128();
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   181
        let mask = bool_mask(tmp < 0);
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   182
        Self {
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   183
            sign_mask: mask,
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   184
            value: ((tmp as u64) ^ mask).wrapping_sub(mask),
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   185
            _marker: PhantomData,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   186
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   187
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   188
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   189
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   190
impl<const FRAC_BITS: u8> ops::Sub for FixedPoint<FRAC_BITS> {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   191
    type Output = Self;
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   192
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   193
    #[inline]
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   194
    fn sub(self, mut rhs: Self) -> Self {
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   195
        rhs.sign_mask = !rhs.sign_mask;
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   196
        self + rhs
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   197
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   198
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   199
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   200
impl<const FRAC_BITS: u8> ops::Neg for FixedPoint<FRAC_BITS> {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   201
    type Output = Self;
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   202
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   203
    #[inline]
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   204
    fn neg(self) -> Self {
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   205
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   206
            sign_mask: !self.sign_mask,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   207
            value: self.value,
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   208
            _marker: PhantomData,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   209
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   210
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   211
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   212
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   213
impl<const FRAC_BITS: u8> ops::Mul for FixedPoint<FRAC_BITS> {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   214
    type Output = Self;
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   215
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   216
    #[inline]
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   217
    fn mul(self, rhs: Self) -> Self {
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   218
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   219
            sign_mask: self.sign_mask ^ rhs.sign_mask,
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   220
            value: ((self.value as u128 * rhs.value as u128) >> FRAC_BITS).saturating_into(),
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   221
            _marker: PhantomData,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   222
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   223
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   224
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   225
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   226
impl<const FRAC_BITS: u8> ops::Mul<i32> for FixedPoint<FRAC_BITS> {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   227
    type Output = Self;
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   228
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   229
    #[inline]
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   230
    fn mul(self, rhs: i32) -> Self {
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   231
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   232
            sign_mask: self.sign_mask ^ bool_mask(rhs < 0),
16079
624b74443b53 Make FPNum use saturating calculations
unC0Rr
parents: 15860
diff changeset
   233
            value: (self.value as u128 * rhs.abs() as u128).saturating_into(),
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   234
            _marker: PhantomData,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   235
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   236
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   237
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   238
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   239
impl<const FRAC_BITS: u8> ops::Div for FixedPoint<FRAC_BITS> {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   240
    type Output = Self;
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   241
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   242
    #[inline]
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   243
    fn div(self, rhs: Self) -> Self {
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   244
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   245
            sign_mask: self.sign_mask ^ rhs.sign_mask,
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   246
            value: (((self.value as u128) << FRAC_BITS) / rhs.value as u128).saturating_into(),
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   247
            _marker: PhantomData,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   248
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   249
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   250
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   251
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   252
impl<const FRAC_BITS: u8> ops::Div<i32> for FixedPoint<FRAC_BITS> {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   253
    type Output = Self;
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   254
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   255
    #[inline]
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   256
    fn div(self, rhs: i32) -> Self {
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   257
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   258
            sign_mask: self.sign_mask ^ bool_mask(rhs < 0),
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   259
            value: self.value / rhs.abs() as u64,
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   260
            _marker: PhantomData,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   261
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   262
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   263
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   264
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   265
impl<const FRAC_BITS: u8> ops::Div<u32> for FixedPoint<FRAC_BITS> {
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   266
    type Output = Self;
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   267
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   268
    #[inline]
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   269
    fn div(self, rhs: u32) -> Self {
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   270
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   271
            sign_mask: self.sign_mask,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   272
            value: self.value / rhs as u64,
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   273
            _marker: PhantomData,
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   274
        }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   275
    }
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   276
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   277
13949
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
   278
#[macro_export]
13911
9ae1184886db add fpnum literal macro
alfadur
parents: 13909
diff changeset
   279
macro_rules! fp {
14722
5e2c892b0222 allow fp! take denominator tokens
alfadur
parents: 14688
diff changeset
   280
    ($n: literal / $d: tt) => {
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   281
        FPNum::new($n, $d)
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   282
    };
14685
def1b9870078 match literals in fp macro
alfadur
parents: 14648
diff changeset
   283
    ($n: literal) => {
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   284
        FPNum::from($n)
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   285
    };
13911
9ae1184886db add fpnum literal macro
alfadur
parents: 13909
diff changeset
   286
}
9ae1184886db add fpnum literal macro
alfadur
parents: 13909
diff changeset
   287
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   288
const LINEARIZE_TRESHOLD: u64 = 0x1_0000;
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   289
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   290
#[derive(Clone, Copy, Debug)]
13949
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
   291
pub struct FPPoint {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   292
    x_sign_mask: u32,
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   293
    y_sign_mask: u32,
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   294
    x_value: u64,
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   295
    y_value: u64,
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   296
}
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   297
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   298
impl FPPoint {
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   299
    #[inline]
14648
2e2b31cf0871 make stuff const
alfadur
parents: 14228
diff changeset
   300
    pub const fn new(x: FPNum, y: FPNum) -> Self {
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   301
        Self {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   302
            x_sign_mask: x.sign_mask as u32,
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   303
            y_sign_mask: y.sign_mask as u32,
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   304
            x_value: x.value,
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   305
            y_value: y.value,
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   306
        }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   307
    }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   308
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   309
    #[inline]
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   310
    pub fn zero() -> FPPoint {
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   311
        FPPoint::new(fp!(0), fp!(0))
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   312
    }
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   313
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   314
    #[inline]
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   315
    pub fn unit_x() -> FPPoint {
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   316
        FPPoint::new(fp!(1), fp!(0))
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   317
    }
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   318
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   319
    #[inline]
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   320
    pub fn unit_y() -> FPPoint {
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   321
        FPPoint::new(fp!(0), fp!(1))
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   322
    }
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   323
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   324
    #[inline]
14648
2e2b31cf0871 make stuff const
alfadur
parents: 14228
diff changeset
   325
    pub const fn x(&self) -> FPNum {
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   326
        FPNum {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   327
            sign_mask: self.x_sign_mask as i32 as u64,
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   328
            value: self.x_value,
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   329
            _marker: PhantomData,
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   330
        }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   331
    }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   332
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   333
    #[inline]
14648
2e2b31cf0871 make stuff const
alfadur
parents: 14228
diff changeset
   334
    pub const fn y(&self) -> FPNum {
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   335
        FPNum {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   336
            sign_mask: self.y_sign_mask as i32 as u64,
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   337
            value: self.y_value,
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   338
            _marker: PhantomData,
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   339
        }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   340
    }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   341
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   342
    #[inline]
14199
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents: 14161
diff changeset
   343
    pub fn is_zero(&self) -> bool {
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents: 14161
diff changeset
   344
        self.x().is_zero() && self.y().is_zero()
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents: 14161
diff changeset
   345
    }
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents: 14161
diff changeset
   346
a4c17cfaa4c9 split hwphysics into modules
alfadur
parents: 14161
diff changeset
   347
    #[inline]
13949
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
   348
    pub fn max_norm(&self) -> FPNum {
16079
624b74443b53 Make FPNum use saturating calculations
unC0Rr
parents: 15860
diff changeset
   349
        cmp::max(self.x().abs(), self.y().abs())
13949
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
   350
    }
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
   351
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
   352
    #[inline]
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
   353
    pub fn sqr_distance(&self) -> FPNum {
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   354
        self.x().sqr() + self.y().sqr()
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   355
    }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   356
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   357
    #[inline]
13949
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
   358
    pub fn distance(&self) -> FPNum {
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   359
        let r = self.x_value | self.y_value;
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   360
        if r < LINEARIZE_TRESHOLD {
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   361
            FPNum::from(r as u32)
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   362
        } else {
15235
58a0f2a6527b optimize sqrts
alfadur
parents: 15234
diff changeset
   363
            let sqr: u128 = (self.x_value as u128).pow(2) + (self.y_value as u128).pow(2);
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   364
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   365
            FPNum {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   366
                sign_mask: POSITIVE_MASK,
15860
c910381d1ea9 limit scope of u128 operations in sqrt
alfadur
parents: 15859
diff changeset
   367
                value: integral_sqrt_ext(sqr),
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   368
                _marker: PhantomData,
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   369
            }
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   370
        }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   371
    }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   372
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   373
    #[inline]
13949
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
   374
    pub fn is_in_range(&self, radius: FPNum) -> bool {
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
   375
        self.max_norm() < radius && self.sqr_distance() < radius.sqr()
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   376
    }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   377
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   378
    #[inline]
13949
5fdc41cd0841 make methods public
alfadur
parents: 13948
diff changeset
   379
    pub fn dot(&self, other: &FPPoint) -> FPNum {
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   380
        self.x() * other.x() + self.y() * other.y()
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   381
    }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   382
}
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   383
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   384
impl PartialEq for FPPoint {
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   385
    #[inline]
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   386
    fn eq(&self, other: &Self) -> bool {
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   387
        self.x() == other.x() && self.y() == other.y()
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   388
    }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   389
}
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   390
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   391
impl Eq for FPPoint {}
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   392
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   393
impl ops::Neg for FPPoint {
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   394
    type Output = Self;
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   395
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   396
    #[inline]
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   397
    fn neg(self) -> Self {
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   398
        Self::new(-self.x(), -self.y())
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   399
    }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   400
}
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   401
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   402
macro_rules! bin_op_impl {
13948
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   403
    ($op: ty, $name: tt) => {
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   404
        impl $op for FPPoint {
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   405
            type Output = Self;
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   406
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   407
            #[inline]
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   408
            fn $name(self, rhs: Self) -> Self {
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   409
                Self::new(self.x().$name(rhs.x()), self.y().$name(rhs.y()))
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   410
            }
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   411
        }
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   412
    };
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   413
}
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   414
13953
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   415
macro_rules! right_scalar_bin_op_impl {
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   416
    ($($op: tt)::+, $name: tt) => {
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   417
        impl $($op)::+<FPNum> for FPPoint {
13948
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   418
            type Output = Self;
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   419
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   420
            #[inline]
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   421
            fn $name(self, rhs: FPNum) -> Self {
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   422
                Self::new(self.x().$name(rhs),
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   423
                          self.y().$name(rhs))
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   424
            }
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   425
        }
14161
3078123e84ea Bezierize land outline
unc0rr
parents: 14160
diff changeset
   426
    };
3078123e84ea Bezierize land outline
unc0rr
parents: 14160
diff changeset
   427
    ($($op: tt)::+<$arg: tt>, $name: tt) => {
3078123e84ea Bezierize land outline
unc0rr
parents: 14160
diff changeset
   428
        impl $($op)::+<$arg> for FPPoint {
3078123e84ea Bezierize land outline
unc0rr
parents: 14160
diff changeset
   429
            type Output = Self;
3078123e84ea Bezierize land outline
unc0rr
parents: 14160
diff changeset
   430
3078123e84ea Bezierize land outline
unc0rr
parents: 14160
diff changeset
   431
            #[inline]
3078123e84ea Bezierize land outline
unc0rr
parents: 14160
diff changeset
   432
            fn $name(self, rhs: $arg) -> Self {
3078123e84ea Bezierize land outline
unc0rr
parents: 14160
diff changeset
   433
                Self::new(self.x().$name(rhs),
3078123e84ea Bezierize land outline
unc0rr
parents: 14160
diff changeset
   434
                          self.y().$name(rhs))
3078123e84ea Bezierize land outline
unc0rr
parents: 14160
diff changeset
   435
            }
3078123e84ea Bezierize land outline
unc0rr
parents: 14160
diff changeset
   436
        }
13948
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   437
    }
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   438
}
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   439
13953
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   440
macro_rules! left_scalar_bin_op_impl {
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   441
    ($($op: tt)::+, $name: tt) => {
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   442
        impl $($op)::+<FPPoint> for FPNum {
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   443
            type Output = FPPoint;
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   444
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   445
            #[inline]
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   446
            fn $name(self, rhs: FPPoint) -> Self::Output {
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   447
                Self::Output::new(self.$name(rhs.x()),
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   448
                                  self.$name(rhs.y()))
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   449
            }
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   450
        }
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   451
    }
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   452
}
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   453
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   454
bin_op_impl!(ops::Add, add);
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   455
bin_op_impl!(ops::Sub, sub);
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   456
bin_op_impl!(ops::Mul, mul);
13950
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   457
bin_op_impl!(ops::Div, div);
13953
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   458
right_scalar_bin_op_impl!(ops::Add, add);
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   459
right_scalar_bin_op_impl!(ops::Mul, mul);
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   460
right_scalar_bin_op_impl!(ops::Sub, sub);
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   461
right_scalar_bin_op_impl!(ops::Div, div);
14161
3078123e84ea Bezierize land outline
unc0rr
parents: 14160
diff changeset
   462
right_scalar_bin_op_impl!(ops::Div<u32>, div);
13953
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   463
left_scalar_bin_op_impl!(ops::Mul, mul);
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   464
13950
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   465
macro_rules! bin_assign_op_impl {
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   466
    ($typ: tt, $($op: tt)::+, $name: tt, $delegate: tt) => {
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   467
        bin_assign_op_impl!($typ, $($op)::+<$typ>, $name, $delegate);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   468
    };
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   469
    ($typ: tt, $($op: tt)::+<$arg: tt>, $name: tt, $delegate: tt) => {
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   470
        impl $($op)::+<$arg> for $typ {
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   471
            #[inline]
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   472
            fn $name(&mut self, rhs: $arg) {
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   473
                *self = *self $delegate rhs;
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   474
            }
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   475
        }
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   476
    }
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   477
}
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   478
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   479
bin_assign_op_impl!(FPNum, ops::AddAssign, add_assign, +);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   480
bin_assign_op_impl!(FPNum, ops::SubAssign, sub_assign, -);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   481
bin_assign_op_impl!(FPNum, ops::MulAssign, mul_assign, *);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   482
bin_assign_op_impl!(FPNum, ops::DivAssign, div_assign, /);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   483
bin_assign_op_impl!(FPNum, ops::MulAssign<i32>, mul_assign, *);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   484
bin_assign_op_impl!(FPNum, ops::DivAssign<i32>, div_assign, /);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   485
bin_assign_op_impl!(FPNum, ops::DivAssign<u32>, div_assign, /);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   486
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   487
bin_assign_op_impl!(FPPoint, ops::AddAssign, add_assign, +);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   488
bin_assign_op_impl!(FPPoint, ops::SubAssign, sub_assign, -);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   489
bin_assign_op_impl!(FPPoint, ops::MulAssign, mul_assign, *);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   490
bin_assign_op_impl!(FPPoint, ops::DivAssign, div_assign, /);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   491
bin_assign_op_impl!(FPPoint, ops::AddAssign<FPNum>, add_assign, +);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   492
bin_assign_op_impl!(FPPoint, ops::SubAssign<FPNum>, sub_assign, -);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   493
bin_assign_op_impl!(FPPoint, ops::MulAssign<FPNum>, mul_assign, *);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   494
bin_assign_op_impl!(FPPoint, ops::DivAssign<FPNum>, div_assign, /);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   495
15239
b2c086629fb8 optimize 64-bit sqrt some more
alfadur
parents: 15235
diff changeset
   496
pub fn integral_sqrt(value: u64) -> u64 {
15859
42109eb6ef51 port sqrt optimization to u128 case
alfadur
parents: 15240
diff changeset
   497
    let mut digits = (64u32 - 1).saturating_sub(value.leading_zeros()) & 0xFE;
15239
b2c086629fb8 optimize 64-bit sqrt some more
alfadur
parents: 15235
diff changeset
   498
    let mut result = if value == 0 { 0u64 } else { 1u64 };
15235
58a0f2a6527b optimize sqrts
alfadur
parents: 15234
diff changeset
   499
15239
b2c086629fb8 optimize 64-bit sqrt some more
alfadur
parents: 15235
diff changeset
   500
    while digits != 0 {
b2c086629fb8 optimize 64-bit sqrt some more
alfadur
parents: 15235
diff changeset
   501
        result <<= 1;
b2c086629fb8 optimize 64-bit sqrt some more
alfadur
parents: 15235
diff changeset
   502
        if (result + 1).pow(2) <= value >> (digits - 2) {
b2c086629fb8 optimize 64-bit sqrt some more
alfadur
parents: 15235
diff changeset
   503
            result += 1;
b2c086629fb8 optimize 64-bit sqrt some more
alfadur
parents: 15235
diff changeset
   504
        }
b2c086629fb8 optimize 64-bit sqrt some more
alfadur
parents: 15235
diff changeset
   505
        digits -= 2;
b2c086629fb8 optimize 64-bit sqrt some more
alfadur
parents: 15235
diff changeset
   506
    }
15235
58a0f2a6527b optimize sqrts
alfadur
parents: 15234
diff changeset
   507
58a0f2a6527b optimize sqrts
alfadur
parents: 15234
diff changeset
   508
    result
58a0f2a6527b optimize sqrts
alfadur
parents: 15234
diff changeset
   509
}
58a0f2a6527b optimize sqrts
alfadur
parents: 15234
diff changeset
   510
15860
c910381d1ea9 limit scope of u128 operations in sqrt
alfadur
parents: 15859
diff changeset
   511
pub fn integral_sqrt_ext(value: u128) -> u64 {
15859
42109eb6ef51 port sqrt optimization to u128 case
alfadur
parents: 15240
diff changeset
   512
    let mut digits = (128u32 - 1).saturating_sub(value.leading_zeros()) & 0xFE;
15860
c910381d1ea9 limit scope of u128 operations in sqrt
alfadur
parents: 15859
diff changeset
   513
    let mut result = if value == 0 { 0u64 } else { 1u64 };
15235
58a0f2a6527b optimize sqrts
alfadur
parents: 15234
diff changeset
   514
15859
42109eb6ef51 port sqrt optimization to u128 case
alfadur
parents: 15240
diff changeset
   515
    while digits != 0 {
42109eb6ef51 port sqrt optimization to u128 case
alfadur
parents: 15240
diff changeset
   516
        result <<= 1;
15860
c910381d1ea9 limit scope of u128 operations in sqrt
alfadur
parents: 15859
diff changeset
   517
        if ((result + 1) as u128).pow(2) <= value >> (digits - 2) {
15859
42109eb6ef51 port sqrt optimization to u128 case
alfadur
parents: 15240
diff changeset
   518
            result += 1;
42109eb6ef51 port sqrt optimization to u128 case
alfadur
parents: 15240
diff changeset
   519
        }
42109eb6ef51 port sqrt optimization to u128 case
alfadur
parents: 15240
diff changeset
   520
        digits -= 2;
42109eb6ef51 port sqrt optimization to u128 case
alfadur
parents: 15240
diff changeset
   521
    }
15235
58a0f2a6527b optimize sqrts
alfadur
parents: 15234
diff changeset
   522
58a0f2a6527b optimize sqrts
alfadur
parents: 15234
diff changeset
   523
    result
58a0f2a6527b optimize sqrts
alfadur
parents: 15234
diff changeset
   524
}
58a0f2a6527b optimize sqrts
alfadur
parents: 15234
diff changeset
   525
15239
b2c086629fb8 optimize 64-bit sqrt some more
alfadur
parents: 15235
diff changeset
   526
#[inline]
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   527
pub fn distance<T, const FRAC_BITS: u8>(x: T, y: T) -> FixedPoint<FRAC_BITS>
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   528
where
16079
624b74443b53 Make FPNum use saturating calculations
unC0Rr
parents: 15860
diff changeset
   529
    T: Into<i128> + std::fmt::Debug,
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   530
{
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   531
    let [x_squared, y_squared] = [x, y].map(|i| (i.into().pow(2) as u128).saturating_mul(1 << FRAC_BITS << FRAC_BITS));
16079
624b74443b53 Make FPNum use saturating calculations
unC0Rr
parents: 15860
diff changeset
   532
    let sqr: u128 = x_squared.saturating_add(y_squared);
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   533
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   534
    FixedPoint {
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   535
        sign_mask: POSITIVE_MASK,
15860
c910381d1ea9 limit scope of u128 operations in sqrt
alfadur
parents: 15859
diff changeset
   536
        value: integral_sqrt_ext(sqr),
16081
6633961698ad Make wider range of numbers representable with FPNum on the cost of smaller precision
unC0Rr
parents: 16079
diff changeset
   537
        _marker: PhantomData,
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   538
    }
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   539
}
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   540
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   541
/* TODO:
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   542
 AngleSin
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   543
 AngleCos
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   544
*/
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   545
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   546
#[test]
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   547
fn basics() {
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   548
    let n = fp!(15 / 2);
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   549
    assert!(n.is_positive());
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   550
    assert!(!n.is_negative());
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   551
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   552
    assert!(!(-n).is_positive());
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   553
    assert!((-n).is_negative());
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   554
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   555
    assert_eq!(-(-n), n);
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   556
    assert_eq!((-n).abs(), n);
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   557
    assert_eq!(-n, fp!(-15 / 2));
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   558
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   559
    assert_eq!(n.round(), 7);
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   560
    assert_eq!((-n).round(), -7);
15240
b71bae455926 fix fpnum to f64 conversion
alfadur
parents: 15239
diff changeset
   561
b71bae455926 fix fpnum to f64 conversion
alfadur
parents: 15239
diff changeset
   562
    assert_eq!(f64::from(fp!(5/2)), 2.5f64);
15860
c910381d1ea9 limit scope of u128 operations in sqrt
alfadur
parents: 15859
diff changeset
   563
c910381d1ea9 limit scope of u128 operations in sqrt
alfadur
parents: 15859
diff changeset
   564
    assert_eq!(integral_sqrt_ext(0xFFFF_FFFF_FFFF_FFFF_FFFF_FFFF_FFFF_FFFF), 0xFFFF_FFFF_FFFF_FFFF);
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   565
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   566
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   567
#[test]
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   568
fn zero() {
13911
9ae1184886db add fpnum literal macro
alfadur
parents: 13909
diff changeset
   569
    let z = fp!(0);
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   570
    let n = fp!(15 / 2);
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   571
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   572
    assert!(z.is_zero());
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   573
    assert!(z.is_positive());
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   574
    assert!((-z).is_negative());
13907
2bfd7472ef1d Add some more tests
unc0rr
parents: 13905
diff changeset
   575
    assert_eq!(n - n, z);
2bfd7472ef1d Add some more tests
unc0rr
parents: 13905
diff changeset
   576
    assert_eq!(-n + n, z);
13947
2717a5289d88 fix test
alfadur
parents: 13946
diff changeset
   577
    assert_eq!(n.with_sign_as(-n), -n);
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   578
}
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   579
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   580
#[test]
13908
fbbb4fcd6a75 delegate cmp to rustlib
alfadur
parents: 13907
diff changeset
   581
fn ord() {
13911
9ae1184886db add fpnum literal macro
alfadur
parents: 13909
diff changeset
   582
    let z = fp!(0);
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   583
    let n1_5 = fp!(3 / 2);
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   584
    let n2_25 = fp!(9 / 4);
13908
fbbb4fcd6a75 delegate cmp to rustlib
alfadur
parents: 13907
diff changeset
   585
fbbb4fcd6a75 delegate cmp to rustlib
alfadur
parents: 13907
diff changeset
   586
    assert!(!(z > z));
fbbb4fcd6a75 delegate cmp to rustlib
alfadur
parents: 13907
diff changeset
   587
    assert!(!(z < z));
fbbb4fcd6a75 delegate cmp to rustlib
alfadur
parents: 13907
diff changeset
   588
    assert!(n2_25 > n1_5);
fbbb4fcd6a75 delegate cmp to rustlib
alfadur
parents: 13907
diff changeset
   589
    assert!(-n2_25 < n1_5);
fbbb4fcd6a75 delegate cmp to rustlib
alfadur
parents: 13907
diff changeset
   590
    assert!(-n2_25 < -n1_5);
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   591
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   592
    assert_eq!(n1_5.signum(), 1);
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   593
    assert_eq!((-n1_5).signum(), -1);
13908
fbbb4fcd6a75 delegate cmp to rustlib
alfadur
parents: 13907
diff changeset
   594
}
fbbb4fcd6a75 delegate cmp to rustlib
alfadur
parents: 13907
diff changeset
   595
fbbb4fcd6a75 delegate cmp to rustlib
alfadur
parents: 13907
diff changeset
   596
#[test]
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   597
fn arith() {
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   598
    let n1_5 = fp!(3 / 2);
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   599
    let n2_25 = fp!(9 / 4);
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   600
    let n_0_15 = fp!(-15 / 100);
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   601
13911
9ae1184886db add fpnum literal macro
alfadur
parents: 13909
diff changeset
   602
    assert_eq!(n1_5 + n1_5, fp!(3));
9ae1184886db add fpnum literal macro
alfadur
parents: 13909
diff changeset
   603
    assert_eq!(-n1_5 - n1_5, fp!(-3));
15232
924f7e38815e optimize fpnum operations
alfadur
parents: 14722
diff changeset
   604
    assert_eq!(n1_5 - n1_5, fp!(0));
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   605
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   606
    assert_eq!(n1_5 * n1_5, n2_25);
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   607
    assert_eq!(-n1_5 * -n1_5, n2_25);
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   608
    assert_eq!(n1_5 * -n1_5, -n2_25);
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   609
    assert_eq!(-n1_5 * n1_5, -n2_25);
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   610
13907
2bfd7472ef1d Add some more tests
unc0rr
parents: 13905
diff changeset
   611
    assert_eq!(-n2_25 / -n1_5, n1_5);
2bfd7472ef1d Add some more tests
unc0rr
parents: 13905
diff changeset
   612
    assert_eq!(n1_5 / -10, n_0_15);
2bfd7472ef1d Add some more tests
unc0rr
parents: 13905
diff changeset
   613
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   614
    assert_eq!(n1_5.sqr(), n2_25);
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   615
    assert_eq!((-n1_5).sqr(), n2_25);
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   616
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   617
    assert_eq!(n2_25.sqrt(), n1_5);
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   618
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   619
    assert_eq!((n1_5 * n1_5 * n1_5.sqr()).sqrt(), n2_25);
13950
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   620
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   621
    let mut m = fp!(1);
ff77c9920007 add opassign implementations
alfadur
parents: 13949
diff changeset
   622
    m += n1_5;
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   623
    assert_eq!(m, fp!(5 / 2));
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   624
}
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   625
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   626
#[test]
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   627
fn test_distance_high_values() {
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   628
    assert_eq!(distance(1_000_000i32, 0), fp!(1_000_000));
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   629
    assert_eq!(
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   630
        FPPoint::new(fp!(1_000_000), fp!(0)).distance(),
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   631
        fp!(1_000_000)
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   632
    );
13905
b172a5d40eee Reimplement hwFloat library in rust for future use
unc0rr
parents:
diff changeset
   633
}
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   634
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   635
#[test]
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   636
fn point() {
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   637
    let z = FPPoint::zero();
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   638
    let n = fp!(16 / 9);
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   639
    let p = FPPoint::new(fp!(1), fp!(-2));
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   640
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   641
    assert_eq!(p.sqr_distance(), fp!(5));
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   642
    assert_eq!(p + -p, FPPoint::zero());
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   643
    assert_eq!(p * z, z);
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   644
    assert_eq!(p.dot(&z), fp!(0));
13953
7bb60596c27e add left scalar operations to fpnum
alfadur
parents: 13950
diff changeset
   645
    assert_eq!(n * p, p * n);
13946
2ee07e751171 add more fpnum functions
alfadur
parents: 13926
diff changeset
   646
    assert_eq!(distance(4, 3), fp!(5));
13948
cf28d7a2b7fe add scalar operations
alfadur
parents: 13947
diff changeset
   647
    assert_eq!(p * fp!(-3), FPPoint::new(fp!(-3), fp!(6)));
13960
665b4c6612ee fix fppoint.max_norm
alfadur
parents: 13953
diff changeset
   648
    assert_eq!(p.max_norm(), fp!(2));
14143
c27461e6a9eb Implement non-overflowing calculations for high values
unc0rr
parents: 14102
diff changeset
   649
}