mirror of
https://github.com/encounter/mkdd.git
synced 2026-03-30 11:30:02 -07:00
SwareJonge
290 lines
7.8 KiB
C++
290 lines
7.8 KiB
C++
#ifndef JMATH_H
|
|
#define JMATH_H
|
|
|
|
#include <math.h>
|
|
#include <float.h>
|
|
|
|
#include <utility>
|
|
#include <dolphin/mtx.h>
|
|
#include <JSystem/JMath/Inline.h>
|
|
|
|
#include "types.h"
|
|
|
|
namespace JMath
|
|
{
|
|
inline void gekko_ps_copy12(register void *pDest, register const void *pSrc)
|
|
{
|
|
register f32 rxxyx;
|
|
register f32 rzxtx;
|
|
register f32 rxyyy;
|
|
register f32 rzyty;
|
|
register f32 rxzyz;
|
|
register f32 rzztz;
|
|
|
|
__asm {
|
|
// clang-format off
|
|
psq_l rxxyx, 0(pSrc), 0, 0
|
|
psq_l rzxtx, 8(pSrc), 0, 0
|
|
psq_l rxyyy, 0x10(pSrc), 0, 0
|
|
psq_l rzyty, 0x18(pSrc), 0, 0
|
|
psq_l rxzyz, 0x20(pSrc), 0, 0
|
|
psq_l rzztz, 0x28(pSrc), 0, 0
|
|
psq_st rxxyx, 0(pDest), 0, 0
|
|
psq_st rzxtx, 8(pDest), 0, 0
|
|
psq_st rxyyy, 0x10(pDest), 0, 0
|
|
psq_st rzyty, 0x18(pDest), 0, 0
|
|
psq_st rxzyz, 0x20(pDest), 0, 0
|
|
psq_st rzztz, 0x28(pDest), 0, 0
|
|
// clang-format on
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
struct TAngleConstant_
|
|
{
|
|
static const f32 RADIAN_DEG090() { return HALF_PI; }
|
|
static const f32 RADIAN_DEG180() { return PI; }
|
|
static const f32 RADIAN_DEG360() { return TAU; }
|
|
static const f32 RADIAN_TO_DEGREE_FACTOR() { return 180.0f / RADIAN_DEG180(); }
|
|
};
|
|
|
|
template <int length, typename T>
|
|
struct TSinCosTable
|
|
{
|
|
TSinCosTable() { init(); }
|
|
|
|
void init()
|
|
{
|
|
for (int i = 0; i < 2048; i++)
|
|
{
|
|
mTable[i].first = sin(((f64)i * TAngleConstant_<f32>::RADIAN_DEG360()) / 2048.0);
|
|
mTable[i].second = cos(((f64)i * TAngleConstant_<f32>::RADIAN_DEG360()) / 2048.0);
|
|
}
|
|
}
|
|
/**
|
|
* elements are pairs of {sine, cosine}
|
|
*/
|
|
f32 sinShort(s16 v) const { return mTable[static_cast<u16>(v) >> 5].first; }
|
|
f32 cosShort(s16 v) const { return mTable[static_cast<u16>(v) >> 5].second; }
|
|
std::pair<f32, f32> mTable[2048];
|
|
};
|
|
|
|
template <int length, typename T>
|
|
struct TAtanTable
|
|
{
|
|
T atan2_(T, T) const;
|
|
T atan_(T) const;
|
|
T mTable[length];
|
|
};
|
|
|
|
template <int length, typename T>
|
|
struct TAsinAcosTable
|
|
{
|
|
T acos2_(T, T) const;
|
|
T acos_(T) const;
|
|
T mTable[length];
|
|
};
|
|
|
|
template <>
|
|
struct TAtanTable<1024, f32>
|
|
{
|
|
TAtanTable() { init(); }
|
|
|
|
void init()
|
|
{
|
|
for (int i = 0; i < (u32)1024; i++)
|
|
{
|
|
mTable[i] = atan(i / 1024.0);
|
|
}
|
|
mTable[0] = 0.0f;
|
|
mTable2[0] = TAngleConstant_<f32>::RADIAN_DEG180() / 4;
|
|
}
|
|
|
|
f32 atan_(f32) const;
|
|
f32 atan2_(f32, f32) const;
|
|
f32 atan2Radian(f32 y, f32 x) const { return atan2_(y, x); }
|
|
f32 alignmentHack(f32 x) const;
|
|
|
|
f32 calc(f32 y, f32 x) const
|
|
{
|
|
if (x >= 0.0f)
|
|
{
|
|
if (x >= y)
|
|
{
|
|
return (0.0f == x ? 0.0f : mTable[(int)((y * 1024.0f) / x + 0.5f)]);
|
|
}
|
|
else
|
|
{
|
|
return HALF_PI - (y == 0.0f ? 0.0f : mTable[(int)((x * 1024.0f) / y + 0.5f)]);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
x = -x;
|
|
if (x < y)
|
|
{
|
|
return (y == 0.0f ? 0.0f : mTable[(int)((x * 1024.0f) / y + 0.5f)]) + HALF_PI;
|
|
}
|
|
else
|
|
{
|
|
return PI - (x == 0.0f ? 0.0f : mTable[(int)((y * 1024.0f) / x + 0.5f)]);
|
|
}
|
|
}
|
|
}
|
|
|
|
f32 calcInverse(f32 y, f32 x) const
|
|
{
|
|
y = -y;
|
|
if (x < 0.0f)
|
|
{
|
|
x = -x;
|
|
if (x >= y)
|
|
{
|
|
return (x == 0.0f ? 0.0f : mTable[(int)((y * 1024.0f) / x + 0.5f)]) + -PI;
|
|
}
|
|
else
|
|
{
|
|
return -HALF_PI - (y == 0.0f ? 0.0f : mTable[(int)((x * 1024.0f) / y + 0.5f)]);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (x < y)
|
|
{
|
|
return (y == 0.0f ? 0.0f : mTable[(int)((x * 1024.0f) / y + 0.5f)]) + -HALF_PI;
|
|
}
|
|
else
|
|
{
|
|
return -(x == 0.0f ? 0.0f : mTable[(int)((y * 1024.0f) / x + 0.5f)]);
|
|
}
|
|
}
|
|
}
|
|
|
|
f32 mTable[1024];
|
|
f32 mTable2[8];
|
|
};
|
|
|
|
template <>
|
|
struct TAsinAcosTable<1024, f32>
|
|
{
|
|
TAsinAcosTable() { init(); }
|
|
void init()
|
|
{
|
|
for (int i = 0; i < 1024; i++)
|
|
{
|
|
mTable[i] = asin(i / 1024.0);
|
|
}
|
|
mTable[0] = 0.0f;
|
|
mTable2[0] = TAngleConstant_<f32>::RADIAN_DEG180() / 4;
|
|
}
|
|
f32 acos2_(f32, f32) const;
|
|
f32 acos_(f32) const;
|
|
f32 mTable[1024];
|
|
f32 mTable2[8];
|
|
};
|
|
|
|
extern const TAsinAcosTable<1024, f32> asinAcosTable_ __attribute__((aligned(32)));
|
|
extern const TSinCosTable<11, f32> sincosTable_ __attribute__((aligned(32)));
|
|
extern const TAtanTable<1024, f32> atanTable_ __attribute__((aligned(32)));
|
|
|
|
class TRandom_fast_
|
|
{
|
|
public:
|
|
TRandom_fast_(u32 num); // maybe has a default ctor?
|
|
u32 get()
|
|
{
|
|
seed = (seed * 0x19660d) + 0x3c6ef35f;
|
|
return seed;
|
|
}
|
|
|
|
// from TP decomp
|
|
f32 get_ufloat_1()
|
|
{
|
|
// !@bug UB: in C++ it's not legal to read from an union member other
|
|
// than the last one that was written to.
|
|
union
|
|
{
|
|
f32 f;
|
|
u32 s;
|
|
} out;
|
|
out.s = (this->get() >> 9) | 0x3f800000;
|
|
return out.f - 1;
|
|
}
|
|
|
|
void setSeed(u32 Seed)
|
|
{
|
|
seed = Seed;
|
|
}
|
|
|
|
protected:
|
|
u32 seed;
|
|
};
|
|
template <typename T>
|
|
class TRandom_ : public TRandom_fast_
|
|
{
|
|
public:
|
|
TRandom_(u32 num) : TRandom_fast_(num){};
|
|
};
|
|
}
|
|
|
|
inline f32 JMAAbs(f32 input) { return __fabsf(input); }
|
|
|
|
inline f32 JMAAtan2Radian(f32 y, f32 x) { return JMath::atanTable_.atan2Radian(y, x); };
|
|
|
|
inline f32 JMASCosShort(s16 v) { return JMath::sincosTable_.cosShort(v); }
|
|
inline f32 JMASinShort(s16 v) { return JMath::sincosTable_.sinShort(v); }
|
|
|
|
inline f32 JMASCos(s16 v) { return JMASCosShort(v); }
|
|
inline f32 JMASSin(s16 v) { return JMASinShort(v); }
|
|
|
|
inline f32 JMAHermiteInterpolation(register f32 p1, register f32 p2, register f32 p3, register f32 p4, register f32 p5, register f32 p6,
|
|
register f32 p7)
|
|
{
|
|
register f32 ff25;
|
|
register f32 ff31;
|
|
register f32 ff30;
|
|
register f32 ff29;
|
|
register f32 ff28;
|
|
register f32 ff27;
|
|
register f32 ff26;
|
|
// clang-format off
|
|
__asm {
|
|
fsubs ff31, p1, p2
|
|
fsubs ff30, p5, p2
|
|
fdivs ff29, ff31, ff30
|
|
fmuls ff28,ff29,ff29
|
|
fadds ff25,ff29,ff29
|
|
fsubs ff27,ff28,ff29
|
|
fsubs ff30, p3, p6
|
|
fmsubs ff26,ff25,ff27,ff28
|
|
fmadds ff25,p4,ff27,p4
|
|
fmadds ff26,ff26,ff30,p3
|
|
fmadds ff25,p7,ff27,ff25
|
|
fmsubs ff25,ff29,p4,ff25
|
|
fnmsubs ff25,ff31,ff25,ff26
|
|
|
|
}
|
|
// clang-format on
|
|
return ff25;
|
|
}
|
|
|
|
inline f32 JMAFastSqrt(register f32 x)
|
|
{
|
|
register f32 recip;
|
|
|
|
if (x > 0.0f)
|
|
{
|
|
__asm { frsqrte recip, x }
|
|
return recip * x;
|
|
}
|
|
return x;
|
|
}
|
|
|
|
void JMAEulerToQuat(s16, s16, s16, Quaternion *);
|
|
void JMAQuatLerp(const Quaternion *, const Quaternion *, f32, Quaternion *);
|
|
void JMAVECScaleAdd(const Vec *, const Vec *, Vec *, f32);
|
|
void JMAVECLerp(const Vec *, const Vec *, Vec *, f32);
|
|
void JMAMTXApplyScale(const Mtx, Mtx, f32, f32, f32);
|
|
|
|
#endif // !JMATH_H
|