A reimplementation of Mario Kart Wii's physics engine in C++
Loading...
Searching...
No Matches
Math.hh
1#pragma once
2
3#include <Common.hh>
4
5#include <cmath>
6
7namespace Kinoko {
8
9static constexpr f32 F_PI = 3.1415927f;
10static constexpr f32 F_TAU = 6.2831855f;
11static constexpr f32 DEG2RAD = 0.017453292f;
12static constexpr f32 DEG2RAD360 = 0.034906585f;
13static constexpr f32 RAD2DEG = 57.2957795f;
14static constexpr f32 DEG2FIDX = 256.0f / 360.0f;
15static constexpr f32 RAD2FIDX = 128.0f / F_PI;
16static constexpr f32 FIDX2RAD = F_PI / 128.0f;
17static constexpr f32 HALF_PI = F_PI / 2.0f;
18
20namespace EGG::Mathf {
21
25[[nodiscard]] static inline constexpr f64 force25Bit(f64 x) {
26 u64 bits = std::bit_cast<u64>(x);
27 bits = (bits & 0xfffffffff8000000ULL) + (bits & 0x8000000);
28 return std::bit_cast<f64>(bits);
29}
30
33[[nodiscard]] static inline constexpr f32 fma(f32 x, f32 y, f32 z) {
34 return static_cast<f32>(
35 static_cast<f64>(x) * force25Bit(static_cast<f64>(y)) + static_cast<f64>(z));
36}
37
40[[nodiscard]] static inline constexpr f32 fms(f32 x, f32 y, f32 z) {
41 return static_cast<f32>(
42 static_cast<f64>(x) * force25Bit(static_cast<f64>(y)) - static_cast<f64>(z));
43}
44
45// sin/cos struct
46struct SinCosEntry {
47 f32 sinVal, cosVal, sinDt, cosDt;
48};
49
50// atan struct
51struct AtanEntry {
52 f32 atanVal, atanDt;
53};
54
56static constexpr SinCosEntry sSinCosTbl[256 + 1] = {
57 {0.000000f, 1.000000f, 0.024541f, -0.000301f},
58 {0.024541f, 0.999699f, 0.024526f, -0.000903f},
59 {0.049068f, 0.998795f, 0.024497f, -0.001505f},
60 {0.073565f, 0.997290f, 0.024453f, -0.002106f},
61 {0.098017f, 0.995185f, 0.024394f, -0.002705f},
62 {0.122411f, 0.992480f, 0.024320f, -0.003303f},
63 {0.146730f, 0.989177f, 0.024231f, -0.003899f},
64 {0.170962f, 0.985278f, 0.024128f, -0.004492f},
65 {0.195090f, 0.980785f, 0.024011f, -0.005083f},
66 {0.219101f, 0.975702f, 0.023879f, -0.005671f},
67 {0.242980f, 0.970031f, 0.023733f, -0.006255f},
68 {0.266713f, 0.963776f, 0.023572f, -0.006836f},
69 {0.290285f, 0.956940f, 0.023397f, -0.007412f},
70 {0.313682f, 0.949528f, 0.023208f, -0.007984f},
71 {0.336890f, 0.941544f, 0.023005f, -0.008551f},
72 {0.359895f, 0.932993f, 0.022788f, -0.009113f},
73 {0.382683f, 0.923880f, 0.022558f, -0.009670f},
74 {0.405241f, 0.914210f, 0.022314f, -0.010220f},
75 {0.427555f, 0.903989f, 0.022056f, -0.010765f},
76 {0.449611f, 0.893224f, 0.021785f, -0.011303f},
77 {0.471397f, 0.881921f, 0.021501f, -0.011834f},
78 {0.492898f, 0.870087f, 0.021205f, -0.012358f},
79 {0.514103f, 0.857729f, 0.020895f, -0.012875f},
80 {0.534998f, 0.844854f, 0.020573f, -0.013384f},
81 {0.555570f, 0.831470f, 0.020238f, -0.013885f},
82 {0.575808f, 0.817585f, 0.019891f, -0.014377f},
83 {0.595699f, 0.803208f, 0.019532f, -0.014861f},
84 {0.615232f, 0.788346f, 0.019162f, -0.015336f},
85 {0.634393f, 0.773010f, 0.018780f, -0.015802f},
86 {0.653173f, 0.757209f, 0.018386f, -0.016258f},
87 {0.671559f, 0.740951f, 0.017982f, -0.016704f},
88 {0.689541f, 0.724247f, 0.017566f, -0.017140f},
89 {0.707107f, 0.707107f, 0.017140f, -0.017566f},
90 {0.724247f, 0.689541f, 0.016704f, -0.017982f},
91 {0.740951f, 0.671559f, 0.016258f, -0.018386f},
92 {0.757209f, 0.653173f, 0.015802f, -0.018780f},
93 {0.773010f, 0.634393f, 0.015336f, -0.019162f},
94 {0.788346f, 0.615232f, 0.014861f, -0.019532f},
95 {0.803208f, 0.595699f, 0.014377f, -0.019891f},
96 {0.817585f, 0.575808f, 0.013885f, -0.020238f},
97 {0.831470f, 0.555570f, 0.013384f, -0.020573f},
98 {0.844854f, 0.534998f, 0.012875f, -0.020895f},
99 {0.857729f, 0.514103f, 0.012358f, -0.021205f},
100 {0.870087f, 0.492898f, 0.011834f, -0.021501f},
101 {0.881921f, 0.471397f, 0.011303f, -0.021785f},
102 {0.893224f, 0.449611f, 0.010765f, -0.022056f},
103 {0.903989f, 0.427555f, 0.010220f, -0.022314f},
104 {0.914210f, 0.405241f, 0.009670f, -0.022558f},
105 {0.923880f, 0.382683f, 0.009113f, -0.022788f},
106 {0.932993f, 0.359895f, 0.008551f, -0.023005f},
107 {0.941544f, 0.336890f, 0.007984f, -0.023208f},
108 {0.949528f, 0.313682f, 0.007412f, -0.023397f},
109 {0.956940f, 0.290285f, 0.006836f, -0.023572f},
110 {0.963776f, 0.266713f, 0.006255f, -0.023733f},
111 {0.970031f, 0.242980f, 0.005671f, -0.023879f},
112 {0.975702f, 0.219101f, 0.005083f, -0.024011f},
113 {0.980785f, 0.195090f, 0.004492f, -0.024128f},
114 {0.985278f, 0.170962f, 0.003899f, -0.024231f},
115 {0.989177f, 0.146730f, 0.003303f, -0.024320f},
116 {0.992480f, 0.122411f, 0.002705f, -0.024394f},
117 {0.995185f, 0.098017f, 0.002106f, -0.024453f},
118 {0.997290f, 0.073565f, 0.001505f, -0.024497f},
119 {0.998795f, 0.049068f, 0.000903f, -0.024526f},
120 {0.999699f, 0.024541f, 0.000301f, -0.024541f},
121 {1.000000f, 0.000000f, -0.000301f, -0.024541f},
122 {0.999699f, -0.024541f, -0.000903f, -0.024526f},
123 {0.998795f, -0.049068f, -0.001505f, -0.024497f},
124 {0.997290f, -0.073565f, -0.002106f, -0.024453f},
125 {0.995185f, -0.098017f, -0.002705f, -0.024394f},
126 {0.992480f, -0.122411f, -0.003303f, -0.024320f},
127 {0.989177f, -0.146730f, -0.003899f, -0.024231f},
128 {0.985278f, -0.170962f, -0.004492f, -0.024128f},
129 {0.980785f, -0.195090f, -0.005083f, -0.024011f},
130 {0.975702f, -0.219101f, -0.005671f, -0.023879f},
131 {0.970031f, -0.242980f, -0.006255f, -0.023733f},
132 {0.963776f, -0.266713f, -0.006836f, -0.023572f},
133 {0.956940f, -0.290285f, -0.007412f, -0.023397f},
134 {0.949528f, -0.313682f, -0.007984f, -0.023208f},
135 {0.941544f, -0.336890f, -0.008551f, -0.023005f},
136 {0.932993f, -0.359895f, -0.009113f, -0.022788f},
137 {0.923880f, -0.382683f, -0.009670f, -0.022558f},
138 {0.914210f, -0.405241f, -0.010220f, -0.022314f},
139 {0.903989f, -0.427555f, -0.010765f, -0.022056f},
140 {0.893224f, -0.449611f, -0.011303f, -0.021785f},
141 {0.881921f, -0.471397f, -0.011834f, -0.021501f},
142 {0.870087f, -0.492898f, -0.012358f, -0.021205f},
143 {0.857729f, -0.514103f, -0.012875f, -0.020895f},
144 {0.844854f, -0.534998f, -0.013384f, -0.020573f},
145 {0.831470f, -0.555570f, -0.013885f, -0.020238f},
146 {0.817585f, -0.575808f, -0.014377f, -0.019891f},
147 {0.803208f, -0.595699f, -0.014861f, -0.019532f},
148 {0.788346f, -0.615232f, -0.015336f, -0.019162f},
149 {0.773010f, -0.634393f, -0.015802f, -0.018780f},
150 {0.757209f, -0.653173f, -0.016258f, -0.018386f},
151 {0.740951f, -0.671559f, -0.016704f, -0.017982f},
152 {0.724247f, -0.689541f, -0.017140f, -0.017566f},
153 {0.707107f, -0.707107f, -0.017566f, -0.017140f},
154 {0.689541f, -0.724247f, -0.017982f, -0.016704f},
155 {0.671559f, -0.740951f, -0.018386f, -0.016258f},
156 {0.653173f, -0.757209f, -0.018780f, -0.015802f},
157 {0.634393f, -0.773010f, -0.019162f, -0.015336f},
158 {0.615232f, -0.788346f, -0.019532f, -0.014861f},
159 {0.595699f, -0.803208f, -0.019891f, -0.014377f},
160 {0.575808f, -0.817585f, -0.020238f, -0.013885f},
161 {0.555570f, -0.831470f, -0.020573f, -0.013384f},
162 {0.534998f, -0.844854f, -0.020895f, -0.012875f},
163 {0.514103f, -0.857729f, -0.021205f, -0.012358f},
164 {0.492898f, -0.870087f, -0.021501f, -0.011834f},
165 {0.471397f, -0.881921f, -0.021785f, -0.011303f},
166 {0.449611f, -0.893224f, -0.022056f, -0.010765f},
167 {0.427555f, -0.903989f, -0.022314f, -0.010220f},
168 {0.405241f, -0.914210f, -0.022558f, -0.009670f},
169 {0.382683f, -0.923880f, -0.022788f, -0.009113f},
170 {0.359895f, -0.932993f, -0.023005f, -0.008551f},
171 {0.336890f, -0.941544f, -0.023208f, -0.007984f},
172 {0.313682f, -0.949528f, -0.023397f, -0.007412f},
173 {0.290285f, -0.956940f, -0.023572f, -0.006836f},
174 {0.266713f, -0.963776f, -0.023733f, -0.006255f},
175 {0.242980f, -0.970031f, -0.023879f, -0.005671f},
176 {0.219101f, -0.975702f, -0.024011f, -0.005083f},
177 {0.195090f, -0.980785f, -0.024128f, -0.004492f},
178 {0.170962f, -0.985278f, -0.024231f, -0.003899f},
179 {0.146730f, -0.989177f, -0.024320f, -0.003303f},
180 {0.122411f, -0.992480f, -0.024394f, -0.002705f},
181 {0.098017f, -0.995185f, -0.024453f, -0.002106f},
182 {0.073565f, -0.997290f, -0.024497f, -0.001505f},
183 {0.049068f, -0.998795f, -0.024526f, -0.000903f},
184 {0.024541f, -0.999699f, -0.024541f, -0.000301f},
185 {0.000000f, -1.000000f, -0.024541f, 0.000301f},
186 {-0.024541f, -0.999699f, -0.024526f, 0.000903f},
187 {-0.049068f, -0.998795f, -0.024497f, 0.001505f},
188 {-0.073565f, -0.997290f, -0.024453f, 0.002106f},
189 {-0.098017f, -0.995185f, -0.024394f, 0.002705f},
190 {-0.122411f, -0.992480f, -0.024320f, 0.003303f},
191 {-0.146730f, -0.989177f, -0.024231f, 0.003899f},
192 {-0.170962f, -0.985278f, -0.024128f, 0.004492f},
193 {-0.195090f, -0.980785f, -0.024011f, 0.005083f},
194 {-0.219101f, -0.975702f, -0.023879f, 0.005671f},
195 {-0.242980f, -0.970031f, -0.023733f, 0.006255f},
196 {-0.266713f, -0.963776f, -0.023572f, 0.006836f},
197 {-0.290285f, -0.956940f, -0.023397f, 0.007412f},
198 {-0.313682f, -0.949528f, -0.023208f, 0.007984f},
199 {-0.336890f, -0.941544f, -0.023005f, 0.008551f},
200 {-0.359895f, -0.932993f, -0.022788f, 0.009113f},
201 {-0.382683f, -0.923880f, -0.022558f, 0.009670f},
202 {-0.405241f, -0.914210f, -0.022314f, 0.010220f},
203 {-0.427555f, -0.903989f, -0.022056f, 0.010765f},
204 {-0.449611f, -0.893224f, -0.021785f, 0.011303f},
205 {-0.471397f, -0.881921f, -0.021501f, 0.011834f},
206 {-0.492898f, -0.870087f, -0.021205f, 0.012358f},
207 {-0.514103f, -0.857729f, -0.020895f, 0.012875f},
208 {-0.534998f, -0.844854f, -0.020573f, 0.013384f},
209 {-0.555570f, -0.831470f, -0.020238f, 0.013885f},
210 {-0.575808f, -0.817585f, -0.019891f, 0.014377f},
211 {-0.595699f, -0.803208f, -0.019532f, 0.014861f},
212 {-0.615232f, -0.788346f, -0.019162f, 0.015336f},
213 {-0.634393f, -0.773010f, -0.018780f, 0.015802f},
214 {-0.653173f, -0.757209f, -0.018386f, 0.016258f},
215 {-0.671559f, -0.740951f, -0.017982f, 0.016704f},
216 {-0.689541f, -0.724247f, -0.017566f, 0.017140f},
217 {-0.707107f, -0.707107f, -0.017140f, 0.017566f},
218 {-0.724247f, -0.689541f, -0.016704f, 0.017982f},
219 {-0.740951f, -0.671559f, -0.016258f, 0.018386f},
220 {-0.757209f, -0.653173f, -0.015802f, 0.018780f},
221 {-0.773010f, -0.634393f, -0.015336f, 0.019162f},
222 {-0.788346f, -0.615232f, -0.014861f, 0.019532f},
223 {-0.803208f, -0.595699f, -0.014377f, 0.019891f},
224 {-0.817585f, -0.575808f, -0.013885f, 0.020238f},
225 {-0.831470f, -0.555570f, -0.013384f, 0.020573f},
226 {-0.844854f, -0.534998f, -0.012875f, 0.020895f},
227 {-0.857729f, -0.514103f, -0.012358f, 0.021205f},
228 {-0.870087f, -0.492898f, -0.011834f, 0.021501f},
229 {-0.881921f, -0.471397f, -0.011303f, 0.021785f},
230 {-0.893224f, -0.449611f, -0.010765f, 0.022056f},
231 {-0.903989f, -0.427555f, -0.010220f, 0.022314f},
232 {-0.914210f, -0.405241f, -0.009670f, 0.022558f},
233 {-0.923880f, -0.382683f, -0.009113f, 0.022788f},
234 {-0.932993f, -0.359895f, -0.008551f, 0.023005f},
235 {-0.941544f, -0.336890f, -0.007984f, 0.023208f},
236 {-0.949528f, -0.313682f, -0.007412f, 0.023397f},
237 {-0.956940f, -0.290285f, -0.006836f, 0.023572f},
238 {-0.963776f, -0.266713f, -0.006255f, 0.023733f},
239 {-0.970031f, -0.242980f, -0.005671f, 0.023879f},
240 {-0.975702f, -0.219101f, -0.005083f, 0.024011f},
241 {-0.980785f, -0.195090f, -0.004492f, 0.024128f},
242 {-0.985278f, -0.170962f, -0.003899f, 0.024231f},
243 {-0.989177f, -0.146730f, -0.003303f, 0.024320f},
244 {-0.992480f, -0.122411f, -0.002705f, 0.024394f},
245 {-0.995185f, -0.098017f, -0.002106f, 0.024453f},
246 {-0.997290f, -0.073565f, -0.001505f, 0.024497f},
247 {-0.998795f, -0.049068f, -0.000903f, 0.024526f},
248 {-0.999699f, -0.024541f, -0.000301f, 0.024541f},
249 {-1.000000f, -0.000000f, 0.000301f, 0.024541f},
250 {-0.999699f, 0.024541f, 0.000903f, 0.024526f},
251 {-0.998795f, 0.049068f, 0.001505f, 0.024497f},
252 {-0.997290f, 0.073565f, 0.002106f, 0.024453f},
253 {-0.995185f, 0.098017f, 0.002705f, 0.024394f},
254 {-0.992480f, 0.122411f, 0.003303f, 0.024320f},
255 {-0.989177f, 0.146730f, 0.003899f, 0.024231f},
256 {-0.985278f, 0.170962f, 0.004492f, 0.024128f},
257 {-0.980785f, 0.195090f, 0.005083f, 0.024011f},
258 {-0.975702f, 0.219101f, 0.005671f, 0.023879f},
259 {-0.970031f, 0.242980f, 0.006255f, 0.023733f},
260 {-0.963776f, 0.266713f, 0.006836f, 0.023572f},
261 {-0.956940f, 0.290285f, 0.007412f, 0.023397f},
262 {-0.949528f, 0.313682f, 0.007984f, 0.023208f},
263 {-0.941544f, 0.336890f, 0.008551f, 0.023005f},
264 {-0.932993f, 0.359895f, 0.009113f, 0.022788f},
265 {-0.923880f, 0.382683f, 0.009670f, 0.022558f},
266 {-0.914210f, 0.405241f, 0.010220f, 0.022314f},
267 {-0.903989f, 0.427555f, 0.010765f, 0.022056f},
268 {-0.893224f, 0.449611f, 0.011303f, 0.021785f},
269 {-0.881921f, 0.471397f, 0.011834f, 0.021501f},
270 {-0.870087f, 0.492898f, 0.012358f, 0.021205f},
271 {-0.857729f, 0.514103f, 0.012875f, 0.020895f},
272 {-0.844854f, 0.534998f, 0.013384f, 0.020573f},
273 {-0.831470f, 0.555570f, 0.013885f, 0.020238f},
274 {-0.817585f, 0.575808f, 0.014377f, 0.019891f},
275 {-0.803208f, 0.595699f, 0.014861f, 0.019532f},
276 {-0.788346f, 0.615232f, 0.015336f, 0.019162f},
277 {-0.773010f, 0.634393f, 0.015802f, 0.018780f},
278 {-0.757209f, 0.653173f, 0.016258f, 0.018386f},
279 {-0.740951f, 0.671559f, 0.016704f, 0.017982f},
280 {-0.724247f, 0.689541f, 0.017140f, 0.017566f},
281 {-0.707107f, 0.707107f, 0.017566f, 0.017140f},
282 {-0.689541f, 0.724247f, 0.017982f, 0.016704f},
283 {-0.671559f, 0.740951f, 0.018386f, 0.016258f},
284 {-0.653173f, 0.757209f, 0.018780f, 0.015802f},
285 {-0.634393f, 0.773010f, 0.019162f, 0.015336f},
286 {-0.615232f, 0.788346f, 0.019532f, 0.014861f},
287 {-0.595699f, 0.803208f, 0.019891f, 0.014377f},
288 {-0.575808f, 0.817585f, 0.020238f, 0.013885f},
289 {-0.555570f, 0.831470f, 0.020573f, 0.013384f},
290 {-0.534998f, 0.844854f, 0.020895f, 0.012875f},
291 {-0.514103f, 0.857729f, 0.021205f, 0.012358f},
292 {-0.492898f, 0.870087f, 0.021501f, 0.011834f},
293 {-0.471397f, 0.881921f, 0.021785f, 0.011303f},
294 {-0.449611f, 0.893224f, 0.022056f, 0.010765f},
295 {-0.427555f, 0.903989f, 0.022314f, 0.010220f},
296 {-0.405241f, 0.914210f, 0.022558f, 0.009670f},
297 {-0.382683f, 0.923880f, 0.022788f, 0.009113f},
298 {-0.359895f, 0.932993f, 0.023005f, 0.008551f},
299 {-0.336890f, 0.941544f, 0.023208f, 0.007984f},
300 {-0.313682f, 0.949528f, 0.023397f, 0.007412f},
301 {-0.290285f, 0.956940f, 0.023572f, 0.006836f},
302 {-0.266713f, 0.963776f, 0.023733f, 0.006255f},
303 {-0.242980f, 0.970031f, 0.023879f, 0.005671f},
304 {-0.219101f, 0.975702f, 0.024011f, 0.005083f},
305 {-0.195090f, 0.980785f, 0.024128f, 0.004492f},
306 {-0.170962f, 0.985278f, 0.024231f, 0.003899f},
307 {-0.146730f, 0.989177f, 0.024320f, 0.003303f},
308 {-0.122411f, 0.992480f, 0.024394f, 0.002705f},
309 {-0.098017f, 0.995185f, 0.024453f, 0.002106f},
310 {-0.073565f, 0.997290f, 0.024497f, 0.001505f},
311 {-0.049068f, 0.998795f, 0.024526f, 0.000903f},
312 {-0.024541f, 0.999699f, 0.024541f, 0.000301f},
313 {-0.000000f, 1.000000f, 0.024541f, -0.000301f},
314};
315
317static constexpr AtanEntry sArcTanTbl[32 + 1] = {
318 {0.000000000f, 1.272825321f},
319 {1.272825321f, 1.270345790f},
320 {2.543171111f, 1.265415586f},
321 {3.808586697f, 1.258091595f},
322 {5.066678293f, 1.248457103f},
323 {6.315135396f, 1.236619467f},
324 {7.551754863f, 1.222707202f},
325 {8.774462065f, 1.206866624f},
326 {9.981328688f, 1.189258212f},
327 {11.170586901f, 1.170052841f},
328 {12.340639741f, 1.149428034f},
329 {13.490067775f, 1.127564381f},
330 {14.617632156f, 1.104642222f},
331 {15.722274378f, 1.080838675f},
332 {16.803113053f, 1.056325088f},
333 {17.859438141f, 1.031264918f},
334 {18.890703059f, 1.005812061f},
335 {19.896515121f, 0.980109621f},
336 {20.876624742f, 0.954289072f},
337 {21.830913814f, 0.928469801f},
338 {22.759383615f, 0.902758952f},
339 {23.662142567f, 0.877251558f},
340 {24.539394125f, 0.852030871f},
341 {25.391424996f, 0.827168886f},
342 {26.218593881f, 0.802726967f},
343 {27.021320848f, 0.778756582f},
344 {27.800077430f, 0.755300081f},
345 {28.555377511f, 0.732391496f},
346 {29.287769007f, 0.710057351f},
347 {29.997826358f, 0.688317453f},
348 {30.686143811f, 0.667185647f},
349 {31.353329458f, 0.646670542f},
350 {32.000000000f, 0.626776175f},
351};
352
354[[nodiscard]] static inline constexpr f32 SinFIdx(f32 fidx) {
355 f32 abs_fidx = fabs(fidx);
356
357 while (abs_fidx >= 65536.0f) {
358 abs_fidx -= 65536.0f;
359 }
360
361 u16 idx = static_cast<u16>(abs_fidx);
362 f32 r = abs_fidx - static_cast<f32>(idx);
363 idx &= 0xFF;
364 f32 val = sSinCosTbl[idx].sinVal + r * sSinCosTbl[idx].sinDt;
365 return fidx < 0.0f ? -val : val;
366}
367
369[[nodiscard]] static inline constexpr f32 CosFIdx(f32 fidx) {
370 f32 abs_fidx = fabs(fidx);
371
372 while (abs_fidx >= 65536.0f) {
373 abs_fidx -= 65536.0f;
374 }
375
376 u16 idx = static_cast<u16>(abs_fidx);
377 f32 r = abs_fidx - static_cast<f32>(idx);
378 idx &= 0xFF;
379
380 return sSinCosTbl[idx].cosVal + r * sSinCosTbl[idx].cosDt;
381}
382
384[[nodiscard]] static inline constexpr std::pair<f32, f32> SinCosFIdx(f32 fidx) {
385 f32 abs_fidx = fabs(fidx);
386
387 while (abs_fidx >= 65536.0f) {
388 abs_fidx -= 65536.0f;
389 }
390
391 u16 idx = static_cast<u16>(abs_fidx);
392 f32 r = abs_fidx - static_cast<f32>(idx);
393 idx &= 0xFF;
394
395 f32 cos = fma(sSinCosTbl[idx].cosDt, r, sSinCosTbl[idx].cosVal);
396 f32 sin = fma(sSinCosTbl[idx].sinDt, r, sSinCosTbl[idx].sinVal);
397
398 if (fidx < 0.0f) {
399 sin = -sin;
400 }
401
402 return {sin, cos};
403}
404
405[[nodiscard]] static inline constexpr f32 AtanFIdx_(f32 x) {
406 x *= 32.0f;
407 u16 idx = static_cast<u16>(x);
408 f32 r = x - static_cast<f32>(idx);
409 return sArcTanTbl[idx].atanVal + r * sArcTanTbl[idx].atanDt;
410}
411
413[[nodiscard]] static inline constexpr f32 Atan2FIdx(f32 y, f32 x) {
414 if (x == 0.0f && y == 0.0f) {
415 return 0.0f;
416 }
417
418 if (x >= 0.0f) {
419 if (y >= 0.0f) {
420 if (x >= y) {
421 return 0.0f + AtanFIdx_(y / x);
422 } else {
423 return 64.0f - AtanFIdx_(x / y);
424 }
425 } else {
426 if (x >= -y) {
427 return 0.0f - AtanFIdx_(-y / x);
428 } else {
429 return -64.0f + AtanFIdx_(x / -y);
430 }
431 }
432 } else {
433 if (y >= 0.0f) {
434 if (-x >= y) {
435 return 128.0f - AtanFIdx_(y / -x);
436 } else {
437 return 64.0f + AtanFIdx_(-x / y);
438 }
439 } else {
440 if (-x >= -y) {
441 return -128.0f + AtanFIdx_(-y / -x);
442 } else {
443 return -64.0f - AtanFIdx_(-x / -y);
444 }
445 }
446 }
447}
448
451[[nodiscard]] static inline constexpr f32 sin(f32 x) {
452 return SinFIdx(x * RAD2FIDX);
453}
454
457[[nodiscard]] static inline constexpr f32 cos(f32 x) {
458 return CosFIdx(x * RAD2FIDX);
459}
460
462[[nodiscard]] static inline constexpr f32 asin(f32 x) {
463 return ::asinl(x);
464}
465
467[[nodiscard]] static inline constexpr f32 acos(f32 x) {
468 return ::acosl(x);
469}
470
472[[nodiscard]] static inline constexpr f32 atan2(f32 y, f32 x) {
473 return Atan2FIdx(y, x) * FIDX2RAD;
474}
475
476[[nodiscard]] static inline constexpr f32 abs(f32 x) {
477 return std::abs(x);
478}
479
480// frsqrte matching courtesy of Geotale, with reference to https://achurch.org/cpu-tests/ppc750cl.s
481
482struct BaseAndDec32 {
483 u32 base;
484 s32 dec;
485};
486
487struct BaseAndDec64 {
488 u64 base;
489 s64 dec;
490};
491
492union c32 {
493 constexpr c32(const f32 p) {
494 f = p;
495 }
496
497 constexpr c32(const u32 p) {
498 u = p;
499 }
500
501 u32 u;
502 f32 f;
503};
504
505union c64 {
506 constexpr c64(const f64 p) {
507 f = p;
508 }
509
510 constexpr c64(const u64 p) {
511 u = p;
512 }
513
514 u64 u;
515 f64 f;
516};
517
518static constexpr u64 EXPONENT_SHIFT_F64 = 52;
519static constexpr u64 MANTISSA_MASK_F64 = 0x000fffffffffffffULL;
520static constexpr u64 EXPONENT_MASK_F64 = 0x7ff0000000000000ULL;
521static constexpr u64 SIGN_MASK_F64 = 0x8000000000000000ULL;
522
523static constexpr std::array<BaseAndDec64, 32> RSQRTE_TABLE = {{
524 {0x69fa000000000ULL, -0x15a0000000LL},
525 {0x5f2e000000000ULL, -0x13cc000000LL},
526 {0x554a000000000ULL, -0x1234000000LL},
527 {0x4c30000000000ULL, -0x10d4000000LL},
528 {0x43c8000000000ULL, -0x0f9c000000LL},
529 {0x3bfc000000000ULL, -0x0e88000000LL},
530 {0x34b8000000000ULL, -0x0d94000000LL},
531 {0x2df0000000000ULL, -0x0cb8000000LL},
532 {0x2794000000000ULL, -0x0bf0000000LL},
533 {0x219c000000000ULL, -0x0b40000000LL},
534 {0x1bfc000000000ULL, -0x0aa0000000LL},
535 {0x16ae000000000ULL, -0x0a0c000000LL},
536 {0x11a8000000000ULL, -0x0984000000LL},
537 {0x0ce6000000000ULL, -0x090c000000LL},
538 {0x0862000000000ULL, -0x0898000000LL},
539 {0x0416000000000ULL, -0x082c000000LL},
540 {0xffe8000000000ULL, -0x1e90000000LL},
541 {0xf0a4000000000ULL, -0x1c00000000LL},
542 {0xe2a8000000000ULL, -0x19c0000000LL},
543 {0xd5c8000000000ULL, -0x17c8000000LL},
544 {0xc9e4000000000ULL, -0x1610000000LL},
545 {0xbedc000000000ULL, -0x1490000000LL},
546 {0xb498000000000ULL, -0x1330000000LL},
547 {0xab00000000000ULL, -0x11f8000000LL},
548 {0xa204000000000ULL, -0x10e8000000LL},
549 {0x9994000000000ULL, -0x0fe8000000LL},
550 {0x91a0000000000ULL, -0x0f08000000LL},
551 {0x8a1c000000000ULL, -0x0e38000000LL},
552 {0x8304000000000ULL, -0x0d78000000LL},
553 {0x7c48000000000ULL, -0x0cc8000000LL},
554 {0x75e4000000000ULL, -0x0c28000000LL},
555 {0x6fd0000000000ULL, -0x0b98000000LL},
556}};
557
558[[nodiscard]] static inline constexpr f64 frsqrte(const f64 val) {
559 c64 bits(val);
560
561 u64 mantissa = bits.u & MANTISSA_MASK_F64;
562 s64 exponent = bits.u & EXPONENT_MASK_F64;
563 bool sign = (bits.u & SIGN_MASK_F64) != 0;
564
565 // Handle 0 case
566 if (mantissa == 0 && exponent == 0) {
567 return std::copysign(std::numeric_limits<f64>::infinity(), bits.f);
568 }
569
570 // Handle NaN-like
571 if (exponent == EXPONENT_MASK_F64) {
572 if (mantissa == 0) {
573 return sign ? std::numeric_limits<f64>::quiet_NaN() : 0.0;
574 }
575
576 return val;
577 }
578
579 // Handle negative inputs
580 if (sign) {
581 return std::numeric_limits<f64>::quiet_NaN();
582 }
583
584 if (exponent == 0) {
585 // Shift so one bit goes to where the exponent would be,
586 // then clear that bit to mimic a not-subnormal number!
587 // Aka, if there are 12 leading zeroes, shift left once
588 u32 shift = std::countl_zero(mantissa) - static_cast<u32>(63 - EXPONENT_SHIFT_F64);
589
590 mantissa <<= shift;
591 mantissa &= MANTISSA_MASK_F64;
592 // The shift is subtracted by 1 because denormals by default
593 // are offset by 1 (exponent 0 doesn't have implied 1 bit)
594 exponent -= static_cast<s64>(shift - 1) << EXPONENT_SHIFT_F64;
595 }
596
597 // In reality this doesn't get the full exponent -- Only the least significant bit
598 // Only that's needed because square roots of higher exponent bits simply multiply the
599 // result by 2!!
600 u32 key = static_cast<u32>((static_cast<u64>(exponent) | mantissa) >> 37);
601 u64 new_exp =
602 (static_cast<u64>((0xbfcLL << EXPONENT_SHIFT_F64) - exponent) >> 1) & EXPONENT_MASK_F64;
603
604 // Remove the bits relating to anything higher than the LSB of the exponent
605 const auto &entry = RSQRTE_TABLE[0x1f & (key >> 11)];
606
607 // The result is given by an estimate then an adjustment based on the original
608 // key that was computed
609 u64 new_mantissa = static_cast<u64>(entry.base + entry.dec * static_cast<s64>(key & 0x7ff));
610
611 return c64(new_exp | new_mantissa).f;
612}
613
614static constexpr std::array<BaseAndDec32, 32> FRES_TABLE = {{
615 {0x00fff000UL, -0x3e1L},
616 {0x00f07000UL, -0x3a7L},
617 {0x00e1d400UL, -0x371L},
618 {0x00d41000UL, -0x340L},
619 {0x00c71000UL, -0x313L},
620 {0x00bac400UL, -0x2eaL},
621 {0x00af2000UL, -0x2c4L},
622 {0x00a41000UL, -0x2a0L},
623 {0x00999000UL, -0x27fL},
624 {0x008f9400UL, -0x261L},
625 {0x00861000UL, -0x245L},
626 {0x007d0000UL, -0x22aL},
627 {0x00745800UL, -0x212L},
628 {0x006c1000UL, -0x1fbL},
629 {0x00642800UL, -0x1e5L},
630 {0x005c9400UL, -0x1d1L},
631 {0x00555000UL, -0x1beL},
632 {0x004e5800UL, -0x1acL},
633 {0x0047ac00UL, -0x19bL},
634 {0x00413c00UL, -0x18bL},
635 {0x003b1000UL, -0x17cL},
636 {0x00352000UL, -0x16eL},
637 {0x002f5c00UL, -0x15bL},
638 {0x0029f000UL, -0x15bL},
639 {0x00248800UL, -0x143L},
640 {0x001f7c00UL, -0x143L},
641 {0x001a7000UL, -0x12dL},
642 {0x0015bc00UL, -0x12dL},
643 {0x00110800UL, -0x11aL},
644 {0x000ca000UL, -0x11aL},
645 {0x00083800UL, -0x108L},
646 {0x00041800UL, -0x106L},
647}};
648
649[[nodiscard]] static inline constexpr f32 fres(const f32 val) {
650 static constexpr u32 EXPONENT_SHIFT_F32 = 23;
651 static constexpr u64 EXPONENT_SHIFT_F64 = 52;
652 static constexpr u32 EXPONENT_MASK_F32 = 0x7f800000UL;
653 static constexpr u64 EXPONENT_MASK_F64 = 0x7ff0000000000000ULL;
654 static constexpr u32 MANTISSA_MASK_F32 = 0x007fffffUL;
655 static constexpr u32 SIGN_MASK_F32 = 0x80000000UL;
656 static constexpr u64 SIGN_MASK_F64 = 0x8000000000000000ULL;
657 static constexpr u64 QUIET_BIT_F64 = 0x0008000000000000ULL;
658 static constexpr c64 LARGEST_FLOAT(static_cast<u64>(0x47d0000000000000ULL));
659
660 c64 bits(val);
661
662 u32 mantissa = static_cast<u32>(
663 bits.u >> (EXPONENT_SHIFT_F64 - static_cast<u64>(EXPONENT_SHIFT_F32))) &
664 MANTISSA_MASK_F32;
665 s32 exponent = static_cast<s32>((bits.u & EXPONENT_MASK_F64) >> EXPONENT_SHIFT_F64) - 0x380;
666 u32 sign = static_cast<u32>(bits.u >> 32) & SIGN_MASK_F32;
667
668 if (exponent < -1) {
669 bool nonzero = (bits.u & !SIGN_MASK_F64) != 0;
670
671 if (nonzero) {
672 // Create the largest normal number. It'll be a better approximation than infinity.
673 c32 cresult(sign | (EXPONENT_MASK_F32 - (1 << EXPONENT_SHIFT_F32)) | MANTISSA_MASK_F32);
674 return cresult.f;
675 } else {
676 return std::copysignf(std::numeric_limits<f32>::infinity(), val);
677 }
678 }
679
680 if ((bits.u & EXPONENT_MASK_F64) >= LARGEST_FLOAT.u) {
681 if (mantissa == 0 || (bits.u & EXPONENT_MASK_F64) != EXPONENT_MASK_F64) {
682 // Infinity or a number which will flush to 0 -- return 0
683 return std::copysignf(0.0f, val);
684 } else if ((bits.u & QUIET_BIT_F64) != 0) {
685 // QNaN -- Just return the original value
686 return val;
687 } else {
688 // SNaN!!
689 return std::numeric_limits<f32>::quiet_NaN();
690 }
691 }
692
693 // Exponent doesn't matter for simple reciprocal because the exponent is a single multiplication
694 u32 key = mantissa >> 18;
695 s32 new_exp = 253 - exponent;
696 const auto &entry = FRES_TABLE[key];
697
698 // The result is given by an estimate and adjusted based on the original key that was computed
699 u32 pre_shift =
700 static_cast<u32>(entry.base + entry.dec * (static_cast<s32>((mantissa >> 8) & 0x3ff)));
701 u32 new_mantissa = pre_shift >> 1;
702
703 if (new_exp <= 0) {
704 // Flush the denormal
705 c32 cresult(sign);
706 return cresult.f;
707 } else {
708 u32 temp = sign | static_cast<u32>(new_exp) << EXPONENT_SHIFT_F32 | new_mantissa;
709 c32 cresult(temp);
710 return cresult.f;
711 }
712}
713
716[[nodiscard]] static inline constexpr f32 frsqrt(f32 x) {
717 // frsqrte instruction
718 f64 est = frsqrte(x);
719
720 // Newton-Raphson refinement
721 f32 tmp0 = static_cast<f32>(est * force25Bit(est));
722 f32 tmp1 = static_cast<f32>(est * static_cast<f64>(0.5f));
723 f32 tmp2 =
724 static_cast<f32>(static_cast<f64>(3.0f) - static_cast<f64>(tmp0) * static_cast<f64>(x));
725 return tmp1 * tmp2;
726}
727
729[[nodiscard]] static inline constexpr f32 sqrt(f32 x) {
730 return x > 0.0f ? x * frsqrt(x) : 0.0f;
731}
732
734static inline constexpr u32 FindRootsQuadratic(f32 a, f32 b, f32 c, f32 &root1, f32 &root2) {
735 constexpr f32 EPSILON = 0.0002f;
736
737 if (b == 0.0f) {
738 f32 x = -c / a;
739 if (x > EPSILON) {
740 root1 = x * frsqrt(x);
741 root2 = -root1;
742 return 2;
743 }
744
745 // Nintendo might've made a typo here?
746 if (x >= -EPSILON) {
747 root1 = 0.0f;
748 return 1;
749 }
750
751 return 0;
752 }
753
754 f32 halfBOverA = b / (2.0f * a);
755 f32 normalizedC = c / (halfBOverA * (a * halfBOverA));
756 f32 normalizedDiscriminant = 1.0f - normalizedC;
757
758 if (normalizedDiscriminant > EPSILON) {
759 f32 sqrtNormalizedDiscriminant = normalizedDiscriminant * frsqrt(normalizedDiscriminant);
760 root2 = (halfBOverA * normalizedC) / (-1.0f - sqrtNormalizedDiscriminant);
761 root1 = halfBOverA * (-1.0f - sqrtNormalizedDiscriminant);
762 return 2;
763 }
764
765 // Nintendo might've typo'd here?
766 if (normalizedDiscriminant >= -EPSILON) {
767 root1 = -halfBOverA;
768 return 1;
769 }
770
771 return 0;
772}
773
775[[nodiscard]] static inline constexpr f32 finv(f32 x) {
776 f32 inv = fres(x);
777 f32 invDouble = inv + inv;
778 f32 invSquare = inv * inv;
779 return -fms(x, invSquare, invDouble);
780}
781
797[[nodiscard]] static inline constexpr f32 Hermite(f32 p0, f32 m0, f32 p1, f32 m1, f32 t) {
798 f32 t2 = t * t;
799 f32 t2_less_t = t2 - t;
800 f32 h3 = t2_less_t * t;
801 f32 f0 = 2.0f * h3;
802 f32 h2 = h3 - t2_less_t;
803 f32 h1 = t2 - f0;
804
805 return h3 * m1 + (h2 * m0 + (p0 - h1 * p0 + h1 * p1));
806}
807
808} // namespace EGG::Mathf
809
810} // namespace Kinoko
Common.hh
This header houses common data types such as our integral types and enums.
Kinoko::EGG::Mathf::cos
static constexpr f32 cos(f32 x)
Definition Math.hh:457
Kinoko::EGG::Mathf::fms
static constexpr f32 fms(f32 x, f32 y, f32 z)
Fused multiply-subtract operation.
Definition Math.hh:40
Kinoko::EGG::Mathf::finv
static constexpr f32 finv(f32 x)
Fused Newton-Raphson operation.
Definition Math.hh:775
Kinoko::EGG::Mathf::Hermite
static constexpr f32 Hermite(f32 p0, f32 m0, f32 p1, f32 m1, f32 t)
Evaluates a cubic Hermite curve at a given parameter t.
Definition Math.hh:797
Kinoko::EGG::Mathf::force25Bit
static constexpr f64 force25Bit(f64 x)
This is used to mimic the Wii's floating-point unit.
Definition Math.hh:25
Kinoko::EGG::Mathf::frsqrt
static constexpr f32 frsqrt(f32 x)
Definition Math.hh:716
Kinoko::EGG::Mathf::sin
static constexpr f32 sin(f32 x)
Definition Math.hh:451
Kinoko::EGG::Mathf::fma
static constexpr f32 fma(f32 x, f32 y, f32 z)
Fused multiply-add operation.
Definition Math.hh:33