1 use std::{cmp, ops, ops::Shl}; |
1 use std::{cmp, ops, ops::Shl}; |
|
2 |
|
3 const POSITIVE_MASK: u64 = 0x0000_0000_0000_0000; |
|
4 const NEGATIVE_MASK: u64 = 0xFFFF_FFFF_FFFF_FFFF; |
|
5 |
|
6 #[inline] |
|
7 fn bool_mask(is_negative: bool) -> u64 { |
|
8 if is_negative { |
|
9 NEGATIVE_MASK |
|
10 } else { |
|
11 POSITIVE_MASK |
|
12 } |
|
13 } |
2 |
14 |
3 #[derive(Clone, Debug, Copy)] |
15 #[derive(Clone, Debug, Copy)] |
4 pub struct FPNum { |
16 pub struct FPNum { |
5 is_negative: bool, |
17 sign_mask: u64, |
6 value: u64, |
18 value: u64, |
7 } |
19 } |
8 |
20 |
9 impl FPNum { |
21 impl FPNum { |
10 #[inline] |
22 #[inline] |
12 FPNum::from(numerator) / denominator |
24 FPNum::from(numerator) / denominator |
13 } |
25 } |
14 |
26 |
15 #[inline] |
27 #[inline] |
16 pub fn signum(&self) -> i8 { |
28 pub fn signum(&self) -> i8 { |
17 if self.is_negative { |
29 (1u8 ^ self.sign_mask as u8).wrapping_sub(self.sign_mask as u8) as i8 |
18 -1 |
|
19 } else { |
|
20 1 |
|
21 } |
|
22 } |
30 } |
23 |
31 |
24 #[inline] |
32 #[inline] |
25 pub const fn is_negative(&self) -> bool { |
33 pub const fn is_negative(&self) -> bool { |
26 self.is_negative |
34 self.sign_mask != POSITIVE_MASK |
27 } |
35 } |
28 |
36 |
29 #[inline] |
37 #[inline] |
30 pub const fn is_positive(&self) -> bool { |
38 pub const fn is_positive(&self) -> bool { |
31 !self.is_negative |
39 self.sign_mask == POSITIVE_MASK |
32 } |
40 } |
33 |
41 |
34 #[inline] |
42 #[inline] |
35 pub const fn is_zero(&self) -> bool { |
43 pub const fn is_zero(&self) -> bool { |
36 self.value == 0 |
44 self.value == 0 |
37 } |
45 } |
38 |
46 |
39 #[inline] |
47 #[inline] |
40 pub const fn abs(&self) -> Self { |
48 pub const fn abs(&self) -> Self { |
41 Self { |
49 Self { |
42 is_negative: false, |
50 sign_mask: POSITIVE_MASK, |
43 value: self.value, |
51 value: self.value, |
44 } |
52 } |
45 } |
53 } |
46 |
54 |
47 #[inline] |
55 #[inline] |
48 pub fn round(&self) -> i32 { |
56 pub fn round(&self) -> i32 { |
49 if self.is_negative { |
57 ((self.value >> 32) as i32 ^ self.sign_mask as i32).wrapping_sub(self.sign_mask as i32) |
50 -((self.value >> 32) as i32) |
|
51 } else { |
|
52 (self.value >> 32) as i32 |
|
53 } |
|
54 } |
58 } |
55 |
59 |
56 #[inline] |
60 #[inline] |
57 pub const fn abs_round(&self) -> u32 { |
61 pub const fn abs_round(&self) -> u32 { |
58 (self.value >> 32) as u32 |
62 (self.value >> 32) as u32 |
59 } |
63 } |
60 |
64 |
61 #[inline] |
65 #[inline] |
62 pub fn sqr(&self) -> Self { |
66 pub fn sqr(&self) -> Self { |
63 Self { |
67 Self { |
64 is_negative: false, |
68 sign_mask: 0, |
65 value: ((self.value as u128).pow(2) >> 32) as u64, |
69 value: ((self.value as u128).pow(2) >> 32) as u64, |
66 } |
70 } |
67 } |
71 } |
68 |
72 |
69 pub fn sqrt(&self) -> Self { |
73 pub fn sqrt(&self) -> Self { |
70 debug_assert!(!self.is_negative); |
74 debug_assert!(self.is_positive()); |
71 |
75 |
72 let mut t: u64 = 0x4000000000000000; |
76 let mut t: u64 = 0x4000_0000_0000_0000; |
73 let mut r: u64 = 0; |
77 let mut r: u64 = 0; |
74 let mut q = self.value; |
78 let mut q = self.value; |
75 |
79 |
76 for _ in 0..32 { |
80 for _ in 0..32 { |
77 let s = r + t; |
81 let s = r + t; |
83 } |
87 } |
84 t >>= 2; |
88 t >>= 2; |
85 } |
89 } |
86 |
90 |
87 Self { |
91 Self { |
88 is_negative: false, |
92 sign_mask: POSITIVE_MASK, |
89 value: r << 16, |
93 value: r << 16, |
90 } |
94 } |
91 } |
95 } |
92 |
96 |
93 #[inline] |
97 #[inline] |
94 pub const fn with_sign(&self, is_negative: bool) -> FPNum { |
98 pub fn with_sign(&self, is_negative: bool) -> FPNum { |
95 FPNum { |
99 FPNum { |
96 is_negative, |
100 sign_mask: bool_mask(is_negative), |
97 ..*self |
101 ..*self |
98 } |
102 } |
99 } |
103 } |
100 |
104 |
101 #[inline] |
105 #[inline] |
102 pub const fn with_sign_as(self, other: FPNum) -> FPNum { |
106 pub const fn with_sign_as(self, other: FPNum) -> FPNum { |
103 self.with_sign(other.is_negative) |
107 FPNum { |
|
108 sign_mask: other.sign_mask, |
|
109 ..self |
|
110 } |
104 } |
111 } |
105 |
112 |
106 #[inline] |
113 #[inline] |
107 pub const fn point(self) -> FPPoint { |
114 pub const fn point(self) -> FPPoint { |
108 FPPoint::new(self, self) |
115 FPPoint::new(self, self) |
109 } |
116 } |
|
117 |
|
118 #[inline] |
|
119 const fn temp_i128(self) -> i128 { |
|
120 ((self.value ^ self.sign_mask) as u128 as i128).wrapping_sub(self.sign_mask as i128) |
|
121 } |
110 } |
122 } |
111 |
123 |
112 impl From<i32> for FPNum { |
124 impl From<i32> for FPNum { |
113 #[inline] |
125 #[inline] |
114 fn from(n: i32) -> Self { |
126 fn from(n: i32) -> Self { |
115 FPNum { |
127 FPNum { |
116 is_negative: n < 0, |
128 sign_mask: bool_mask(n < 0), |
117 value: (n.abs() as u64) << 32, |
129 value: (n.abs() as u64) << 32, |
118 } |
130 } |
119 } |
131 } |
120 } |
132 } |
121 |
133 |
122 impl From<u32> for FPNum { |
134 impl From<u32> for FPNum { |
123 #[inline] |
135 #[inline] |
124 fn from(n: u32) -> Self { |
136 fn from(n: u32) -> Self { |
125 Self { |
137 Self { |
126 is_negative: false, |
138 sign_mask: NEGATIVE_MASK, |
127 value: (n as u64) << 32, |
139 value: (n as u64) << 32, |
128 } |
140 } |
129 } |
141 } |
130 } |
142 } |
131 |
143 |
132 impl From<FPNum> for f64 { |
144 impl From<FPNum> for f64 { |
133 #[inline] |
145 #[inline] |
134 fn from(n: FPNum) -> Self { |
146 fn from(n: FPNum) -> Self { |
135 if n.is_negative { |
147 if n.is_negative() { |
136 n.value as f64 / (-0x10000000 as f64) |
148 n.value as f64 / (-0x10000000 as f64) |
137 } else { |
149 } else { |
138 n.value as f64 / 0x10000000 as f64 |
150 n.value as f64 / 0x10000000 as f64 |
139 } |
151 } |
140 } |
152 } |
141 } |
153 } |
142 |
154 |
143 impl PartialEq for FPNum { |
155 impl PartialEq for FPNum { |
144 #[inline] |
156 #[inline] |
145 fn eq(&self, other: &Self) -> bool { |
157 fn eq(&self, other: &Self) -> bool { |
146 self.value == other.value && (self.is_negative == other.is_negative || self.value == 0) |
158 self.value == other.value && (self.sign_mask == other.sign_mask || self.value == 0) |
147 } |
159 } |
148 } |
160 } |
149 |
161 |
150 impl Eq for FPNum {} |
162 impl Eq for FPNum {} |
151 |
163 |
157 } |
169 } |
158 |
170 |
159 impl Ord for FPNum { |
171 impl Ord for FPNum { |
160 #[inline] |
172 #[inline] |
161 fn cmp(&self, rhs: &Self) -> cmp::Ordering { |
173 fn cmp(&self, rhs: &Self) -> cmp::Ordering { |
162 #[inline] |
174 self.temp_i128().cmp(&(rhs.temp_i128())) |
163 fn extend(n: &FPNum) -> i128 { |
|
164 if n.is_negative { |
|
165 -(n.value as i128) |
|
166 } else { |
|
167 n.value as i128 |
|
168 } |
|
169 } |
|
170 extend(self).cmp(&(extend(rhs))) |
|
171 } |
175 } |
172 } |
176 } |
173 |
177 |
174 impl ops::Add for FPNum { |
178 impl ops::Add for FPNum { |
175 type Output = Self; |
179 type Output = Self; |
176 |
180 |
177 #[inline] |
181 #[inline] |
178 fn add(self, rhs: Self) -> Self { |
182 fn add(self, rhs: Self) -> Self { |
179 if self.is_negative == rhs.is_negative { |
183 let tmp = self.temp_i128() + rhs.temp_i128(); |
180 Self { |
184 let mask = bool_mask(tmp < 0); |
181 is_negative: self.is_negative, |
185 Self { |
182 value: self.value + rhs.value, |
186 sign_mask: mask, |
183 } |
187 value: ((tmp as u64) ^ mask).wrapping_sub(mask), |
184 } else if self.value > rhs.value { |
|
185 Self { |
|
186 is_negative: self.is_negative, |
|
187 value: self.value - rhs.value, |
|
188 } |
|
189 } else { |
|
190 Self { |
|
191 is_negative: rhs.is_negative, |
|
192 value: rhs.value - self.value, |
|
193 } |
|
194 } |
188 } |
195 } |
189 } |
196 } |
190 } |
197 |
191 |
198 impl ops::Sub for FPNum { |
192 impl ops::Sub for FPNum { |
199 type Output = Self; |
193 type Output = Self; |
200 |
194 |
201 #[inline] |
195 #[inline] |
202 fn sub(self, rhs: Self) -> Self { |
196 fn sub(self, mut rhs: Self) -> Self { |
203 if self.is_negative == rhs.is_negative { |
197 rhs.sign_mask = !rhs.sign_mask; |
204 if self.value > rhs.value { |
198 self + rhs |
205 Self { |
|
206 is_negative: self.is_negative, |
|
207 value: self.value - rhs.value, |
|
208 } |
|
209 } else { |
|
210 Self { |
|
211 is_negative: !rhs.is_negative, |
|
212 value: rhs.value - self.value, |
|
213 } |
|
214 } |
|
215 } else { |
|
216 Self { |
|
217 is_negative: self.is_negative, |
|
218 value: self.value + rhs.value, |
|
219 } |
|
220 } |
|
221 } |
199 } |
222 } |
200 } |
223 |
201 |
224 impl ops::Neg for FPNum { |
202 impl ops::Neg for FPNum { |
225 type Output = Self; |
203 type Output = Self; |
226 |
204 |
227 #[inline] |
205 #[inline] |
228 fn neg(self) -> Self { |
206 fn neg(self) -> Self { |
229 Self { |
207 Self { |
230 is_negative: !self.is_negative, |
208 sign_mask: !self.sign_mask, |
231 value: self.value, |
209 value: self.value, |
232 } |
210 } |
233 } |
211 } |
234 } |
212 } |
235 |
213 |
305 |
283 |
306 const LINEARIZE_TRESHOLD: u64 = 0x1_0000; |
284 const LINEARIZE_TRESHOLD: u64 = 0x1_0000; |
307 |
285 |
308 #[derive(Clone, Copy, Debug)] |
286 #[derive(Clone, Copy, Debug)] |
309 pub struct FPPoint { |
287 pub struct FPPoint { |
310 x_is_negative: bool, |
288 x_sign_mask: u32, |
311 y_is_negative: bool, |
289 y_sign_mask: u32, |
312 x_value: u64, |
290 x_value: u64, |
313 y_value: u64, |
291 y_value: u64, |
314 } |
292 } |
315 |
293 |
316 impl FPPoint { |
294 impl FPPoint { |
317 #[inline] |
295 #[inline] |
318 pub const fn new(x: FPNum, y: FPNum) -> Self { |
296 pub const fn new(x: FPNum, y: FPNum) -> Self { |
319 Self { |
297 Self { |
320 x_is_negative: x.is_negative, |
298 x_sign_mask: x.sign_mask as u32, |
321 y_is_negative: y.is_negative, |
299 y_sign_mask: y.sign_mask as u32, |
322 x_value: x.value, |
300 x_value: x.value, |
323 y_value: y.value, |
301 y_value: y.value, |
324 } |
302 } |
325 } |
303 } |
326 |
304 |
576 let z = fp!(0); |
554 let z = fp!(0); |
577 let n = fp!(15 / 2); |
555 let n = fp!(15 / 2); |
578 |
556 |
579 assert!(z.is_zero()); |
557 assert!(z.is_zero()); |
580 assert!(z.is_positive()); |
558 assert!(z.is_positive()); |
581 assert!((-z).is_negative); |
559 assert!((-z).is_negative()); |
582 assert_eq!(n - n, z); |
560 assert_eq!(n - n, z); |
583 assert_eq!(-n + n, z); |
561 assert_eq!(-n + n, z); |
584 assert_eq!(n.with_sign_as(-n), -n); |
562 assert_eq!(n.with_sign_as(-n), -n); |
585 } |
563 } |
586 |
564 |
593 assert!(!(z > z)); |
571 assert!(!(z > z)); |
594 assert!(!(z < z)); |
572 assert!(!(z < z)); |
595 assert!(n2_25 > n1_5); |
573 assert!(n2_25 > n1_5); |
596 assert!(-n2_25 < n1_5); |
574 assert!(-n2_25 < n1_5); |
597 assert!(-n2_25 < -n1_5); |
575 assert!(-n2_25 < -n1_5); |
|
576 |
|
577 assert_eq!(n1_5.signum(), 1); |
|
578 assert_eq!((-n1_5).signum(), -1); |
598 } |
579 } |
599 |
580 |
600 #[test] |
581 #[test] |
601 fn arith() { |
582 fn arith() { |
602 let n1_5 = fp!(3 / 2); |
583 let n1_5 = fp!(3 / 2); |
603 let n2_25 = fp!(9 / 4); |
584 let n2_25 = fp!(9 / 4); |
604 let n_0_15 = fp!(-15 / 100); |
585 let n_0_15 = fp!(-15 / 100); |
605 |
586 |
606 assert_eq!(n1_5 + n1_5, fp!(3)); |
587 assert_eq!(n1_5 + n1_5, fp!(3)); |
607 assert_eq!(-n1_5 - n1_5, fp!(-3)); |
588 assert_eq!(-n1_5 - n1_5, fp!(-3)); |
|
589 assert_eq!(n1_5 - n1_5, fp!(0)); |
608 |
590 |
609 assert_eq!(n1_5 * n1_5, n2_25); |
591 assert_eq!(n1_5 * n1_5, n2_25); |
610 assert_eq!(-n1_5 * -n1_5, n2_25); |
592 assert_eq!(-n1_5 * -n1_5, n2_25); |
611 assert_eq!(n1_5 * -n1_5, -n2_25); |
593 assert_eq!(n1_5 * -n1_5, -n2_25); |
612 assert_eq!(-n1_5 * n1_5, -n2_25); |
594 assert_eq!(-n1_5 * n1_5, -n2_25); |