nx_q14_math.nx source
↩ module page · 202 lines · 6840 B
1// nx_q14_math.nx -- fixed-point math library.
2//
3// Q14 throughout: 16384 = 1.0. Range ~ ±131071 (suitable for game
4// world coordinates, direction vectors, camera transforms).
5// NO floating point — Cortex-M0+ compatible.
6//
7// Shared library: nx_raycast_voxel + nx_camera_q14 + nx_world_seed
8// all use these helpers instead of hand-rolling.
9//
10// Operations:
11// abs / sign
12// sqrt (Newton's method, 8 iterations)
13// sin / cos (16-entry lookup + linear interpolation)
14// normalize (vec3 to unit length)
15// dot / cross (vec3 products)
16
17import "nx_syscalls.nx"
18import "nx_tier.nx"
19
20const NX_Q14: nx_int = 16384
21const NX_Q14_2PI: nx_int = 102944 // 2π in Q14
22const NX_Q14_PI: nx_int = 51472 // π in Q14
23
24// ===== nx_q14_abs ==================================================
25
26func nx_q14_abs(x: nx_int) -> nx_int {
27 if x < 0 { return 0 - x }
28 return x
29}
30
31// ===== nx_q14_sign =================================================
32
33func nx_q14_sign(x: nx_int) -> nx_int {
34 if x > 0 { return 1 }
35 if x < 0 { return -1 }
36 return 0
37}
38
39// ===== nx_q14_clamp ================================================
40
41func nx_q14_clamp(x: nx_int, lo: nx_int, hi: nx_int) -> nx_int {
42 if x < lo { return lo }
43 if x > hi { return hi }
44 return x
45}
46
47// ===== nx_q14_mul ==================================================
48//
49// (a * b) in Q14. The product of two Q14 values is Q28; divide by Q14
50// to get back to Q14. Caller must ensure |a*b| < 2^31 to avoid overflow
51// (caller-side discipline).
52
53func nx_q14_mul(a: nx_int, b: nx_int) -> nx_int {
54 return (a * b) / NX_Q14
55}
56
57// ===== nx_q14_div ==================================================
58//
59// (a / b) in Q14. Scale numerator by Q14 before dividing.
60
61func nx_q14_div(a: nx_int, b: nx_int) -> nx_int {
62 if b == 0 { return 0 }
63 return (a * NX_Q14) / b
64}
65
66// ===== nx_q14_sqrt =================================================
67//
68// Newton's method: x_{n+1} = (x_n + v/x_n) / 2. 8 iterations is
69// enough for Q14 precision. Returns sqrt(v) in Q14 (v already Q14).
70// Input must be >= 0; negative returns 0.
71
72func nx_q14_sqrt(v: nx_int) -> nx_int {
73 if v <= 0 { return 0 }
74 // Initial guess: v itself (scale-correct for v in [Q14, infinity))
75 var x: nx_int = v
76 if x < NX_Q14 { x = NX_Q14 } // floor at 1.0 to avoid div by 0
77 var i: nx_int = 0
78 while i < 8 {
79 let q: nx_int = (v * NX_Q14) / x
80 x = (x + q) / 2
81 i = i + 1
82 }
83 return x
84}
85
86// ===== _sin_lut_lookup ============================================
87//
88// 16-entry sin lookup over [0, π/2). Values are Q14 sin(k * π/32)
89// for k in 0..15. We mirror + invert to cover the full circle.
90
91func _sin_lut(idx: nx_int) -> nx_int {
92 // Pre-computed Q14 values of sin(k * π/32) for k in 0..15
93 if idx == 0 { return 0 }
94 if idx == 1 { return 1606 } // sin(π/32) ≈ 0.0980
95 if idx == 2 { return 3196 } // sin(2π/32)
96 if idx == 3 { return 4756 }
97 if idx == 4 { return 6270 }
98 if idx == 5 { return 7723 }
99 if idx == 6 { return 9102 }
100 if idx == 7 { return 10394 }
101 if idx == 8 { return 11585 } // sin(8π/32) = sin(π/4) ≈ 0.7071
102 if idx == 9 { return 12665 }
103 if idx == 10 { return 13623 }
104 if idx == 11 { return 14449 }
105 if idx == 12 { return 15137 }
106 if idx == 13 { return 15679 }
107 if idx == 14 { return 16069 }
108 if idx == 15 { return 16305 }
109 return 16384 // idx == 16 -> sin(π/2) = 1.0
110}
111
112// ===== nx_q14_sin =================================================
113//
114// Sine of angle in Q14 (where 2π = 102944). Reduces to first
115// quadrant via symmetry + lookup with linear interp.
116
117func nx_q14_sin(angle_q14: nx_int) -> nx_int {
118 // Reduce to [0, 2π)
119 var a: nx_int = angle_q14
120 while a < 0 { a = a + NX_Q14_2PI }
121 while a >= NX_Q14_2PI { a = a - NX_Q14_2PI }
122
123 // Quadrant index 0..3
124 let quad: nx_int = a / (NX_Q14_2PI / 4) // = a / 25736
125 let in_quad: nx_int = a - quad * (NX_Q14_2PI / 4) // angle within quadrant
126
127 // Map to [0, π/2): for quad 1, reflect; quad 2, full + sign flip; quad 3, reflect + sign flip
128 var theta: nx_int = in_quad
129 var negate: nx_int = 0
130 if quad == 1 { theta = (NX_Q14_2PI / 4) - in_quad }
131 if quad == 2 { theta = in_quad; negate = 1 }
132 if quad == 3 { theta = (NX_Q14_2PI / 4) - in_quad; negate = 1 }
133
134 // Lookup at fractional index in [0, 16]
135 // theta covers [0, π/2) = [0, 25736). Each lookup step = 25736/16 = 1608.5
136 let lut_step: nx_int = 1608 // approximate; close enough at Q14
137 let lut_idx: nx_int = theta / lut_step
138 let frac: nx_int = theta - lut_idx * lut_step
139 var lo: nx_int = 0
140 var hi: nx_int = 0
141 if lut_idx < 16 { lo = _sin_lut(lut_idx) }
142 if lut_idx >= 16 { lo = 16384 }
143 if lut_idx + 1 <= 16 { hi = _sin_lut(lut_idx + 1) }
144 if lut_idx + 1 > 16 { hi = 16384 }
145 let val: nx_int = lo + ((hi - lo) * frac) / lut_step
146 if negate == 1 { return 0 - val }
147 return val
148}
149
150// ===== nx_q14_cos =================================================
151
152func nx_q14_cos(angle_q14: nx_int) -> nx_int {
153 return nx_q14_sin(angle_q14 + NX_Q14_2PI / 4)
154}
155
156// ===== nx_q14_vec3_dot =============================================
157
158func nx_q14_vec3_dot(ax: nx_int, ay: nx_int, az: nx_int,
159 bx: nx_int, by: nx_int, bz: nx_int) -> nx_int {
160 let xx: nx_int = (ax * bx) / NX_Q14
161 let yy: nx_int = (ay * by) / NX_Q14
162 let zz: nx_int = (az * bz) / NX_Q14
163 return xx + yy + zz
164}
165
166// ===== nx_q14_vec3_length ==========================================
167
168func nx_q14_vec3_length(x: nx_int, y: nx_int, z: nx_int) -> nx_int {
169 let dot: nx_int = nx_q14_vec3_dot(x, y, z, x, y, z)
170 return nx_q14_sqrt(dot)
171}
172
173// ===== nx_q14_vec3_normalize =======================================
174//
175// Scale (x,y,z) to unit length. Returns 0 if zero vector; otherwise
176// writes the normalized components back through pointers.
177
178func nx_q14_vec3_normalize(x_q14: nx_int, y_q14: nx_int, z_q14: nx_int,
179 out_x: *i64, out_y: *i64, out_z: *i64) -> nx_int {
180 let len: nx_int = nx_q14_vec3_length(x_q14, y_q14, z_q14)
181 if len == 0 {
182 out_x[0] = 0
183 out_y[0] = 0
184 out_z[0] = 0
185 return 0
186 }
187 out_x[0] = (x_q14 * NX_Q14) / len as i64
188 out_y[0] = (y_q14 * NX_Q14) / len as i64
189 out_z[0] = (z_q14 * NX_Q14) / len as i64
190 return 1
191}
192
193// ===== nx_q14_vec3_cross ===========================================
194
195func nx_q14_vec3_cross(ax: nx_int, ay: nx_int, az: nx_int,
196 bx: nx_int, by: nx_int, bz: nx_int,
197 out_x: *i64, out_y: *i64, out_z: *i64) -> nx_int {
198 out_x[0] = ((ay * bz) - (az * by)) / NX_Q14
199 out_y[0] = ((az * bx) - (ax * bz)) / NX_Q14
200 out_z[0] = ((ax * by) - (ay * bx)) / NX_Q14
201 return 0
202}