c_m3d, c_request OK. Work on Z2Audience (#1900)

7 files changed, 187 insertions, 9 deletions

diff --git a/include/JSystem/J3DU/J3DUD.h b/include/JSystem/J3DU/J3DUD.h
new file mode 100644
index 0000000000..f0f0e6e963
--- /dev/null
+++ b/include/JSystem/J3DU/J3DUD.h
@@ -0,0 +1,13 @@
+#ifndef J3DUD_H
+#define J3DUD_H
+
+#include "MSL_C/math.h"
+#include "dolphin/types.h"
+
+namespace J3DUD {
+    inline f32 JMAAbs(f32 x) {
+        return __fabsf(x);
+    }
+}
+
+#endif /* J3DUD_H */
\ No newline at end of file
diff --git a/include/JSystem/JAudio2/JAISeq.h b/include/JSystem/JAudio2/JAISeq.h
index 88dde573f4..9d295b8380 100644
--- a/include/JSystem/JAudio2/JAISeq.h
+++ b/include/JSystem/JAudio2/JAISeq.h
@@ -16,7 +16,7 @@ public:
         TInner() : mSeqData(NULL, 0) {}
 
         /* 0x000 */ JASTrack outputTrack;
-        /* 0x248 */ JASPoolAllocObject<JAISoundChild>* mSoundChild[32];
+        /* 0x248 */ JAISoundChild* mSoundChild[32];
         /* 0x2C8 */ JAITempoMgr mTempoMgr;
         /* 0x2D8 */ JASSoundParams mSoundParams;
         /* 0x2EC */ JAISeqData mSeqData;
diff --git a/include/JSystem/JAudio2/JASHeapCtrl.h b/include/JSystem/JAudio2/JASHeapCtrl.h
index 581135682f..ce17158c6f 100644
--- a/include/JSystem/JAudio2/JASHeapCtrl.h
+++ b/include/JSystem/JAudio2/JASHeapCtrl.h
@@ -61,15 +61,15 @@ public:
 };
 
 template <typename T>
-class JASPoolAllocObject : public T {
+class JASPoolAllocObject {
 public:
     static void* operator new(size_t n) {
         JASMemPool<T>* memPool = getMemPool();
-        return memPool->alloc(n);
+        return memPool->alloc(sizeof(T));
     }
     static void operator delete(void* ptr, size_t n) {
         JASMemPool<T>* memPool_ = getMemPool();
-        memPool_->free(ptr, n);
+        memPool_->free(ptr, sizeof(T));
     }
     static void newMemPool(int param_0) {
         JASMemPool<T>* memPool_ = getMemPool();
@@ -103,15 +103,15 @@ public:
 };
 
 template <typename T>
-class JASPoolAllocObject_MultiThreaded : public T {
+class JASPoolAllocObject_MultiThreaded {
 public:
     static void* operator new(size_t n) {
         JASMemPool_MultiThreaded<T>* memPool_ = getMemPool();
-        return memPool_->alloc(n);
+        return memPool_->alloc(sizeof(T));
     }
     static void operator delete(void* ptr, size_t n) {
         JASMemPool_MultiThreaded<T>* memPool_ = getMemPool();
-        memPool_->free(ptr, n);
+        memPool_->free(ptr, sizeof(T));
     }
 
     static void newMemPool(int n) {
diff --git a/include/JSystem/JGeometry.h b/include/JSystem/JGeometry.h
index eddf814984..4b1503f5e4 100644
--- a/include/JSystem/JGeometry.h
+++ b/include/JSystem/JGeometry.h
@@ -46,9 +46,9 @@ inline void setTVec3f(const f32* vec_a, f32* vec_b) {
     register f32 b_x;
 
     asm {
-        psq_l a_x, 0(v_a), 0, 0 /* qr0 */
+        psq_l a_x, 0(v_a), 0, 0
         lfs b_x, 8(v_a)
-        psq_st a_x, 0(v_b), 0, 0 /* qr0 */
+        psq_st a_x, 0(v_b), 0, 0
         stfs b_x, 8(v_b)
     };
 }
@@ -165,6 +165,21 @@ struct TVec3<f32> {
         scale(norm);
     }
 
+    void normalize(const TVec3<f32>& other) {
+        f32 sq = other.squared();
+        if (sq <= FLT_EPSILON * 32.0f) {
+            zero();
+            return;
+        }
+        f32 norm;
+        if (sq <= 0.0f) {
+            norm = sq;
+        } else {
+            norm = fsqrt_step(sq);
+        }
+        scale(norm, other);
+    }
+
     f32 length() const {
         return PSVECMag((Vec*)this);
     }
@@ -184,6 +199,22 @@ struct TVec3<f32> {
         };
     }
 
+    void scale(register f32 sc, const TVec3<f32>& other) {
+        register const f32* src = &other.x;
+        register f32 z;
+        register f32 x_y;
+        register f32* dst = &x;
+        register f32 zres;
+        asm {
+            psq_l    x_y, 0(src),  0, 0
+            psq_l    z,   8(src),  1, 0
+            ps_muls0 x_y,    x_y, sc
+            psq_st   x_y, 0(dst),  0, 0
+            ps_muls0 zres,       z, sc
+            psq_st   zres,  8(dst),  1, 0
+        };
+    }
+
     void negateInternal(TVec3<f32>* dst) {
         register f32* rdst = &dst->x;
         const register f32* src = &x;
@@ -232,6 +263,25 @@ struct TVec3<f32> {
         }
         scale(norm * len);
     }
+
+    f32 dot(const TVec3<f32>& other) const {
+        register const f32* pThis = &x;
+        register const f32* pOther = &other.x;
+        register f32 otherReg;
+        register f32 thisyz;
+        register f32 res;
+        register f32 thisxy;
+        asm {
+            psq_l thisyz, 4(pThis), 0, 0
+            psq_l otherReg, 4(pOther), 0, 0
+            ps_mul thisyz, thisyz, otherReg
+            psq_l thisxy, 0(pThis), 0, 0
+            psq_l otherReg, 0(pOther), 0, 0
+            ps_madd res, thisxy, otherReg, thisyz
+            ps_sum0 res, res, thisyz, thisyz
+        };
+        return res;
+    }
 };
 
 template <typename T>
@@ -272,6 +322,23 @@ struct TVec2 {
         return (x >= other.x) && (y >= other.y) ? true : false;
     }
 
+    f32 dot(const TVec2<T>& other) {
+        return x * other.x + y * other.y;
+    }
+
+    f32 squared() {
+        return dot(*this);
+    }
+
+    f32 length() {
+        f32 sqr = squared();
+        if (sqr <= 0.0f) {
+            return sqr;
+        }
+        sqr *= fsqrt_step(sqr);
+        return sqr;
+    }
+
     T x;
     T y;
 };
@@ -330,6 +397,19 @@ struct TBox2 : TBox<TVec2<T> > {
     void set(f32 x0, f32 y0, f32 x1, f32 y1) { i.set(x0, y0); f.set(x1, y1); }
 };
 
+template<typename T>
+struct TUtil {
+    static inline T clamp(T v, T min, T max) {
+        if (v < min) {
+            return min;
+        }
+        if (v > max) {
+            return max;
+        }
+        return v;
+    }
+};
+
 // clang-format on
 
 }  // namespace JGeometry
diff --git a/include/JSystem/JMath/JMATrigonometric.h b/include/JSystem/JMath/JMATrigonometric.h
index 2b09c3939d..6fe4c32169 100644
--- a/include/JSystem/JMath/JMATrigonometric.h
+++ b/include/JSystem/JMath/JMATrigonometric.h
@@ -16,6 +16,20 @@ struct TSinCosTable {
         }
         return table[(u16)(8192.0f * v) & 0x1fff].first;
     }
