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