Implement C_MTXReorder and C_MTXROMultVecArray (#84)

* Implement C_MTXReorder and C_MTXROMultVecArray

* Fixes

* Add necessary declarations to the header

* Move around and fix declarations

Author: dbalatoni13

include/dolphin/mtx.h

@@ -81,29 +81,48 @@ void C_MTXMultVec(const Mtx m, const Vec* src, Vec* dst);
 void C_MTXMultVecArray(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count);
 void C_MTXMultVecSR(const Mtx m, const Vec* src, Vec* dst);
 void C_MTXMultVecArraySR(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count);
+void C_MTXROMultVecArray(const ROMtx m, const Vec *srcBase, Vec *dstBase, u32 count);
 
 #ifdef GEKKO
 void PSMTXMultVec(const Mtx m, const Vec* src, Vec* dst);
 void PSMTXMultVecArray(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count);
 void PSMTXMultVecSR(const Mtx m, const Vec* src, Vec* dst);
 void PSMTXMultVecArraySR(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count);
+void PSMTXMultS16VecArray(const Mtx m, const S16Vec* srcBase, Vec* dstBase, u32 count);
+
+void PSMTXROMultVecArray(const ROMtx m, const Vec *srcBase, Vec *dstBase, u32 count);
+void PSMTXROSkin2VecArray(const ROMtx m0, const ROMtx m1, const f32* wtBase, const Vec* srcBase,
+                          Vec* dstBase, u32 count);
+void PSMTXROMultS16VecArray(const ROMtx m, const S16Vec* srcBase, Vec* dstBase, u32 count);
 #else
 #define PSMTXMultVec MTXMultVec
 #define PSMTXMultVecArray MTXMultVecArray
 #define PSMTXMultVecSR MTXMultVecSR
 #define PSMTXMultVecArraySR MTXMultVecArraySR
+#define PSMTXMultS16VecArray MTXMultS16VecArray
+
+#define PSMTXROMultVecArray MTXROMultVecArray
+#define PSMTXROSkin2VecArray MTXROSkin2VecArray
+#define PSMTXROMultS16VecArray MTXROMultS16VecArray
 #endif
 
 #ifdef MTX_USE_PS
 #define MTXMultVec PSMTXMultVec
 #define MTXMultVecArray PSMTXMultVecArray
 #define MTXMultVecSR PSMTXMultVecSR
 #define MTXMultVecArraySR PSMTXMultVecArraySR
+#define MTXMultS16VecArray PSMTXMultS16VecArray
+
+#define MTXROMultVecArray PSMTXROMultVecArray
+#define MTXROSkin2VecArray PSMTXROSkin2VecArray
+#define MTXROMultS16VecArray PSMTXROMultS16VecArray
 #else // MTX_USE_C
 #define MTXMultVec C_MTXMultVec
 #define MTXMultVecArray C_MTXMultVecArray
 #define MTXMultVecSR C_MTXMultVecSR
 #define MTXMultVecArraySR C_MTXMultVecArraySR
+
+#define MTXROMultVecArray C_MTXROMultVecArray
 #endif
 
 void C_MTXQuat(Mtx m, const Quaternion* q);
@@ -118,6 +137,8 @@ void C_MTXRotRad(Mtx m, char axis, f32 rad);
 void C_MTXRotTrig(Mtx m, char axis, f32 sinA, f32 cosA);
 void C_MTXRotAxisRad(Mtx m, const Vec* axis, f32 rad);
 
+void C_MTXReorder(const Mtx src, ROMtx dst);
+
 #ifdef GEKKO
 void PSMTXQuat(Mtx m, const Quaternion* q);
 void PSMTXReflect(Mtx m, const Vec* p, const Vec* n);
@@ -130,6 +151,8 @@ void PSMTXScaleApply(const Mtx src, Mtx dst, f32 xS, f32 yS, f32 zS);
 void PSMTXRotRad(Mtx m, char axis, f32 rad);
 void PSMTXRotTrig(Mtx m, char axis, f32 sinA, f32 cosA);
 void PSMTXRotAxisRad(Mtx m, const Vec* axis, f32 rad);
+
+void PSMTXReorder(const Mtx src, ROMtx dest);
 #else
 #define PSMTXQuat MTXQuat
 #define PSMTXReflect MTXReflect
@@ -143,6 +166,8 @@ void PSMTXRotAxisRad(Mtx m, const Vec* axis, f32 rad);
 
 #define PSMTXRotRad MTXRotRad
 #define PSMTXRotAxisRad MTXRotAxisRad
+
+#define PSMTXReorder MTXReorder
 #endif
 
 #ifdef MTX_USE_PS
@@ -159,6 +184,8 @@ void PSMTXRotAxisRad(Mtx m, const Vec* axis, f32 rad);
 #define MTXRotDeg(m, axis, deg) PSMTXRotRad(m, axis, MTXDegToRad(deg))
 #define MTXRotAxisDeg(m, axis, deg) PSMTXRotAxisRad(m, axis, MTXDegToRad(deg))
 
+#define MTXReorder PSMTXReorder
+
 #else // MTX_USE_C
 #define MTXQuat C_MTXQuat
 #define MTXReflect C_MTXReflect
@@ -173,6 +200,8 @@ void PSMTXRotAxisRad(Mtx m, const Vec* axis, f32 rad);
 #define MTXRotDeg(m, axis, deg) C_MTXRotRad(m, axis, MTXDegToRad(deg))
 #define MTXRotAxisDeg(m, axis, deg) C_MTXRotAxisRad(m, axis, MTXDegToRad(deg))
 
+#define MTXReorder C_MTXReorder
+
 #endif
 
 void C_MTXLookAt(Mtx m, const Point3d* camPos, const Vec* camUp, const Point3d* target);
@@ -336,15 +365,6 @@ void PSQUATInverse(const Quaternion* src, Quaternion* inv);
 #define QUATSquad C_QUATSquad
 #define QUATCompA C_QUATCompA
 
-#ifdef GEKKO
-void PSMTXReorder(const Mtx src, ROMtx dest);
-void PSMTXROMultVecArray(const ROMtx m, const Vec* srcBase, Vec* dstBase, u32 count);
-void PSMTXROSkin2VecArray(const ROMtx m0, const ROMtx m1, const f32* wtBase, const Vec* srcBase,
-                          Vec* dstBase, u32 count);
-void PSMTXMultS16VecArray(const Mtx m, const S16Vec* srcBase, Vec* dstBase, u32 count);
-void PSMTXROMultS16VecArray(const ROMtx m, const S16Vec* srcBase, Vec* dstBase, u32 count);
-#endif
-
 void MTXInitStack(MtxStack* sPtr, u32 numMtx);
 MtxPtr MTXPush(MtxStack* sPtr, const Mtx m);
 MtxPtr MTXPushFwd(MtxStack* sPtr, const Mtx m);

lib/dolphin/mtx/mtx.c

@@ -505,6 +505,17 @@ void C_MTXRotAxisRad(Mtx m, const Vec* axis, f32 rad) {
   m[2][3] = 0;
 }
 
+void C_MTXReorder(const Mtx src, ROMtx dst)
+{
+  u32 i;
+  u32 j;
+  for (i = 0; i < 3; i++) {
+    for (j = 0; j < 4; j++) {
+      dst[j][i] = src[i][j];
+    }
+  }
+}
+
 void C_MTXLookAt(Mtx m, const Point3d* camPos, const Vec* camUp, const Point3d* target) {
   Vec vLook;
   Vec vRight;

lib/dolphin/mtx/mtxvec.c

@@ -73,3 +73,15 @@ void C_MTXMultVecArraySR(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 coun
   }
 }
 
+void C_MTXROMultVecArray(const ROMtx m, const Vec *srcBase, Vec *dstBase, u32 count)
+{
+  u32 i;
+  for (i = 0; i < count; i++) {
+    const Vec* src = &srcBase[i];
+    Vec* dst = &dstBase[i];
+
+    dst->x = m[0][0] * src->x + m[0][1] * src->y + m[0][2] * src->z;
+    dst->y = m[1][0] * src->x + m[1][1] * src->y + m[1][2] * src->z;
+    dst->z = m[2][0] * src->x + m[2][1] * src->y + m[2][2] * src->z;
+  }
+}