1#include "Library/Math/MathUtil.h"
2
3#include <math/seadMathCalcCommon.h>
4#include <math/seadMatrix.h>
5#include <prim/seadBitUtil.h>
6#include <random/seadGlobalRandom.h>
7
8#include "Library/Matrix/MatrixUtil.h"
9
10namespace al {
11f32 calcAngleRadian(const sead::Vector3f& a, const sead::Vector3f& b) {
12 f32 dot = a.dot(t: b);
13 sead::Vector3f cross;
14 cross.setCross(a, b);
15 return sead::Mathf::atan2(y: cross.length(), x: dot);
16}
17
18f32 calcAngleDegree(const sead::Vector3f& a, const sead::Vector3f& b) {
19 return sead::Mathf::rad2deg(rad: calcAngleRadian(a, b));
20}
21
22f32 calcAngleDegree(const sead::Vector2f& a, const sead::Vector2f& b) {
23 if (isNearZero(vec: a) || isNearZero(vec: b))
24 return 0.0f;
25
26 return sead::Mathf::rad2deg(rad: sead::Mathf::atan2(y: a.cross(t: b), x: a.dot(t: b)));
27}
28
29bool isNearZero(const sead::Vector2f& vec, f32 tolerance) {
30 return vec.squaredLength() < tolerance * tolerance;
31}
32
33bool tryCalcAngleDegree(f32* out, const sead::Vector3f& a, const sead::Vector3f& b) {
34 if (isNearZero(vec: a) || isNearZero(vec: b))
35 return false;
36
37 *out = calcAngleDegree(a, b);
38 return true;
39}
40
41bool isNearZero(const sead::Vector3f& vec, f32 tolerance) {
42 return vec.squaredLength() < tolerance * tolerance;
43}
44
45f32 calcAngleOnPlaneRadian(const sead::Vector3f& a, const sead::Vector3f& b,
46 const sead::Vector3f& vertical) {
47 sead::Vector3f planeA;
48 verticalizeVec(out: &planeA, vertical, vec: a);
49 sead::Vector3f planeB;
50 verticalizeVec(out: &planeB, vertical, vec: b);
51
52 f32 dot = planeA.dot(t: planeB);
53 sead::Vector3f cross;
54 cross.setCross(a: planeA, b: planeB);
55 f32 angle = sead::Mathf::atan2(y: cross.length(), x: dot);
56
57 return vertical.dot(t: cross) < 0.0f ? -angle : angle;
58}
59
60/**
61 * Takes the plane perpendicular to unit vector `vertical`, projects `vec` onto it, and
62 * stores the result in `out`. The effect is that `vec` and `out` will look equal
63 * if looking in the direction of `vertical`.
64 */
65void verticalizeVec(sead::Vector3f* out, const sead::Vector3f& vertical,
66 const sead::Vector3f& vec) {
67 out->setScaleAdd(t: -vertical.dot(t: vec), a: vertical, b: vec);
68}
69
70f32 calcAngleOnPlaneDegree(const sead::Vector3f& a, const sead::Vector3f& b,
71 const sead::Vector3f& vertical) {
72 return sead::Mathf::rad2deg(rad: calcAngleOnPlaneRadian(a, b, vertical));
73}
74
75f32 calcAngleOnPlaneDegreeOrZero(const sead::Vector3f& a, const sead::Vector3f& b,
76 const sead::Vector3f& vertical) {
77 f32 angle = 0.0f;
78 if (!tryCalcAngleOnPlaneDegree(angle: &angle, a, b, vertical))
79 return 0.0f;
80
81 return angle;
82}
83
84bool isParallelDirection(const sead::Vector3f& a, const sead::Vector3f& b, f32 tolerance) {
85 if (sead::Mathf::abs(x: a.y * b.z - a.z * b.y) > tolerance)
86 return false;
87 if (sead::Mathf::abs(x: a.z * b.x - a.x * b.z) > tolerance)
88 return false;
89 if (sead::Mathf::abs(x: a.x * b.y - a.y * b.x) > tolerance)
90 return false;
91 return true;
92}
93
94s32 calcAngleSignOnPlane(const sead::Vector3f& a, const sead::Vector3f& b,
95 const sead::Vector3f& vertical) {
96 sead::Vector3f planeA;
97 verticalizeVec(out: &planeA, vertical, vec: a);
98 sead::Vector3f planeB;
99 verticalizeVec(out: &planeB, vertical, vec: b);
100
101 sead::Vector3f cross;
102 cross.setCross(a: planeA, b: planeB);
103 const f32 angle = vertical.dot(t: cross);
104
105 if (angle > 0.0f)
106 return 1;
107 if (angle < 0.0f)
108 return -1;
109
110 return 0;
111}
112
113bool isNearAngleRadian(const sead::Vector2f& a, const sead::Vector2f& b, f32 tolerance) {
114 if (isNearZero(vec: a))
115 return false;
116 if (isNearZero(vec: b))
117 return false;
118
119 sead::Vector2f aNorm;
120 normalize(out: &aNorm, vec: a);
121 sead::Vector2f bNorm;
122 normalize(out: &bNorm, vec: b);
123
124 return aNorm.dot(t: bNorm) >= sead::Mathf::cos(t: tolerance);
125}
126
127void normalize(sead::Vector2f* out, const sead::Vector2f& vec) {
128 *out = vec;
129 normalize(vec: out);
130}
131
132bool isNearAngleRadian(const sead::Vector3f& a, const sead::Vector3f& b, f32 tolerance) {
133 if (isNearZero(vec: a))
134 return false;
135 if (isNearZero(vec: b))
136 return false;
137
138 sead::Vector3f aNorm;
139 normalize(out: &aNorm, vec: a);
140 sead::Vector3f bNorm;
141 normalize(out: &bNorm, vec: b);
142
143 return aNorm.dot(t: bNorm) >= sead::Mathf::cos(t: tolerance);
144}
145
146void normalize(sead::Vector3f* out, const sead::Vector3f& vec) {
147 *out = vec;
148 normalize(vec: out);
149}
150
151bool isNearAngleDegree(const sead::Vector2f& a, const sead::Vector2f& b, f32 tolerance) {
152 return isNearAngleRadian(a, b, tolerance: sead::Mathf::deg2rad(deg: tolerance));
153}
154
155bool isNearAngleDegree(const sead::Vector3f& a, const sead::Vector3f& b, f32 tolerance) {
156 return isNearAngleRadian(a, b, tolerance: sead::Mathf::deg2rad(deg: tolerance));
157}
158
159bool tryNormalizeOrZero(sead::Vector3f* vec) {
160 if (isNearZero(vec: *vec)) {
161 *vec = {0.0f, 0.0f, 0.0f};
162 return false;
163 }
164
165 normalize(vec);
166 return true;
167}
168
169bool tryNormalizeOrZero(sead::Vector3f* out, const sead::Vector3f& vec) {
170 *out = vec;
171 return tryNormalizeOrZero(vec: out);
172}
173
174// TODO: Rename parameters here and in header
175bool isNearAngleDegreeHV(const sead::Vector3f& a, const sead::Vector3f& b, const sead::Vector3f& c,
176 f32 d, f32 e) {
177 return isNearAngleRadianHV(a, b, c, sead::Mathf::deg2rad(deg: d), sead::Mathf::deg2rad(deg: e));
178}
179
180bool isNear(f32 value, f32 target, f32 tolerance) {
181 return sead::Mathf::abs(x: value - target) < sead::Mathf::abs(x: tolerance);
182}
183
184bool isNear(const sead::Vector2f& value, const sead::Vector2f& target, f32 tolerance) {
185 return (value - target).length() <= tolerance;
186}
187
188bool isNear(const sead::Vector3f& value, const sead::Vector3f& target, f32 tolerance) {
189 return (value - target).length() <= tolerance;
190}
191
192bool isNear(const sead::Color4f& value, const sead::Color4f& target, f32 tolerance) {
193 return sead::Mathf::abs(x: value.r - target.r) < tolerance &&
194 sead::Mathf::abs(x: value.g - target.g) < tolerance &&
195 sead::Mathf::abs(x: value.b - target.b) < tolerance &&
196 sead::Mathf::abs(x: value.a - target.a) < tolerance;
197}
198
199bool isNearZero(f32 value, f32 tolerance) {
200 return sead::Mathf::abs(x: value) < tolerance;
201}
202
203bool isNearZero(const sead::Matrix34f& value, f32 tolerance) {
204 sead::Vector3f vec;
205
206 value.getBase(o&: vec, axis: 0);
207 if (isNearZero(vec, tolerance))
208 return true;
209 value.getBase(o&: vec, axis: 1);
210 if (isNearZero(vec, tolerance))
211 return true;
212 value.getBase(o&: vec, axis: 2);
213 if (isNearZero(vec, tolerance))
214 return true;
215
216 return false;
217}
218
219bool isNearZeroOrGreater(f32 value, f32 tolerance) {
220 return value >= 0.0f || isNearZero(value, tolerance);
221}
222
223bool isNearZeroOrLess(f32 value, f32 tolerance) {
224 return value <= 0.0f || isNearZero(value, tolerance);
225}
226
227bool isExistNearZeroVal(const sead::Vector3f& vec, f32 tolerance) {
228 return isNearZero(value: vec.x, tolerance) || isNearZero(value: vec.y, tolerance) ||
229 isNearZero(value: vec.z, tolerance);
230}
231
232bool isNormalize(const sead::Vector3f& vec, f32 tolerance) {
233 return sead::Mathf::abs(x: 1.0f - vec.length()) <= tolerance;
234}
235
236bool isParallelDirection(const sead::Vector2f& a, const sead::Vector2f& b, f32 tolerance) {
237 return !(sead::Mathf::abs(x: a.cross(t: b)) > tolerance);
238}
239
240bool isNearDirection(const sead::Vector2f& a, const sead::Vector2f& b, f32 tolerance) {
241 if (a.dot(t: b) < 0.0f)
242 return false;
243
244 return isParallelDirection(a, b, tolerance);
245}
246
247bool isInRange(s32 x, s32 a, s32 b) {
248 return (b < a) ? (a >= x && x >= b) : (b >= x && x >= a);
249}
250
251bool isInRange(f32 x, f32 a, f32 b) {
252 if (b < a) {
253 if (x < b || a < x)
254 return false;
255 return true;
256 } else {
257 if (x < a || b < x)
258 return false;
259 return true;
260 }
261}
262
263void normalize(sead::Vector2f* vec) {
264 vec->normalize();
265}
266
267void normalize(sead::Vector3f* vec) {
268 vec->normalize();
269}
270
271void normalize(sead::Matrix33f* mtx) {
272 sead::Vector3f up = mtx->getBase(axis: 0);
273 sead::Vector3f front = mtx->getBase(axis: 1);
274 sead::Vector3f side = mtx->getBase(axis: 2);
275
276 up.normalize();
277 front.normalize();
278 side.normalize();
279
280 mtx->setBase(axis: 0, v: up);
281 mtx->setBase(axis: 1, v: front);
282 mtx->setBase(axis: 2, v: side);
283}
284
285void normalize(sead::Matrix34f* mtx) {
286 sead::Vector3f up = mtx->getBase(axis: 0);
287 sead::Vector3f front = mtx->getBase(axis: 1);
288 sead::Vector3f side = mtx->getBase(axis: 2);
289
290 up.normalize();
291 front.normalize();
292 side.normalize();
293
294 mtx->setBase(axis: 0, v: up);
295 mtx->setBase(axis: 1, v: front);
296 mtx->setBase(axis: 2, v: side);
297}
298
299bool tryNormalizeOrZero(sead::Vector2f* vec) {
300 if (isNearZero(vec: *vec)) {
301 *vec = {0.0f, 0.0f};
302 return false;
303 }
304
305 normalize(vec);
306 return true;
307}
308
309bool tryNormalizeOrZero(sead::Vector2f* out, const sead::Vector2f& vec) {
310 *out = vec;
311 return tryNormalizeOrZero(vec: out);
312}
313
314bool tryNormalizeOrDirZ(sead::Vector3f* vec) {
315 if (isNearZero(vec: *vec)) {
316 // mismatches if this isn't set twice
317 vec->set(x_: 0.0f, y_: 0.0f, z_: 0.0f);
318 vec->set(sead::Vector3f::ez);
319 return false;
320 }
321
322 normalize(vec);
323 return true;
324}
325
326bool tryNormalizeOrDirZ(sead::Vector3f* out, const sead::Vector3f& vec) {
327 *out = vec;
328 return tryNormalizeOrDirZ(vec: out);
329}
330
331u32 getMaxAbsElementIndex(const sead::Vector3f& vec) {
332 f32 x = sead::Mathf::abs(x: vec.x);
333 f32 y = sead::Mathf::abs(x: vec.y);
334 f32 z = sead::Mathf::abs(x: vec.z);
335
336 return x > z && x > y ? 0 : y > z ? 1 : 2;
337}
338
339void setLength(sead::Vector3f* vec, f32 length) {
340 f32 curLen = vec->length();
341 if (curLen > 0.0f) {
342 f32 scale = length / curLen;
343 *vec *= scale;
344 }
345}
346
347void setProjectionLength(sead::Vector3f* out, const sead::Vector3f& vec, f32 length) {
348 f32 scale = length / sead::Mathf::abs(x: vec.dot(t: *out));
349 *out *= scale;
350}
351
352bool limitLength(sead::Vector2f* out, const sead::Vector2f& vec, f32 limit) {
353 f32 len = vec.length();
354 if (len > limit) {
355 f32 invLen = limit / len;
356 out->setScale(a: vec, t: invLen);
357 return true;
358 } else {
359 out->set(vec);
360 return false;
361 }
362}
363
364bool limitLength(sead::Vector3f* out, const sead::Vector3f& vec, f32 limit) {
365 f32 len = vec.length();
366 if (len > limit) {
367 f32 invLen = limit / len;
368 out->setScale(a: vec, t: invLen);
369 return true;
370 } else {
371 out->set(vec);
372 return false;
373 }
374}
375
376f32 normalizeAbs(f32 x, f32 min, f32 max) {
377 if (x >= 0)
378 return normalize(x, min, max);
379 else
380 return -normalize(x: -x, min, max);
381}
382
383f32 normalize(f32 x, f32 min, f32 max) {
384 if (sead::Mathf::abs(x: max - min) < 0.001f) {
385 if (x < min)
386 return 0.0f;
387 else
388 return 1.0f;
389 }
390
391 f32 clamped = sead::Mathf::clamp(value: x, low: min, high: max);
392 return (clamped - min) / (max - min);
393}
394
395f32 normalize(s32 x, s32 min, s32 max) {
396 if (x <= min)
397 return 0.0f;
398 if (max - min <= 0) {
399 if (x < min)
400 return 0.0f;
401 else
402 return 1.0f;
403 }
404
405 return (static_cast<f32>(clamp(value: x, min, max) - min) / static_cast<f32>(max - min));
406}
407
408f32 sign(f32 x) {
409 if (x < 0.0f)
410 return -1.0f;
411 if (x > 0.0f)
412 return 1.0f;
413 return x;
414}
415
416s32 sign(s32 x) {
417 if (x < 0)
418 return -1;
419 if (x > 0)
420 return 1;
421 return x;
422}
423
424f32 cubeRoot(f32 x) {
425 f32 onethird = 1.0f / 3.0f;
426
427 u32 i = 0x54a0fc86 - sead::BitUtil::bitCast<u32>(value: x) / 3;
428 f32 y = sead::BitUtil::bitCast<f32>(value: i);
429
430 y = y * onethird * (4.0f - x * y * y * y);
431 y = y * onethird * (4.0f - x * y * y * y);
432 y = y * onethird * (4.0f - x * y * y * y);
433 return x * y * y;
434}
435
436void clampV3f(sead::Vector3f* out, const sead::Vector3f& min, const sead::Vector3f& max) {
437 out->x = sead::Mathf::clamp(value: out->x, low: min.x, high: max.x);
438 out->y = sead::Mathf::clamp(value: out->y, low: min.y, high: max.y);
439 out->z = sead::Mathf::clamp(value: out->z, low: min.z, high: max.z);
440}
441
442void clampV2f(sead::Vector2f* out, const sead::Vector2f& min, const sead::Vector2f& max) {
443 out->x = sead::Mathf::clamp(value: out->x, low: min.x, high: max.x);
444 out->y = sead::Mathf::clamp(value: out->y, low: min.y, high: max.y);
445}
446
447f32 easeIn(f32 t) {
448 return (((t * -0.5f) + 1.5f) * t) * t;
449}
450
451f32 easeOut(f32 t) {
452 return (((t * -0.5f) * t) + 1.5f) * t;
453}
454
455f32 easeInOut(f32 t) {
456 return (((t * -2.0f) + 3.0f) * t) * t;
457}
458
459f32 squareIn(f32 t) {
460 return t * t;
461}
462
463f32 squareOut(f32 t) {
464 return (2.0f - t) * t;
465}
466
467f32 powerIn(f32 t, f32 exp) {
468 return sead::Mathf::pow(x: t, y: exp);
469}
470
471f32 powerOut(f32 t, f32 exp) {
472 return sead::Mathf::pow(x: t, y: 1.0 / exp);
473}
474
475f32 logarithmIn(f32 t, f32 base) {
476 f32 base1 = base + sead::Mathf::epsilon();
477 f32 a = powf(base1 + 0.0f, 1.0 - t);
478 f32 b = powf(base1 + 1.0f, t);
479 return a * b - base1;
480}
481
482f32 logarithmOut(f32 t, f32 base) {
483 return 1.0f - logarithmIn(t: 1.0f - t, base);
484}
485
486f32 exponentIn(f32 t, f32 exp) {
487 return t * exp2f(exp * (t - 1.0f));
488}
489
490f32 exponentOut(f32 t, f32 exp) {
491 return 1.0f - exponentIn(t: 1.0f - t, exp);
492}
493
494f32 hermiteRate(f32 t, f32 m0, f32 m1) {
495 return hermite(y0: 0.0f, m0, y1: 1.0f, m1, t);
496}
497
498f32 calcFourthOrderRate(f32 t, f32 scale) {
499 return ((scale + -3.0f) * t * t + (scale * -2.0f + 4.0f) * t + scale) * t * t;
500}
501
502f32 calcTriangleWave01(f32 t, f32 period) {
503 f32 val = t / (2 * period);
504 s32 rounded = (s32)val + (((s32)val == val) ? 0 : -!(val >= 0.0f));
505
506 val = sead::Mathf::abs(x: val - rounded - 0.5f);
507 return 1.0f - (2 * val);
508}
509
510f32 calcTriangleWave(f32 t, f32 min, f32 max, f32 period) {
511 f32 pos = sead::Mathf::clamp(value: calcTriangleWave01(t, period), low: 0.0f, high: 1.0f);
512 return (1.0f - pos) * min + pos * max;
513}
514
515f32 lerpValue(f32 a, f32 b, f32 t) {
516 t = sead::Mathf::clamp(value: t, low: 0.0f, high: 1.0f);
517 return (a * (1.0f - t)) + (t * b);
518}
519
520f32 calcRate01(f32 t, f32 min, f32 max) {
521 f32 range = max - min;
522 if (isNearZero(value: range))
523 return 1.0f;
524 return sead::Mathf::clamp(value: (t - min) / range, low: 0.0f, high: 1.0f);
525}
526
527f32 easeByType(f32 t, s32 easeType) {
528 switch (easeType) {
529 case EaseType_EaseIn:
530 return easeIn(t);
531 case EaseType_EaseOut:
532 return easeOut(t);
533 case EaseType_EaseInOut:
534 return easeInOut(t);
535 case EaseType_SquareIn:
536 return squareIn(t);
537 case EaseType_SquareOut:
538 return squareOut(t);
539 default:
540 return t;
541 }
542}
543
544/**
545 * Interpolates between `y0` and `y1` as `t` goes from 0.0 to 1.0. This interpolation is defined by
546 * `m0` and `m1`, which are the rates of change of `t` at the points `y0` and `y1` respectively.
547 */
548f32 hermite(f32 y0, f32 m0, f32 y1, f32 m1, f32 t) {
549 f32 coef_m1 = t * (t * t - t);
550 f32 coef_y1 = t * t + -2.0f * coef_m1;
551 f32 coef_m0 = coef_m1 - (t * t - t);
552 return y0 - coef_y1 * y0 + coef_y1 * y1 + coef_m0 * m0 + coef_m1 * m1;
553}
554
555f32 hermite(f32 y0, f32 m0, f32 y1, f32 m1, f32 t, f32 width) {
556 t *= 1.0f / width;
557 f32 a1 = y0 - y1;
558 f32 a2 = t - 1.0f;
559 f32 a3 = t + t - 3.0f;
560 return y0 + (a1 * a3) * t * t + (t * a2) * (t * m1 + a2 * m0);
561}
562
563u8 reverseBit8(u8 x) {
564 x = ((x & 0x55) << 1) | ((x >> 1) & 0x55); // 0101...
565 x = ((x & 0x33) << 2) | ((x >> 2) & 0x33); // 0011...
566 return x >> 4 | x << 4;
567}
568
569u16 reverseBit16(u16 x) {
570 x = ((x & 0x5555) << 1) | ((x >> 1) & 0x5555); // 01010101...
571 x = ((x & 0x3333) << 2) | ((x >> 2) & 0x3333); // 00110011...
572 x = ((x & 0xf0f) << 4) | ((x >> 4) & 0xf0f); // 11110000..
573 return x >> 8 | x << 8;
574}
575
576u32 reverseBit32(u32 x) {
577 x = ((x & 0x55555555) << 1) | ((x >> 1) & 0x55555555); // 0101010101010101...
578 x = ((x & 0x33333333) << 2) | ((x >> 2) & 0x33333333); // 0011001100110011...
579 x = ((x & 0xf0f0f0f) << 4) | ((x >> 4) & 0xf0f0f0f); // 1111000011110000...
580 x = ((x & 0xff00ff) << 8) | ((x >> 8) & 0xff00ff); // 1111111100000000...
581 return x >> 16 | x << 16;
582}
583
584f32 calcVanDerCorput(u32 x) {
585 return reverseBit32(x) * (f32)0x1p-32;
586}
587
588void calcHammersleyPoint(sead::Vector2f* outPoint, u32 i, u32 num) {
589 outPoint->x = (f32)i / (f32)num;
590 outPoint->y = calcVanDerCorput(x: i);
591}
592
593f32 getRandom() {
594 u32 random = (sead::GlobalRandom::instance()->getU32() >> 9) | 0x3F800000;
595 return (*reinterpret_cast<f32*>(&random)) - 1;
596}
597
598f32 getRandom(f32 factor) {
599 return getRandom(min: 0.0f, max: factor);
600}
601
602f32 getRandom(f32 min, f32 max) {
603 return (getRandom() * (max - min)) + min;
604}
605
606s32 getRandom(s32 factor) {
607 return getRandom(min: 0, max: factor);
608}
609
610s32 getRandom(s32 min, s32 max) {
611 return (s32)getRandom(min: (f32)min, max: (f32)max);
612}
613
614f32 getRandomDegree() {
615 return getRandom(factor: 360.0f);
616}
617
618f32 getRandomRadian() {
619 return getRandom(factor: 6.2832f);
620}
621
622void getRandomVector(sead::Vector3f* vec, f32 factor) {
623 f32 x = (getRandom() * (factor + factor)) - factor;
624 f32 y = (getRandom() * (factor + factor)) - factor;
625 f32 z = (getRandom() * (factor + factor)) - factor;
626 vec->x = x;
627 vec->y = y;
628 vec->z = z;
629}
630
631void getRandomDir(sead::Vector3f* vec) {
632 getRandomVector(vec, factor: 10.0f);
633 while (vec->dot(t: *vec) < 0.000001f) {
634 *vec = {0.0f, 0.0f, 0.0f};
635 getRandomVector(vec, factor: 10.0f);
636 }
637 vec->normalize();
638}
639
640f32 modf(f32 a, f32 b) {
641 return std::fmodf(a, b);
642}
643
644s32 modi(s32 a, s32 b) {
645 return a - (a / b) * b;
646}
647
648f32 calcSpeedMax(f32 accel, f32 friction) {
649 return (accel * friction) / (1.0f - friction);
650}
651
652f32 calcAccel(f32 speed, f32 friction) {
653 return (1.0f - friction) * speed / friction;
654}
655
656f32 calcFriction(f32 accel, f32 speed) {
657 // BUG: N's mistake here. Correct function: friction = speed / (speed + accel)
658 return (accel + speed) / speed;
659}
660
661inline f32 round(f32 v) {
662 return (s32)(v + (v >= 0.0f ? 0.5f : -0.5f));
663}
664
665void roundOffVec(sead::Vector3f* outVec, const sead::Vector3f& vec) {
666 outVec->x = round(v: vec.x);
667 outVec->y = round(v: vec.y);
668 outVec->z = round(v: vec.z);
669}
670
671void roundOffVec(sead::Vector3f* vec) {
672 roundOffVec(outVec: vec, vec: *vec);
673}
674
675void roundOffVec(sead::Vector2f* outVec, const sead::Vector2f& vec) {
676 outVec->x = round(v: vec.x);
677 outVec->y = round(v: vec.y);
678}
679
680void roundOffVec(sead::Vector2f* vec) {
681 roundOffVec(outVec: vec, vec: *vec);
682}
683
684f32 snapToGrid(f32 val, f32 gridSize, f32 offset) {
685 return round(v: (val - offset) / gridSize) * gridSize + offset;
686}
687
688void snapVecToGrid(sead::Vector3f* outVec, const sead::Vector3f& vec, f32 gridSize,
689 const sead::Vector3f& offset) {
690 outVec->x = snapToGrid(val: vec.x, gridSize, offset: offset.x);
691 outVec->y = snapToGrid(val: vec.y, gridSize, offset: offset.y);
692 outVec->z = snapToGrid(val: vec.z, gridSize, offset: offset.z);
693}
694
695void snapVecToGrid(sead::Vector3f* outVec, const sead::Vector3f& vec,
696 const sead::Vector3f& gridSize, const sead::Vector3f& offset) {
697 outVec->x = snapToGrid(val: vec.x, gridSize: gridSize.x, offset: offset.x);
698 outVec->y = snapToGrid(val: vec.y, gridSize: gridSize.y, offset: offset.y);
699 outVec->z = snapToGrid(val: vec.z, gridSize: gridSize.z, offset: offset.z);
700}
701
702void limitVectorOppositeDir(sead::Vector3f* outVec, const sead::Vector3f& inVec,
703 const sead::Vector3f& dir, f32 scale) {
704 scale = sead::Mathf::clamp(value: dir.dot(t: inVec), low: -scale, high: 0.0f);
705 outVec->setScaleAdd(t: -scale, a: inVec, b: dir);
706}
707
708void scaleVectorDirection(sead::Vector3f* outVec, const sead::Vector3f& inVec,
709 const sead::Vector3f& dir, f32 scale) {
710 sead::Vector3f direction = inVec * inVec.dot(t: dir);
711 outVec->setAdd(a: dir - direction, b: direction * scale);
712}
713
714void scaleVectorExceptDirection(sead::Vector3f* outVec, const sead::Vector3f& inVec,
715 const sead::Vector3f& dir, f32 scale) {
716 sead::Vector3f direction = inVec * inVec.dot(t: dir);
717 outVec->setScaleAdd(t: scale, a: dir - direction, b: direction);
718}
719
720bool calcDir(sead::Vector3f* outVec, const sead::Vector3f& vecA, const sead::Vector3f& vecB) {
721 outVec->setSub(a: vecB, b: vecA);
722 return !tryNormalizeOrZero(vec: outVec);
723}
724
725bool calcDirH(sead::Vector3f* outVec, const sead::Vector3f& vecA, const sead::Vector3f& vecB) {
726 return calcDirOnPlane(outVec, vecA, vecB, plane: sead::Vector3f::ey);
727}
728
729bool calcDirOnPlane(sead::Vector3f* outVec, const sead::Vector3f& vecA, const sead::Vector3f& vecB,
730 const sead::Vector3f& plane) {
731 outVec->setSub(a: vecB, b: vecA);
732 outVec->setScaleAdd(t: -plane.dot(t: *outVec), a: plane, b: *outVec);
733 return !tryNormalizeOrZero(vec: outVec);
734}
735
736f32 mapRangeLogarithmic(f32 x, f32 min, f32 max, f32 start, f32 end, f32 exponent) {
737 f32 base = sead::Mathf::pow(x: 10.0f, y: -exponent);
738 f32 val = (x - min) / (max - min);
739 f32 range = sead::Mathf::abs(x: end - start);
740 f32 direction = end > start ? 1.0f : -1.0f;
741
742 return sead::Mathf::pow(x: val, y: base) * direction * range + start;
743}
744
745void calcDirFromLongitudeLatitude(sead::Vector3f* outVec, f32 longitude, f32 latitude) {
746 outVec->y = -sead::Mathf::sin(t: sead::Mathf::deg2rad(deg: latitude));
747 f32 cosLatitude = -sead::Mathf::cos(t: sead::Mathf::deg2rad(deg: latitude));
748 outVec->x = sead::Mathf::sin(t: sead::Mathf::deg2rad(deg: longitude)) * cosLatitude;
749 outVec->z = sead::Mathf::cos(t: sead::Mathf::deg2rad(deg: longitude)) * cosLatitude;
750}
751
752void calcLongitudeLatitudeFromDir(f32* longitude, f32* latitude, const sead::Vector3f& dir) {
753 sead::Vector3f dirNormalized = dir;
754 dirNormalized.normalize();
755 if (isNearZero(vec: dirNormalized))
756 return;
757 *latitude = sead::Mathf::asin(t: sead::Mathf::clamp(value: -dirNormalized.y, low: -1.0f, high: 1.0f));
758
759 sead::Vector2f newVec = {-dirNormalized.z, -dirNormalized.x};
760 newVec.normalize();
761 if (isNearZero(vec: newVec))
762 return;
763
764 *longitude = sead::Mathf::atan2(y: newVec.y, x: newVec.x);
765}
766
767u32 getMaxAbsElementIndex(const sead::Vector3i& vec) {
768 s32 x = sead::Mathi::abs(x: vec.x);
769 s32 y = sead::Mathi::abs(x: vec.y);
770 s32 z = sead::Mathi::abs(x: vec.z);
771
772 return x > z && x > y ? 0 : y > z ? 1 : 2;
773}
774
775f32 getMaxAbsElementValue(const sead::Vector3f& vec) {
776 switch (getMaxAbsElementIndex(vec)) {
777 case 0:
778 return vec.x;
779 case 1:
780 return vec.y;
781 case 2:
782 return vec.z;
783 }
784 return vec.z;
785}
786
787s32 getMaxAbsElementValue(const sead::Vector3i& vec) {
788 switch (getMaxAbsElementIndex(vec)) {
789 case 0:
790 return vec.x;
791 case 1:
792 return vec.y;
793 case 2:
794 return vec.z;
795 }
796 return vec.z;
797}
798
799u32 getMinAbsElementIndex(const sead::Vector3f& vec) {
800 f32 x = sead::Mathf::abs(x: vec.x);
801 f32 y = sead::Mathf::abs(x: vec.y);
802 f32 z = sead::Mathf::abs(x: vec.z);
803
804 return x < z && x < y ? 0 : y < z ? 1 : 2;
805}
806
807u32 getMinAbsElementIndex(const sead::Vector3i& vec) {
808 s32 x = sead::Mathi::abs(x: vec.x);
809 s32 y = sead::Mathi::abs(x: vec.y);
810 s32 z = sead::Mathi::abs(x: vec.z);
811
812 return x < z && x < y ? 0 : y < z ? 1 : 2;
813}
814
815f32 getMinAbsElementValue(const sead::Vector3f& vec) {
816 switch (getMinAbsElementIndex(vec)) {
817 case 0:
818 return vec.x;
819 case 1:
820 return vec.y;
821 case 2:
822 return vec.z;
823 }
824 return vec.z;
825}
826
827s32 getMinAbsElementValue(const sead::Vector3i& vec) {
828 switch (getMinAbsElementIndex(vec)) {
829 case 0:
830 return vec.x;
831 case 1:
832 return vec.y;
833 case 2:
834 return vec.z;
835 }
836 return vec.z;
837}
838
839void makeQuatFromTwoAxis(sead::Quatf* outQuat, const sead::Vector3f& vectorA,
840 const sead::Vector3f& vectorB, s32 axisA, s32 axisB) {
841 sead::Matrix34f mtx = sead::Matrix34f::ident;
842 makeMtxFromTwoAxis(outMtx: &mtx, vectorA, vectorB, axisA, axisB);
843 mtx.toQuat(q&: *outQuat);
844}
845
846void makeQuatFrontUp(sead::Quatf* outQuat, const sead::Vector3f& front, const sead::Vector3f& up) {
847 sead::Matrix34f mtx = sead::Matrix34f::ident;
848 makeMtxFrontUp(outMtx: &mtx, front, up);
849 mtx.toQuat(q&: *outQuat);
850}
851
852void makeQuatFrontSide(sead::Quatf* outQuat, const sead::Vector3f& front,
853 const sead::Vector3f& side) {
854 sead::Matrix34f mtx = sead::Matrix34f::ident;
855 makeMtxFrontSide(outMtx: &mtx, front, side);
856 mtx.toQuat(q&: *outQuat);
857}
858
859void makeQuatFrontNoSupport(sead::Quatf* outQuat, const sead::Vector3f& front) {
860 sead::Matrix34f mtx = sead::Matrix34f::ident;
861 makeMtxFrontNoSupport(outMtx: &mtx, front);
862 mtx.toQuat(q&: *outQuat);
863}
864
865void makeQuatUpFront(sead::Quatf* outQuat, const sead::Vector3f& up, const sead::Vector3f& front) {
866 sead::Matrix34f mtx = sead::Matrix34f::ident;
867 makeMtxUpFront(outMtx: &mtx, up, front);
868 mtx.toQuat(q&: *outQuat);
869}
870
871void makeQuatUpSide(sead::Quatf* outQuat, const sead::Vector3f& up, const sead::Vector3f& side) {
872 sead::Matrix34f mtx = sead::Matrix34f::ident;
873 makeMtxUpSide(outMtx: &mtx, up, side);
874 mtx.toQuat(q&: *outQuat);
875}
876
877void makeQuatUpNoSupport(sead::Quatf* outQuat, const sead::Vector3f& up) {
878 sead::Matrix34f mtx = sead::Matrix34f::ident;
879 makeMtxUpNoSupport(outMtx: &mtx, up);
880 mtx.toQuat(q&: *outQuat);
881}
882
883void makeQuatSideUp(sead::Quatf* outQuat, const sead::Vector3f& side, const sead::Vector3f& up) {
884 sead::Matrix34f mtx = sead::Matrix34f::ident;
885 makeMtxFromTwoAxis(outMtx: &mtx, vectorA: side, vectorB: up, axisA: 0, axisB: 1);
886 mtx.toQuat(q&: *outQuat);
887}
888
889void makeQuatSideFront(sead::Quatf* outQuat, const sead::Vector3f& side,
890 const sead::Vector3f& front) {
891 sead::Matrix34f mtx = sead::Matrix34f::ident;
892 makeMtxFromTwoAxis(outMtx: &mtx, vectorA: side, vectorB: front, axisA: 0, axisB: 2);
893 mtx.toQuat(q&: *outQuat);
894}
895
896void makeQuatSideNoSupport(sead::Quatf* outQuat, const sead::Vector3f& side) {
897 sead::Matrix34f mtx = sead::Matrix34f::ident;
898 makeMtxSideNoSupport(outMtx: &mtx, side);
899 mtx.toQuat(q&: *outQuat);
900}
901
902// https://decomp.me/scratch/MqKUQ
903// NON_MATCHING: Regswap on Mult and Add
904void rotateQuatRadian(sead::Quatf* outQuat, const sead::Quatf& quat, const sead::Vector3f& vec,
905 f32 angle) {
906 f32 cos = sead::Mathf::cos(t: angle * 0.5f);
907 f32 sin = sead::Mathf::sin(t: angle * 0.5f);
908
909 sead::Quatf rotation;
910 rotation.w = cos;
911 rotation.x = sin * vec.x;
912 rotation.y = sin * vec.y;
913 rotation.z = sin * vec.z;
914
915 *outQuat = rotation * quat;
916 outQuat->normalize();
917}
918
919void makeQuatXDegree(sead::Quatf* outQuat, f32 angle) {
920 f32 angleRad = sead::Mathf::deg2rad(deg: angle * 0.5f);
921 f32 cos = sead::Mathf::cos(t: angleRad);
922 f32 sin = sead::Mathf::sin(t: angleRad);
923 outQuat->w = cos;
924 outQuat->x = sin;
925 outQuat->y = 0.0f;
926 outQuat->z = 0.0f;
927}
928
929void makeQuatYDegree(sead::Quatf* outQuat, f32 angle) {
930 f32 angleRad = sead::Mathf::deg2rad(deg: angle * 0.5f);
931 f32 cos = sead::Mathf::cos(t: angleRad);
932 f32 sin = sead::Mathf::sin(t: angleRad);
933 outQuat->w = cos;
934 outQuat->x = 0.0f;
935 outQuat->y = sin;
936 outQuat->z = 0.0f;
937}
938
939void makeQuatZDegree(sead::Quatf* outQuat, f32 angle) {
940 f32 angleRad = sead::Mathf::deg2rad(deg: angle * 0.5f);
941 f32 cos = sead::Mathf::cos(t: angleRad);
942 f32 sin = sead::Mathf::sin(t: angleRad);
943 outQuat->w = cos;
944 outQuat->x = 0.0f;
945 outQuat->y = 0.0f;
946 outQuat->z = sin;
947}
948
949// https://decomp.me/scratch/utMuy
950// NON_MATCHING: Regswap on Add
951void rotateQuatXDirDegree(sead::Quatf* outQuat, const sead::Quatf& quat, f32 angle) {
952 sead::Quatf rotation;
953 makeQuatXDegree(outQuat: &rotation, angle);
954 *outQuat = quat * rotation;
955 outQuat->normalize();
956}
957
958// https://decomp.me/scratch/DEZoH
959// NON_MATCHING: Regswap on Add
960void rotateQuatYDirDegree(sead::Quatf* outQuat, const sead::Quatf& quat, f32 angle) {
961 sead::Quatf rotation;
962 makeQuatYDegree(outQuat: &rotation, angle);
963 *outQuat = quat * rotation;
964 outQuat->normalize();
965}
966
967// https://decomp.me/scratch/iJBbn
968// NON_MATCHING: Regswap on Add
969void rotateQuatZDirDegree(sead::Quatf* outQuat, const sead::Quatf& quat, f32 angle) {
970 sead::Quatf rotation;
971 makeQuatZDegree(outQuat: &rotation, angle);
972 *outQuat = quat * rotation;
973 outQuat->normalize();
974}
975
976void rotateQuatLocalDirDegree(sead::Quatf* outQuat, const sead::Quatf& quat, s32 axis, f32 angle) {
977 sead::Vector3f vec;
978 switch (axis) {
979 case 0:
980 vec.setRotated(q: quat, a: sead::Vector3f::ex);
981 break;
982 case 1:
983 vec.setRotated(q: quat, a: sead::Vector3f::ey);
984 break;
985 case 2:
986 vec.setRotated(q: quat, a: sead::Vector3f::ez);
987 break;
988 default:
989 return;
990 }
991
992 rotateQuatRadian(outQuat, quat, vec, angle: sead::Mathf::deg2rad(deg: angle));
993}
994
995// https://decomp.me/scratch/nV2zl
996// NON_MATCHING: Multiple issues
997void rotateQuatMoment(sead::Quatf* outQuat, const sead::Quatf& quat, const sead::Vector3f& vec) {
998 sead::Vector3f vecNorm;
999 tryNormalizeOrZero(out: &vecNorm, vec);
1000
1001 f32 angle = vec.length() * 0.5f;
1002 f32 cos = sead::Mathf::cos(t: angle);
1003 f32 sin = sead::Mathf::sin(t: angle);
1004
1005 sead::Quatf rotation;
1006 rotation.w = cos;
1007 rotation.x = sin * vecNorm.x;
1008 rotation.y = sin * vecNorm.y;
1009 rotation.z = sin * vecNorm.z;
1010 *outQuat = rotation * quat;
1011 outQuat->normalize();
1012}
1013
1014// https://decomp.me/scratch/l1K35
1015// NON_MATCHING: Multiple issues
1016void rotateQuatMomentDegree(sead::Quatf* outQuat, const sead::Quatf& quat,
1017 const sead::Vector3f& vec) {
1018 sead::Vector3f vecNorm;
1019 tryNormalizeOrZero(out: &vecNorm, vec);
1020
1021 f32 angle = sead::Mathf::deg2rad(deg: vec.length()) * 0.5f;
1022 f32 cos = sead::Mathf::cos(t: angle);
1023 f32 sin = sead::Mathf::sin(t: angle);
1024
1025 sead::Quatf rotation;
1026 rotation.w = cos;
1027 rotation.x = sin * vecNorm.x;
1028 rotation.y = sin * vecNorm.y;
1029 rotation.z = sin * vecNorm.z;
1030 *outQuat = rotation * quat;
1031 outQuat->normalize();
1032}
1033
1034void rotateQuatRollBall(sead::Quatf* outQuat, const sead::Quatf& quat, const sead::Vector3f& vecA,
1035 const sead::Vector3f& vecB, f32 scale) {
1036 sead::Vector3f vecNorm;
1037 calcMomentRollBall(&vecNorm, vecA, vecB, scale);
1038 rotateQuatMoment(outQuat, quat, vec: vecNorm);
1039}
1040
1041void calcMomentRollBall(sead::Vector3f* outVec, const sead::Vector3f& vecA,
1042 const sead::Vector3f& vecB, f32 scale) {
1043 sead::Vector3f vecNorm = vecB;
1044 if (!tryNormalizeOrZero(vec: &vecNorm)) {
1045 *outVec = vecNorm;
1046 return;
1047 }
1048
1049 vecNorm.setCross(a: vecNorm, b: vecA);
1050 scale = 1.0f / scale;
1051 *outVec = scale * vecNorm;
1052}
1053
1054void calcCirclePointPicking(sead::Vector2f* outPoint, f32 x, f32 y) {
1055 f32 invLength = 1.0 / (x * x + y * y);
1056 outPoint->x = (x * x - y * y) * invLength;
1057 outPoint->y = 2 * x * y * invLength;
1058}
1059
1060void pickUniformPointsOnCircleHammersley(sead::Vector2f* outPoint, u32 x, u32 y) {
1061 sead::Vector2f hammersleyPoint;
1062 calcHammersleyPoint(outPoint: &hammersleyPoint, i: x, num: y);
1063 calcCirclePointPicking(outPoint, x: hammersleyPoint.x, y: hammersleyPoint.y);
1064}
1065
1066void calcDiskPointPicking(sead::Vector2f* outPoint, f32 radius, f32 angle) {
1067 radius = sead::Mathf::sqrt(t: radius);
1068 f32 cos = sead::Mathf::cos(t: angle * sead::Mathf::pi2());
1069 f32 sin = sead::Mathf::sin(t: angle * sead::Mathf::pi2());
1070 outPoint->x = radius * cos;
1071 outPoint->y = radius * sin;
1072}
1073
1074void pickUniformPointsOnDiskHammersley(sead::Vector2f* outPoint, u32 x, u32 y) {
1075 sead::Vector2f hammersleyPoint;
1076 calcHammersleyPoint(outPoint: &hammersleyPoint, i: x, num: y);
1077 calcDiskPointPicking(outPoint, radius: hammersleyPoint.x, angle: hammersleyPoint.y);
1078}
1079
1080void pickUniformPointOnDisk(sead::Vector2f* outPoint) {
1081 calcDiskPointPicking(outPoint, radius: getRandom(), angle: getRandom());
1082}
1083
1084void calcSpherePointPicking(sead::Vector3f* outPoint, f32 x, f32 y) {
1085 f32 xx = 2 * x - 1.0f;
1086 f32 angle = y * sead::Mathf::pi() * 2;
1087 f32 radius = sead::Mathf::sqrt(t: 1.0f - xx * xx);
1088 f32 cos = radius * sead::Mathf::cos(t: angle);
1089 f32 sin = radius * sead::Mathf::sin(t: angle);
1090 outPoint->x = cos;
1091 outPoint->y = sin;
1092 outPoint->z = xx;
1093}
1094
1095void pickUniformPointOnSphere(sead::Vector3f* outPoint) {
1096 calcSpherePointPicking(outPoint, x: getRandom(), y: getRandom());
1097}
1098
1099void calcParabolicFunctionParam(f32* gravity, f32* initialVelY, f32 maxHeight,
1100 f32 verticalDistance) {
1101 f32 maxHeightSign = sign(x: maxHeight);
1102
1103 f32 maxHeightAdjusted =
1104 sead::Mathf::sqrt(t: sead::Mathf::clampMin(val: (maxHeight - verticalDistance) * maxHeight, min_: 0.0));
1105 *initialVelY = 2 * ((maxHeightSign * maxHeightAdjusted) + maxHeight);
1106 *gravity = verticalDistance - *initialVelY;
1107}
1108
1109} // namespace al
1110
1111namespace Intersect {
1112
1113bool calcX(sead::Vector3f* outVec, f32 value, const sead::Vector3f& vectorA,
1114 const sead::Vector3f& vectorB, const sead::Vector3f& min, const sead::Vector3f& max) {
1115 f32 x = (value - vectorA.x) / vectorB.x;
1116 if ((x < 0.0f || x != 1.0f) && (x < 0.0f || 1.0f <= x))
1117 return false;
1118
1119 f32 y = vectorA.y + x * vectorB.y;
1120 if (!(min.y <= y && y <= max.y))
1121 return false;
1122
1123 f32 z = vectorA.z + x * vectorB.z;
1124 if (!(min.z <= z && z <= max.z))
1125 return false;
1126
1127 x = vectorA.x + vectorB.x * x;
1128 if (outVec) {
1129 outVec->x = x;
1130 outVec->y = y;
1131 outVec->z = z;
1132 }
1133 return true;
1134}
1135
1136bool calcY(sead::Vector3f* outVec, f32 value, const sead::Vector3f& vectorA,
1137 const sead::Vector3f& vectorB, const sead::Vector3f& min, const sead::Vector3f& max) {
1138 f32 y = (value - vectorA.y) / vectorB.y;
1139 if ((y < 0.0f || y != 1.0f) && (y < 0.0f || 1.0f <= y))
1140 return false;
1141
1142 f32 x = vectorA.x + y * vectorB.x;
1143 if (!(min.x <= x && x <= max.x))
1144 return false;
1145
1146 f32 z = vectorA.z + y * vectorB.z;
1147 if (!(min.z <= z && z <= max.z))
1148 return false;
1149 y = vectorA.y + vectorB.y * y;
1150 if (outVec) {
1151 outVec->x = x;
1152 outVec->y = y;
1153 outVec->z = z;
1154 }
1155 return true;
1156}
1157
1158bool calcZ(sead::Vector3f* outVec, f32 value, const sead::Vector3f& vectorA,
1159 const sead::Vector3f& vectorB, const sead::Vector3f& min, const sead::Vector3f& max) {
1160 f32 z = (value - vectorA.z) / vectorB.z;
1161 if ((z < 0.0f || z != 1.0f) && (z < 0.0f || 1.0f <= z))
1162 return false;
1163
1164 f32 x = vectorA.x + z * vectorB.x;
1165 if (!(min.x <= x && x <= max.x))
1166 return false;
1167 f32 y = vectorA.y + z * vectorB.y;
1168 if (!(min.y <= y && y <= max.y))
1169 return false;
1170
1171 z = vectorA.z + vectorB.z * z;
1172 if (outVec) {
1173 outVec->x = x;
1174 outVec->y = y;
1175 outVec->z = z;
1176 }
1177 return true;
1178}
1179
1180} // namespace Intersect
1181