psx_math/
sincos.rs

1//! Compact sin/cos lookup for Q0.12 angles.
2//!
3//! A Q0.12 angle represents one full revolution in a 12-bit
4//! unsigned: `0..4096` maps linearly onto `0°..360°`. This
5//! matches the PSX GTE's `ONE` / `4096` angle precision. The lookup
6//! table stays compact at 256 entries, and `sin_q12` linearly
7//! interpolates between entries so every Q0.12 angle step produces a
8//! distinct result when the table slope permits it.
9//!
10//! Values are Q1.12 fixed-point: `4096` = 1.0, `-4096` = -1.0.
11//!
12//! # Example
13//!
14//! ```ignore
15//! // Rotate point (x, y) by `angle` around origin.
16//! let s = sin_q12(angle);
17//! let c = cos_q12(angle);
18//! let xr = (x * c - y * s) >> 12;
19//! let yr = (x * s + y * c) >> 12;
20//! ```
21//!
22//! Integer multiplication before the shift keeps the arithmetic
23//! inside i32; `>> 12` drops the Q1.12 fractional bits after the
24//! multiplies combine.
25
26/// 256-entry sine table, Q1.12 fixed-point (one revolution).
27///
28/// Index = `(angle_q12 >> 4) & 0xFF`, where `angle_q12` is a u16
29/// in `[0, 4096)` mapping linearly to `[0°, 360°)`. `sin_q12`
30/// interpolates using the low 4 angle bits. Values are i16 in
31/// `[-4096, 4096]`.
32pub const SIN_TABLE: [i16; 256] = [
33    0, 101, 201, 301, 401, 501, 601, 700, 799, 897, 995, 1092, 1189, 1285, 1380, 1474, 1567, 1660,
34    1751, 1842, 1931, 2019, 2106, 2191, 2276, 2359, 2440, 2520, 2598, 2675, 2751, 2824, 2896, 2967,
35    3035, 3102, 3166, 3229, 3290, 3349, 3406, 3461, 3513, 3564, 3612, 3659, 3703, 3745, 3784, 3822,
36    3857, 3889, 3920, 3948, 3973, 3996, 4017, 4036, 4052, 4065, 4076, 4085, 4091, 4095, 4096, 4095,
37    4091, 4085, 4076, 4065, 4052, 4036, 4017, 3996, 3973, 3948, 3920, 3889, 3857, 3822, 3784, 3745,
38    3703, 3659, 3612, 3564, 3513, 3461, 3406, 3349, 3290, 3229, 3166, 3102, 3035, 2967, 2896, 2824,
39    2751, 2675, 2598, 2520, 2440, 2359, 2276, 2191, 2106, 2019, 1931, 1842, 1751, 1660, 1567, 1474,
40    1380, 1285, 1189, 1092, 995, 897, 799, 700, 601, 501, 401, 301, 201, 101, 0, -101, -201, -301,
41    -401, -501, -601, -700, -799, -897, -995, -1092, -1189, -1285, -1380, -1474, -1567, -1660,
42    -1751, -1842, -1931, -2019, -2106, -2191, -2276, -2359, -2440, -2520, -2598, -2675, -2751,
43    -2824, -2896, -2967, -3035, -3102, -3166, -3229, -3290, -3349, -3406, -3461, -3513, -3564,
44    -3612, -3659, -3703, -3745, -3784, -3822, -3857, -3889, -3920, -3948, -3973, -3996, -4017,
45    -4036, -4052, -4065, -4076, -4085, -4091, -4095, -4096, -4095, -4091, -4085, -4076, -4065,
46    -4052, -4036, -4017, -3996, -3973, -3948, -3920, -3889, -3857, -3822, -3784, -3745, -3703,
47    -3659, -3612, -3564, -3513, -3461, -3406, -3349, -3290, -3229, -3166, -3102, -3035, -2967,
48    -2896, -2824, -2751, -2675, -2598, -2520, -2440, -2359, -2276, -2191, -2106, -2019, -1931,
49    -1842, -1751, -1660, -1567, -1474, -1380, -1285, -1189, -1092, -995, -897, -799, -700, -601,
50    -501, -401, -301, -201, -101,
51];
52
53/// `sin(angle_q12)` as a Q1.12 value. Input wraps modulo 4096.
54#[inline]
55pub fn sin_q12(angle_q12: u16) -> i32 {
56    let angle = angle_q12 & 0x0FFF;
57    let idx = (angle >> 4) as usize;
58    let frac = (angle & 0x000F) as i32;
59    let a = SIN_TABLE[idx] as i32;
60    let b = SIN_TABLE[(idx + 1) & 0xFF] as i32;
61    a + (((b - a) * frac) / 16)
62}
63
64/// `cos(angle_q12)` as a Q1.12 value, implemented as
65/// `sin(angle + 90°)`. Input wraps modulo 4096.
66#[inline]
67pub fn cos_q12(angle_q12: u16) -> i32 {
68    // 90° = 1024 in Q0.12 (4096 / 4). Wrap-add is automatic in u16.
69    sin_q12(angle_q12.wrapping_add(1024))
70}
71
72/// Q0.12 angle of the vector `(x, y)`: the inverse of [`sin_q12`] /
73/// [`cos_q12`], so `atan2_q12(sin, cos)` round-trips an angle.
74///
75/// Octant-linear approximation (exact on the axes and diagonals, worst
76/// error about 4° mid-octant): right for facing/steering/aiming logic,
77/// not for precision geometry. `(0, 0)` returns 0. Integer-only; large
78/// coordinates are pre-scaled so the math never leaves u32.
79pub fn atan2_q12(y: i32, x: i32) -> u16 {
80    if x == 0 && y == 0 {
81        return 0;
82    }
83    let (mut ax, mut ay) = (x.unsigned_abs(), y.unsigned_abs());
84    // Keep `small * 512` inside u32: scale both magnitudes below 2^23
85    // together, which preserves their ratio (the only thing used below).
86    while ax >= (1 << 23) || ay >= (1 << 23) {
87        ax >>= 8;
88        ay >>= 8;
89    }
90    // First-octant fold: angle 0..1024 (= 90°), 45° exactly at ax == ay.
91    let q = if ax >= ay {
92        (ay * 512 / ax) as i32
93    } else {
94        1024 - (ax * 512 / ay) as i32
95    };
96    let a = match (x >= 0, y >= 0) {
97        (true, true) => q,
98        (false, true) => 2048 - q,
99        (false, false) => 2048 + q,
100        (true, false) => 4096 - q,
101    };
102    (a & 0xFFF) as u16
103}
104
105#[cfg(test)]
106mod tests {
107    use super::*;
108
109    #[test]
110    fn sin_anchors() {
111        assert_eq!(sin_q12(0), 0);
112        assert_eq!(sin_q12(1024), 4096); // 90°
113        assert_eq!(sin_q12(2048), 0); //    180°
114        assert_eq!(sin_q12(3072), -4096); // 270°
115    }
116
117    #[test]
118    fn cos_anchors() {
119        assert_eq!(cos_q12(0), 4096);
120        assert_eq!(cos_q12(1024), 0);
121        assert_eq!(cos_q12(2048), -4096);
122        assert_eq!(cos_q12(3072), 0);
123    }
124
125    #[test]
126    fn wraps_modulo_4096() {
127        // 4096 is one full revolution; the u16 wrap makes it 0.
128        assert_eq!(sin_q12(4096), sin_q12(0));
129        assert_eq!(sin_q12(5120), sin_q12(1024));
130    }
131
132    #[test]
133    fn interpolates_low_angle_bits() {
134        assert_eq!(sin_q12(0), SIN_TABLE[0] as i32);
135        assert!(sin_q12(1) > sin_q12(0));
136        assert!(sin_q12(1) < SIN_TABLE[1] as i32);
137        assert_eq!(sin_q12(16), SIN_TABLE[1] as i32);
138    }
139
140    #[test]
141    fn atan2_cardinals_and_diagonals() {
142        assert_eq!(atan2_q12(0, 0), 0);
143        assert_eq!(atan2_q12(0, 100), 0); //     +x =   0°
144        assert_eq!(atan2_q12(100, 100), 512); // diag = 45°
145        assert_eq!(atan2_q12(100, 0), 1024); //  +y =  90°
146        assert_eq!(atan2_q12(100, -100), 1536);
147        assert_eq!(atan2_q12(0, -100), 2048); // -x = 180°
148        assert_eq!(atan2_q12(-100, -100), 2560);
149        assert_eq!(atan2_q12(-100, 0), 3072); // -y = 270°
150        assert_eq!(atan2_q12(-100, 100), 3584);
151    }
152
153    #[test]
154    fn atan2_roundtrips_sincos_within_octant_error() {
155        // Octant-linear: allow ~4° (= 46 Q0.12 units) of error.
156        for &angle in &[0u16, 100, 512, 900, 1024, 2000, 2048, 3000, 3072, 4000] {
157            let (s, c) = (sin_q12(angle), cos_q12(angle));
158            let back = atan2_q12(s, c);
159            let diff = (back.wrapping_sub(angle) & 0xFFF).min(angle.wrapping_sub(back) & 0xFFF);
160            assert!(diff <= 46, "angle {angle} came back as {back}");
161        }
162    }
163
164    #[test]
165    fn atan2_large_coordinates_stay_in_u32() {
166        // Same direction at any magnitude; the pre-scale must not change it.
167        assert_eq!(atan2_q12(i32::MAX, i32::MAX), 512);
168        assert_eq!(atan2_q12(0, i32::MIN), 2048);
169        assert_eq!(atan2_q12(i32::MIN, 0), 3072);
170        assert_eq!(atan2_q12(1 << 28, 1 << 28), atan2_q12(1, 1));
171    }
172
173    #[test]
174    fn interpolates_across_table_wrap() {
175        assert!(sin_q12(4095) < 0);
176        assert!(sin_q12(4095) > SIN_TABLE[255] as i32);
177        assert_eq!(sin_q12(4096), 0);
178    }
179}
psx_math/sincos.rs

psx_math/
sincos.rs