+
+    inline f32 sinDegree(f32 degree) {
+        if (degree < 0.0f) {
+            return -table[(u16)(-22.755556106567383f * degree) & 0x1fffU].first;
+        } 
+        return table[(u16)(22.755556106567383f * degree) & 0x1fffU].first;
+    }
+
+    inline f32 cosDegree(f32 degree) {
+        if (degree < 0.0f) {
+            degree = -degree;
+        } 
+        return table[(u16)(22.755556106567383f * degree) & 0x1fffU].second;
+    }
 };
 
 struct TAtanTable {
@@ -52,4 +66,12 @@ inline f32 JMASinLap(f32 v) {
     return JMath::sincosTable_.sinLap(v);
 }
 
+inline f32 JMASinDegree(f32 degree) {
+    return JMath::sincosTable_.sinDegree(degree);
+}
+
+inline f32 JMACosDegree(f32 degree) {
+    return JMath::sincosTable_.cosDegree(degree);
+}
+
 #endif /* JMATRIGONOMETRIC_H */
diff --git a/include/JSystem/JMath/JMath.h b/include/JSystem/JMath/JMath.h
index f22eeaf2e4..a6978cdb05 100644
--- a/include/JSystem/JMath/JMath.h
+++ b/include/JSystem/JMath/JMath.h
@@ -38,6 +38,29 @@ inline f32 fastReciprocal(f32 value) {
     return JMAFastReciprocal(value);
 }
 
+inline void gekko_ps_copy12(register f32* dst, register const f32* src) {
+    register f32 src0;
+    register f32 src1;
+    register f32 src2;
+    register f32 src3;
+    register f32 src4;
+    register f32 src5;
+    asm {
+        psq_l src0, 0(src), 0, 0
+        psq_l src1, 8(src), 0, 0
+        psq_l src2, 16(src), 0, 0
+        psq_l src3, 24(src), 0, 0
+        psq_l src4, 32(src), 0, 0
+        psq_l src5, 40(src), 0, 0
+        psq_st src0, 0(dst), 0, 0
+        psq_st src1, 8(dst), 0, 0
+        psq_st src2, 16(dst), 0, 0
+        psq_st src3, 24(dst), 0, 0
+        psq_st src4, 32(dst), 0, 0
+        psq_st src5, 40(dst), 0, 0
+    };
+}
+
 };  // namespace JMath
 
 #endif /* JMATH_H */
diff --git a/include/JSystem/TPosition3.hh b/include/JSystem/TPosition3.hh
new file mode 100644
index 0000000000..80138dd749
--- /dev/null
+++ b/include/JSystem/TPosition3.hh
@@ -0,0 +1,40 @@
+#ifndef TPOSITION3_H
+#define TPOSITION3_H
+
+#include "dolphin/mtx/mtx.h"
+#include "JSystem/JMath/JMath.h"
+
+namespace JGeometry {
+
+template <typename T>
+struct SMatrix34C {
+    T data[3][4];
+};
+
+template <>
+struct SMatrix34C<f32> {
+    f32 data[3][4];
+
+    void identity() { PSMTXIdentity(data); }
+
+    typedef f32 ArrType[4];
+    void set(const ArrType* src) { JMath::gekko_ps_copy12((f32*)data, (f32*)src); }
+
+    operator ArrType*() { return data; }
+    operator const ArrType*() const { return data; }
+};
+
+template <typename T>
+struct TMatrix34 : public T {};
+
+template <typename T>
+struct TRotation3 : public T {};
+
+template <typename T>
+struct TPosition3 : public T {};
+
+typedef TPosition3<TRotation3<TMatrix34<SMatrix34C<f32> > > > TPosition3f32;
+
+}  // namespace JGeometry
+
+#endif
\ No newline at end of file