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