TP-PC-Port/include/JSystem/JMath/JMath.h

#ifndef JMATH_H
#define JMATH_H

#include "dolphin/mtx.h"

#include <cstring>
#include <math.h>

void JMAMTXApplyScale(const Mtx, Mtx, f32, f32, f32);
void JMAEulerToQuat(s16 param_0, s16 param_1, s16 param_2, Quaternion* param_3);
void JMAQuatLerp(const Quaternion*, const Quaternion*, f32, Quaternion*);
void JMAFastVECNormalize(const Vec* src, Vec* dst);
void JMAVECScaleAdd(const Vec* vec1, const Vec* vec2, Vec* dst,
                    f32 scale);

inline int JMAAbs(int value) {
    return abs(value);
}

inline f32 JMAAbs(f32 x) {
    return fabsf(x);
}

inline f32 JMAFastReciprocal(f32 value) {
    return 1.0f / value;
}

inline float __frsqrtes(double f) {
    return 1.0f / sqrt(f);
}

inline f32 JMAFastSqrt(const f32 input) {
    if (input > 0.0f) {
        return sqrt(input);
    } else {
        return input;
    }
}

// temp until i can understand how this func actually works
inline f32 JMAHermiteInterpolation(f32 p1, f32 p2, f32 p3,
                                   f32 p4, f32 p5, f32 p6,
                                   f32 p7) {
    f32 ff25;
    f32 ff31;
    f32 ff30;
    f32 ff29;
    f32 ff28;
    f32 ff27;
    f32 ff26;

    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);

    return ff25;
}

namespace JMath {

template <typename T>
inline T fastSqrt(T value) {
    return JMAFastSqrt(value);
}

inline f32 fastReciprocal(f32 value) {
    return JMAFastReciprocal(value);
}

inline void fastVECNormalize(const Vec* src, Vec* dst) {
    return JMAFastVECNormalize(src, dst);
}

inline void gekko_ps_copy3(void* dst, const void* src) {
    memcpy(dst, src, sizeof(float) * 3);
}

inline void gekko_ps_copy6(void* dst, const void* src) {
    memcpy(dst, src, sizeof(float) * 6);

}

inline void gekko_ps_copy12(void* dst, const void* src) {
    memcpy(dst, src, sizeof(float) * 12);
}

inline void gekko_ps_copy16(void* dst, const void* src) {
    memcpy(dst, src, sizeof(float) * 16);
}

};  // namespace JMath

namespace JMathInlineVEC {
    inline void C_VECAdd(const Vec* a, const Vec* b, Vec* ab) {
        ab->x = a->x + b->x;
        ab->y = a->y + b->y;
        ab->z = a->z + b->z;
    }

    inline void C_VECSubtract(const Vec* a, const Vec* b, Vec* ab) {
        ab->x = a->x - b->x;
        ab->y = a->y - b->y;
        ab->z = a->z - b->z;
    }

    inline f32 C_VECSquareMag(const Vec* v) {
        return v->x * v->x + v->y * v->y + v->z * v->z;
    }

    inline f32 C_VECDotProduct(const Vec *a, const Vec *b) {
        return a->x * b->x + a->y * b->y + a->z * b->z;
    }
};

template<typename T>
inline T JMAMax(T param_0, T param_1) {
    T ret;
    if (param_0 > param_1) {
        ret = param_0;
    } else {
        ret = param_1;
    }
    return ret;
}

#endif /* JMATH_H */