fixed_math_taylor/
lib.rs

1// Copyright (C) 2026 Jorge Andre Castro
2// This program is free software: you can redistribute it and/or modify
3// it under the terms of the GNU General Public License as published by
4// the Free Software Foundation, either version 2 or the License, or
5// (at your option) any later version.
6
7#![no_std]
8
9//! # Fixed-Math-Taylor (Modular Edition)
10//! 
11//! Bibliothèque de trigonométrie haute performance.
12//! Activez les moteurs souhaités via les Cargo Features :
13//! - `lut` : Virgule fixe Q15 ultra-rapide (recommandé pour MCU).
14//! - `taylor` : Série de Taylor (f32) pour la précision.
15//! - `fast-sin` : Approximation de Bhaskara I (f32) pour la vitesse.
16
17// --- TYPES DE BASE ---
18pub type Angle = u16; // 0..65535 = 0..2π
19pub type Fixed = i16; // Q15
20
21// ==========================================
22// MOTEUR LUT (FEATURE "lut")
23// ==========================================
24#[cfg(feature = "lut")]
25mod lut_impl {
26    use super::{Angle, Fixed};
27    const QUADRANT_BITS: u32 = 14;
28    const LUT_SIZE: usize = 256;
29    const LUT_BITS: u32 = 8;
30    const LUT_MASK: u32 = (1 << (QUADRANT_BITS - LUT_BITS)) - 1;
31
32    // Inclusion de la table de sinus (0 à PI/2)
33    static SIN_LUT: [Fixed; 257] = include!("sin_table.rs.inc");
34
35    #[inline(always)]
36    pub fn sin_fixed(angle: Angle) -> Fixed {
37        let quadrant = (angle >> QUADRANT_BITS) as usize;
38        let idx = (angle & 0x3FFF) as u32;
39        let lut_idx = (idx >> (QUADRANT_BITS - LUT_BITS)) as usize;
40        let frac = (idx & LUT_MASK) as i32;
41
42        match quadrant {
43            0 => interpolate(SIN_LUT[lut_idx], SIN_LUT[lut_idx + 1], frac),
44            1 => interpolate(SIN_LUT[LUT_SIZE - lut_idx], SIN_LUT[LUT_SIZE - lut_idx - 1], frac),
45            2 => -interpolate(SIN_LUT[lut_idx], SIN_LUT[lut_idx + 1], frac),
46            _ => -interpolate(SIN_LUT[LUT_SIZE - lut_idx], SIN_LUT[LUT_SIZE - lut_idx - 1], frac),
47        }
48    }
49
50    #[inline(always)]
51    fn interpolate(y0: Fixed, y1: Fixed, frac: i32) -> Fixed {
52        let y0_32 = y0 as i32;
53        let y1_32 = y1 as i32;
54        (y0_32 + (((y1_32 - y0_32) * frac) >> (QUADRANT_BITS - LUT_BITS))) as Fixed
55    }
56}
57
58// Ré-exportation et fonctions publiques liées à la LUT
59#[cfg(feature = "lut")]
60pub use lut_impl::sin_fixed;
61
62#[cfg(feature = "lut")]
63#[inline(always)]
64pub fn cos(angle: Angle) -> Fixed {
65    sin_fixed(angle.wrapping_add(16384))
66}
67
68#[cfg(feature = "lut")]
69#[inline(always)]
70pub fn sin_cos(angle: Angle) -> (Fixed, Fixed) {
71    (sin_fixed(angle), cos(angle))
72}
73
74// ==========================================
75// MOTEUR TAYLOR (Q15 - 100% Entiers)
76// ==========================================
77#[cfg(feature = "taylor")]
78pub mod taylor_impl {
79    use super::{Angle, Fixed};
80    
81    pub fn sin_taylor(angle: Angle) -> Fixed {
82        let x_input = if angle > 32768 { 65536 - angle as i32 } else { angle as i32 };
83        let x = if x_input > 16384 { 32768 - x_input } else { x_input };
84
85        let x_rad = (x * 51472) >> 14; 
86
87        let x2 = (x_rad * x_rad) >> 15;
88        let x3 = (x2 * x_rad) >> 15;
89        let x5 = (((x3 * x2) >> 15) * x2) >> 15;
90
91        let term3 = (x3 * 5461) >> 15; 
92        let term5 = (x5 * 273) >> 15;
93
94        // C'EST CETTE LIGNE QUI DOIT ÊTRE ICI :
95        let res = (x_rad - term3 + term5) as Fixed;
96        
97        if angle > 32768 { -res } else { res }
98    }
99}
100// ==========================================
101// MOTEUR FAST (Bhaskara I Q15)
102// ==========================================
103#[cfg(feature = "fast-sin")]
104pub mod fast_impl {
105    use super::{Angle, Fixed};
106
107    pub fn sin_fast(angle: Angle) -> Fixed {
108        // 0..PI (0..32768)
109        let x = (angle & 0x7FFF) as i32; 
110        let pi = 32768i32;
111        
112        // num = 4x(pi-x)
113        let x_pi_x = (x * (pi - x)) >> 15; // Reste en Q15
114        
115        // Formule de Bhaskara simplifiée pour calcul entier :
116        // sin(x) ≈ (16x(pi-x)) / (5pi^2 - 4x(pi-x))
117        let num = (x_pi_x as i64) * 16;
118        let den = (5 * 32768) - ((4 * x_pi_x) >> 0); // Approximation du dénominateur
119        
120        // On scale le numérateur pour la division Q15
121        let res = (num * 32767) / den as i64;
122        
123        let val = res as Fixed;
124        if angle > 32768 { -val } else { val }
125    }
126}
127// ==========================================
128// UTILITAIRES COMMUNS
129// ==========================================
130
131#[inline(always)]
132pub fn to_fixed(x: f32) -> Fixed { (x * 32767.0) as Fixed }
133
134#[inline(always)]
135pub fn from_fixed(x: Fixed) -> f32 { (x as f32) / 32767.0 }
136
137#[inline(always)]
138pub fn radians_to_angle(rads: f32) -> Angle {
139    let scale = 65536.0 / (2.0 * core::f32::consts::PI);
140    (rads * scale) as i32 as u16
141}
142
143// ==========================================
144// TESTS UNITAIRES
145// ==========================================
146#[cfg(test)]
147mod tests {
148    extern crate std;
149    use super::*;
150    use core::f32::consts::PI;
151
152   #[cfg(feature = "lut")]
153    #[test]
154    fn test_sin_fixed_precision() {
155        // Points cardinaux : Précision exacte (tolérance 1 bit)
156        assert!((sin_fixed(0) - 0).abs() <= 1);
157        assert!((sin_fixed(16384) - 32767).abs() <= 1); // PI/2 (1.0)
158        assert!((sin_fixed(32768) - 0).abs() <= 1);     // PI (0.0)
159        assert!((sin_fixed(49152) - (-32767)).abs() <= 1); // 3PI/2 (-1.0)
160
161        // Test à 45° (Angle 8192 = Index 128 dans une table de 256 pts)
162        // Dans ta table, SIN_LUT[128] est exactement 23203.
163        let res_raw = sin_fixed(8192); 
164        let expected_raw = 23203; 
165        
166        assert_eq!(res_raw, expected_raw, "Erreur de précision à 45°");
167    }
168
169    #[cfg(feature = "lut")]
170    #[test]
171    fn test_cos_fixed() {
172        assert!((cos(0) - 32767).abs() <= 1);
173        assert!(cos(16384).abs() <= 1);
174        assert!((cos(32768) - (-32767)).abs() <= 1);
175    }
176
177  #[cfg(feature = "taylor")]
178    #[test]
179    fn test_taylor_accuracy() {
180        let res = taylor_impl::sin_taylor(8192); // 45°
181        let expected = 23170; 
182        assert!((res - expected).abs() < 1000); // Marge plus réaliste pour Taylor Q15
183    }
184 #[cfg(feature = "fast-sin")]
185    #[test]
186    fn test_fast_sin_approximation() {
187        // Test à 30 degrés (Angle 5461) -> sin(30) = 0.5 (16384)
188        let res = fast_impl::sin_fast(5461);
189        let expected = 16384; 
190        // On autorise un écart de 1500 (environ 4.5%)
191        assert!((res - expected).abs() < 1500, "Valeur reçue: {}", res);
192    }
193    #[test]
194    fn test_radians_to_angle_wrapping() {
195        assert_eq!(radians_to_angle(0.0), 0);
196        assert_eq!(radians_to_angle(2.0 * PI), 0);
197        // Utilisation de la tolérance pour f32
198        let a = radians_to_angle(-PI / 2.0);
199        assert!(a == 49152 || a == 49151); 
200    }
201
202    #[test]
203    fn test_fixed_conversion_roundtrip() {
204        let original = 0.5f32;
205        let fixed = to_fixed(original);
206        let back = from_fixed(fixed);
207        assert!((original - back).abs() < 0.0001);
208    }
209
210    #[test]
211    fn test_sin_cos_simultaneous() {
212        #[cfg(feature = "lut")]
213        {
214            let (s, c) = sin_cos(0);
215            assert_eq!(s, 0);
216            assert_eq!(c, 32767);
217        }
218    }
219}
fixed_math_taylor/lib.rs

fixed_math_taylor/
lib.rs
