irox_tools/primitives/
f64.rs

1// SPDX-License-Identifier: MIT
2// Copyright 2025 IROX Contributors
3//
4
5//!
6//! A collection of utilities for the f64 built-in
7//!
8
9use crate::{FloatIsh, FromF64, One, PrimitiveMath, ToF64, ToSigned, WrappingSub, Zero};
10
11///
12/// Finds the minimum and maximum value in the provided iterator.
13/// Example:
14/// ```
15/// let values : Vec<f64> = vec![0.0, 5.0, 30.0, 20.0, 2.0];
16/// let (min, max) = irox_tools::f64::min_max(&values);
17///
18/// assert_eq!(min, 0.0);
19/// assert_eq!(max, 30.0);
20/// ```
21#[must_use]
22pub fn min_max(iter: &[f64]) -> (f64, f64) {
23    let mut min = f64::MAX;
24    let mut max = f64::MIN;
25
26    for val in iter {
27        min = min.min(*val);
28        max = max.max(*val);
29    }
30
31    (min, max)
32}
33
34pub trait FloatExt: PrimitiveMath {
35    type Type;
36    type Size;
37    fn trunc(self) -> Self::Type;
38    fn fract(self) -> Self::Type;
39    fn abs(self) -> Self::Type;
40    fn round(self) -> Self::Type;
41    fn floor(self) -> Self::Type;
42    fn ceil(self) -> Self::Type;
43    fn signum(self) -> Self::Type;
44    fn clamp(self, min: Self, max: Self) -> Self::Type;
45
46    fn exp(self) -> Self::Type;
47    fn ln(self) -> Self::Type;
48    fn log10(self) -> Self::Type;
49
50    fn powi(self, val: i32) -> Self::Type;
51    fn powf(self, val: Self::Type) -> Self::Type;
52
53    fn sqrt(self) -> Self::Type;
54    fn to_bits(self) -> Self::Size;
55    fn exponent(self) -> u16;
56    fn significand(self) -> Self::Size;
57    fn sin(self) -> Self::Type;
58    fn cos(self) -> Self::Type;
59    fn tan(self) -> Self::Type;
60    fn atan(self) -> Self::Type;
61    fn atan2(self, o: Self) -> Self::Type;
62}
63#[allow(unused)]
64fn cordic_k(n: usize) -> f64 {
65    let mut k = 1.0;
66    let mut i = n;
67    let mut ik = 1.0;
68    while i > 0 {
69        k *= 1. / f64::sqrt(1. + ik);
70        ik /= 4.0;
71        i -= 1;
72    }
73    k
74}
75const CORDIC_K_64: f64 = 0.6072529350088814;
76const CORDIC_ITERS: usize = 64;
77const THETA_TABLE: [f64; CORDIC_ITERS] = [
78    core::f64::consts::FRAC_PI_4,
79    0.4636476090008061,
80    0.24497866312686414,
81    0.12435499454676144,
82    0.06241880999595735,
83    0.031239833430268277,
84    0.015623728620476831,
85    0.007812341060101111,
86    0.0039062301319669718,
87    0.0019531225164788188,
88    0.0009765621895593195,
89    0.0004882812111948983,
90    0.00024414062014936177,
91    0.00012207031189367021,
92    0.00006103515617420877,
93    0.000030517578115526096,
94    0.000015258789061315762,
95    0.00000762939453110197,
96    0.000003814697265606496,
97    0.000001907348632810187,
98    0.0000009536743164059608,
99    0.00000047683715820308884,
100    0.00000023841857910155797,
101    0.00000011920928955078068,
102    0.00000005960464477539055,
103    0.000000029802322387695303,
104    0.000000014901161193847655,
105    0.000000007450580596923828,
106    0.000000003725290298461914,
107    0.000000001862645149230957,
108    0.0000000009313225746154785,
109    0.0000000004656612873077393,
110    0.00000000023283064365386963,
111    0.00000000011641532182693481,
112    0.00000000005820766091346741,
113    0.000000000029103830456733704,
114    0.000000000014551915228366852,
115    0.000000000007275957614183426,
116    0.000000000003637978807091713,
117    0.0000000000018189894035458565,
118    0.0000000000009094947017729282,
119    0.0000000000004547473508864641,
120    0.00000000000022737367544323206,
121    0.00000000000011368683772161603,
122    0.00000000000005684341886080802,
123    0.00000000000002842170943040401,
124    0.000000000000014210854715202004,
125    0.000000000000007105427357601002,
126    0.000000000000003552713678800501,
127    0.0000000000000017763568394002505,
128    0.0000000000000008881784197001252,
129    0.0000000000000004440892098500626,
130    0.0000000000000002220446049250313,
131    0.00000000000000011102230246251565,
132    0.00000000000000005551115123125783,
133    0.000000000000000027755575615628914,
134    0.000000000000000013877787807814457,
135    0.000000000000000006938893903907228,
136    0.000000000000000003469446951953614,
137    0.000000000000000001734723475976807,
138    0.0000000000000000008673617379884035,
139    0.0000000000000000004336808689942018,
140    0.0000000000000000002168404344971009,
141    0.00000000000000000010842021724855044,
142];
143/// https://en.wikipedia.org/wiki/CORDIC
144pub fn cordic<T: PrimitiveMath + One + Zero + FromF64 + PartialOrd + Copy>(alpha: T) -> (T, T) {
145    let k_n = T::from_f64(CORDIC_K_64);
146    let mut theta = T::ZERO;
147    let mut x = T::ONE;
148    let mut y = T::ZERO;
149    let mut p2i = T::ONE;
150    let mut idx = 0;
151    while idx < CORDIC_ITERS {
152        #[allow(clippy::indexing_slicing)]
153        let atan = T::from_f64(THETA_TABLE[idx]);
154        let xt = x;
155        if theta < alpha {
156            theta += atan;
157            x -= y * p2i;
158            y += xt * p2i;
159        } else {
160            theta -= atan;
161            x += y * p2i;
162            y -= xt * p2i;
163        }
164        p2i /= T::from_f64(2.);
165        idx += 1;
166    }
167    (x * k_n, y * k_n)
168}
169#[cfg(not(feature = "std"))]
170impl FloatExt for f64 {
171    type Type = f64;
172    type Size = u64;
173
174    ///
175    /// Truncate the value
176    /// Just casts to u64 then back to f64.
177    fn trunc(self) -> f64 {
178        (self as u64) as f64
179    }
180
181    fn fract(self) -> f64 {
182        self - self.trunc()
183    }
184
185    ///
186    /// Force the value to be positive by zeroing out the highest sign bit.
187    fn abs(self) -> f64 {
188        f64::from_bits(self.to_bits() & 0x7FFF_FFFF_FFFF_FFFFu64)
189    }
190
191    fn round(self) -> f64 {
192        (self + 0.5 * self.signum()).trunc()
193    }
194
195    fn floor(self) -> f64 {
196        if self.is_sign_negative() {
197            return (self - 1.0).trunc();
198        }
199        self.trunc()
200    }
201
202    fn ceil(self) -> f64 {
203        if self.is_sign_positive() {
204            return (self + 1.0).trunc();
205        }
206        self.trunc()
207    }
208
209    fn signum(self) -> f64 {
210        if self.is_nan() {
211            return f64::NAN;
212        }
213        if self.is_sign_negative() {
214            return -1.0;
215        }
216        1.0
217    }
218
219    fn clamp(self, min: Self, max: Self) -> Self::Type {
220        if self < min {
221            return min;
222        } else if self > max {
223            return max;
224        }
225        self
226    }
227
228    ///
229    /// Implementation of Exponential Function from NIST DTMF eq 4.2.19: `<https://dlmf.nist.gov/4.2.E19>`
230    fn exp(self) -> Self::Type {
231        if self.is_nan() || self.is_infinite() {
232            return self;
233        }
234        let mut out = 1.0;
235        let i = self;
236        let mut idx = 1;
237        let mut next = self;
238
239        while next.abs() != 0.0 {
240            out += next;
241            idx += 1;
242            next *= i / idx as Self::Type;
243        }
244
245        out
246    }
247
248    ///
249    /// Implementation of Natural Logarithm using NIST DLMF eq 4.6.4: `<https://dlmf.nist.gov/4.6.E4>`
250    fn ln(self) -> Self::Type {
251        if !self.is_normal() {
252            return self;
253        }
254        let z = self;
255        if z == 0. {
256            return 1.;
257        } else if z < 0. {
258            return f64::NAN;
259        }
260        let iter = (z - 1.) / (z + 1.);
261        let mut out = 0.0;
262        let mut next = 2.0 * iter;
263        let mut idx = 1.0;
264        let mut base = iter;
265        while next != 0.0 {
266            out += next;
267            idx += 2.0;
268            base *= iter * iter;
269            next = 2.0 * base / idx;
270        }
271        out
272    }
273
274    fn log10(self) -> Self::Type {
275        self.ln() / core::f64::consts::LN_10
276    }
277
278    ///
279    /// Implementation of general power function using NIST DLMF eq 4.2.26: `<https://dlmf.nist.gov/4.2.E26>`
280    fn powf(self, a: Self::Type) -> Self::Type {
281        if !self.is_normal() {
282            return self;
283        }
284        let z = self;
285
286        (a * z.ln()).exp()
287    }
288
289    /// Naive implementation of integer power fn.  Will do something smarter later.
290    fn powi(self, val: i32) -> Self::Type {
291        if !self.is_normal() {
292            return self;
293        }
294        let mut out = self;
295        let i = self;
296        for _ in 0..val.abs() {
297            out *= i;
298        }
299        out
300    }
301
302    fn sqrt(self) -> Self::Type {
303        self.powf(0.5)
304    }
305
306    fn to_bits(self) -> Self::Size {
307        f64::to_bits(self)
308    }
309
310    fn exponent(self) -> u16 {
311        ((self.to_bits() >> 52) & 0x7FF) as u16
312    }
313
314    fn significand(self) -> Self::Size {
315        self.to_bits() & 0xF_FFFF_FFFF_FFFF
316    }
317
318    fn sin(self) -> Self::Type {
319        cordic(self).0
320    }
321    fn cos(self) -> Self::Type {
322        cordic(self).1
323    }
324
325    fn tan(self) -> Self::Type {
326        self.sin() / self.cos()
327    }
328
329    fn atan(self) -> Self::Type {
330        todo!()
331    }
332
333    fn atan2(self, _o: Self) -> Self::Type {
334        todo!()
335    }
336}
337
338#[cfg(feature = "std")]
339impl FloatExt for f64 {
340    type Type = f64;
341    type Size = u64;
342
343    fn trunc(self) -> Self::Type {
344        f64::trunc(self)
345    }
346
347    fn fract(self) -> Self::Type {
348        f64::fract(self)
349    }
350
351    fn abs(self) -> Self::Type {
352        f64::abs(self)
353    }
354
355    fn round(self) -> Self::Type {
356        f64::round(self)
357    }
358
359    fn floor(self) -> Self::Type {
360        f64::floor(self)
361    }
362
363    fn ceil(self) -> Self::Type {
364        f64::ceil(self)
365    }
366
367    fn signum(self) -> Self::Type {
368        f64::signum(self)
369    }
370
371    fn clamp(self, min: Self, max: Self) -> Self::Type {
372        f64::clamp(self, min, max)
373    }
374
375    fn exp(self) -> Self::Type {
376        f64::exp(self)
377    }
378
379    fn ln(self) -> Self::Type {
380        f64::ln(self)
381    }
382
383    fn log10(self) -> Self::Type {
384        f64::log10(self)
385    }
386
387    fn powi(self, val: i32) -> Self::Type {
388        f64::powi(self, val)
389    }
390
391    fn powf(self, val: Self::Type) -> Self::Type {
392        f64::powf(self, val)
393    }
394
395    fn sqrt(self) -> Self::Type {
396        f64::sqrt(self)
397    }
398
399    fn to_bits(self) -> Self::Size {
400        f64::to_bits(self)
401    }
402
403    fn exponent(self) -> u16 {
404        ((self.to_bits() >> 52) & 0x7FF) as u16
405    }
406
407    fn significand(self) -> Self::Size {
408        self.to_bits() & 0xF_FFFF_FFFF_FFFF
409    }
410
411    fn sin(self) -> Self::Type {
412        f64::sin(self)
413    }
414
415    fn cos(self) -> Self::Type {
416        f64::cos(self)
417    }
418
419    fn tan(self) -> Self::Type {
420        f64::tan(self)
421    }
422
423    fn atan(self) -> Self::Type {
424        f64::atan(self)
425    }
426
427    fn atan2(self, o: Self) -> Self::Type {
428        f64::atan2(self, o)
429    }
430}
431
432impl WrappingSub for f64 {
433    fn wrapping_sub(&self, rhs: Self) -> Self {
434        self - rhs
435    }
436}
437impl ToF64 for f64 {
438    fn to_f64(&self) -> f64 {
439        *self
440    }
441}
442impl FromF64 for f64 {
443    fn from_f64(value: f64) -> Self {
444        value
445    }
446}
447impl ToSigned for f64 {
448    type Output = f64;
449
450    fn to_signed(self) -> Self::Output {
451        self
452    }
453    fn negative_one() -> Self::Output {
454        -1.
455    }
456}
457
458impl PrimitiveMath for f64 {}
459impl FloatIsh for f64 {}
460
461#[cfg(test)]
462mod tests {
463    use crate::f64::{cordic, cordic_k};
464    use std::f64::consts::PI;
465
466    #[test]
467    pub fn test_ln() {
468        assert_eq_eps!(0.0, crate::f64::FloatExt::ln(1.0f64), 1e-16);
469        assert_eq_eps!(1.0, crate::f64::FloatExt::ln(core::f64::consts::E), 1e-15);
470        assert_eq_eps!(4.605170185988092, crate::f64::FloatExt::ln(100f64), 1e-13);
471        assert_eq_eps!(
472            11.090339630053647,
473            crate::f64::FloatExt::ln(u16::MAX as f64),
474            1e-11
475        );
476    }
477
478    #[test]
479    pub fn test_exp() {
480        assert_eq_eps!(1.0, crate::f64::FloatExt::exp(0.0f64), 1e-16);
481        assert_eq_eps!(
482            core::f64::consts::E,
483            crate::f64::FloatExt::exp(1.0f64),
484            1e-15
485        );
486        assert_eq_eps!(7.38905609893065, crate::f64::FloatExt::exp(2.0f64), 1e-14);
487        assert_eq_eps!(
488            15.154262241479262,
489            crate::f64::FloatExt::exp(core::f64::consts::E),
490            1e-15
491        );
492    }
493
494    #[test]
495    pub fn test_sqrt() {
496        assert_eq!(2., crate::f64::FloatExt::sqrt(4.0f64));
497    }
498
499    #[test]
500    #[ignore]
501    pub fn theta_table() {
502        println!("{}\n", cordic_k(80));
503        for i in 0..80 {
504            println!("{},", 1f64.atan2(2f64.powi(i)))
505        }
506    }
507    #[test]
508    pub fn test_cordic() {
509        for x in -90..90 {
510            let x = x as f64;
511            let angr = x * (PI / 180.);
512            let (cosx, sinx) = cordic(angr);
513            let dcos = cosx - angr.cos();
514            let dsin = sinx - angr.sin();
515            let dcosulps = dcos / f64::EPSILON;
516            let dsinulps = dsin / f64::EPSILON;
517            println!("{x:0.05} {sinx:0.16} {dsin:0.16} {dsinulps:0.1} {cosx:0.16} {dcos:0.16} {dcosulps:0.1}");
518            assert!(dcosulps.abs() < 5.0, "{x}");
519            assert!(dsinulps.abs() < 5.0, "{x}");
520        }
521    }
522}
irox_tools/primitives/f64.rs

irox_tools/primitives/
f64.rs
