mtx from dolsdk2004 (#748)

5 files changed, 2058 insertions, 0 deletions

diff --git a/src/dolphin/mtx/mtx.c b/src/dolphin/mtx/mtx.c
new file mode 100644
index 00000000..efc6f0c5
--- /dev/null
+++ b/src/dolphin/mtx/mtx.c
@@ -0,0 +1,1165 @@
+
+#include <dolphin/mtx/mtx.h>
+#include <dolphin/mtx/vec.h>
+#include <dolphin/os/OS.h>
+#include <math.h>
+
+static f32 Unit01[2] = {
+    0.0f,
+    1.0f
+};
+
+void C_MTXIdentity(Mtx m) {
+    ASSERTMSGLINE(189, m, "MtxIdentity():  NULL Mtx 'm' ");
+    m[0][0] = 1;
+    m[0][1] = 0;
+    m[0][2] = 0;
+    m[0][3] = 0;
+    m[1][0] = 0;
+    m[1][1] = 1;
+    m[1][2] = 0;
+    m[1][3] = 0;
+    m[2][0] = 0;
+    m[2][1] = 0;
+    m[2][2] = 1;
+    m[2][3] = 0;
+}
+
+#ifdef __MWERKS__
+void PSMTXIdentity(register Mtx m) {
+    register f32 c_zero = 0.0f;
+    register f32 c_one = 1.0f;
+    register f32 c_01;
+    register f32 c_10;
+
+    asm {
+        psq_st c_zero, 8(m), 0, 0
+        ps_merge01 c_01, c_zero, c_one
+        psq_st c_zero, 24(m), 0, 0
+        ps_merge10 c_10, c_one, c_zero
+        psq_st c_zero, 32(m), 0, 0
+        psq_st c_01, 16(m), 0, 0
+        psq_st c_10, 0(m), 0, 0
+        psq_st c_10, 40(m), 0, 0
+    }
+}
+#endif
+
+void C_MTXCopy(const Mtx src, Mtx dst) {
+    ASSERTMSGLINE(250, src, "MTXCopy():  NULL MtxPtr 'src' ");
+    ASSERTMSGLINE(251, dst, "MTXCopy():  NULL MtxPtr 'dst' ");
+    if (src != dst) {
+        dst[0][0] = src[0][0];
+        dst[0][1] = src[0][1];
+        dst[0][2] = src[0][2];
+        dst[0][3] = src[0][3];
+        dst[1][0] = src[1][0];
+        dst[1][1] = src[1][1];
+        dst[1][2] = src[1][2];
+        dst[1][3] = src[1][3];
+        dst[2][0] = src[2][0];
+        dst[2][1] = src[2][1];
+        dst[2][2] = src[2][2];
+        dst[2][3] = src[2][3];
+    }
+}
+
+#ifdef __MWERKS__
+asm void PSMTXCopy(const register Mtx src, register Mtx dst) {
+    psq_l f0, 0(src), 0, 0
+    psq_st f0, 0(dst), 0, 0
+    psq_l f1, 8(src), 0, 0
+    psq_st f1, 8(dst), 0, 0
+    psq_l f2, 16(src), 0, 0
+    psq_st f2, 16(dst), 0, 0
+    psq_l f3, 24(src), 0, 0
+    psq_st f3, 24(dst), 0, 0
+    psq_l f4, 32(src), 0, 0
+    psq_st f4, 32(dst), 0, 0
+    psq_l f5, 40(src), 0, 0
+    psq_st f5, 40(dst), 0, 0
+}
+#endif
+
+void C_MTXConcat(const Mtx a, const Mtx b, Mtx ab) {
+    Mtx mTmp;
+    MtxP m;
+
+    ASSERTMSGLINE(324, a, "MTXConcat():  NULL MtxPtr 'a'  ");
+    ASSERTMSGLINE(325, b, "MTXConcat():  NULL MtxPtr 'b'  ");
+    ASSERTMSGLINE(326, ab, "MTXConcat():  NULL MtxPtr 'ab' ");
+
+    if (ab == a || ab == b) {
+        m = mTmp;
+    } else {
+        m = ab;
+    }
+
+    m[0][0] =       0 +  a[0][2] * b[2][0] + ((a[0][0] * b[0][0]) + (a[0][1] * b[1][0]));
+    m[0][1] =       0 +  a[0][2] * b[2][1] + ((a[0][0] * b[0][1]) + (a[0][1] * b[1][1]));
+    m[0][2] =       0 +  a[0][2] * b[2][2] + ((a[0][0] * b[0][2]) + (a[0][1] * b[1][2]));
+    m[0][3] = a[0][3] + (a[0][2] * b[2][3] +  (a[0][0] * b[0][3]  + (a[0][1] * b[1][3])));
+
+    m[1][0] =       0 +  a[1][2] * b[2][0] + ((a[1][0] * b[0][0]) + (a[1][1] * b[1][0]));
+    m[1][1] =       0 +  a[1][2] * b[2][1] + ((a[1][0] * b[0][1]) + (a[1][1] * b[1][1]));
+    m[1][2] =       0 +  a[1][2] * b[2][2] + ((a[1][0] * b[0][2]) + (a[1][1] * b[1][2]));
+    m[1][3] = a[1][3] + (a[1][2] * b[2][3] +  (a[1][0] * b[0][3]  + (a[1][1] * b[1][3])));
+
+    m[2][0] =       0 +  a[2][2] * b[2][0] + ((a[2][0] * b[0][0]) + (a[2][1] * b[1][0]));
+    m[2][1] =       0 +  a[2][2] * b[2][1] + ((a[2][0] * b[0][1]) + (a[2][1] * b[1][1]));
+    m[2][2] =       0 +  a[2][2] * b[2][2] + ((a[2][0] * b[0][2]) + (a[2][1] * b[1][2]));
+    m[2][3] = a[2][3] + (a[2][2] * b[2][3] +  (a[2][0] * b[0][3]  + (a[2][1] * b[1][3])));
+
+    if (m == mTmp) {
+        C_MTXCopy(mTmp, ab);
+    }
+}
+
+#ifdef __MWERKS__
+asm void PSMTXConcat(const register Mtx a, const register Mtx b, register Mtx ab) {
+    nofralloc
+    stwu r1, -64(r1)
+    psq_l f0, 0(a), 0, 0
+    stfd f14, 8(r1)
+    psq_l f6, 0(b), 0, 0
+    lis r6, Unit01@ha
+    psq_l f7, 8(b), 0, 0
+    stfd f15, 16(r1)
+    addi r6, r6, Unit01@l
+    stfd f31, 40(r1)
+    psq_l f8, 16(b), 0, 0
+    ps_muls0 f12, f6, f0
+    psq_l f2, 16(a), 0, 0
+    ps_muls0 f13, f7, f0
+    psq_l f31, 0(r6), 0, 0
+    ps_muls0 f14, f6, f2
+    psq_l f9, 24(b), 0, 0
+    ps_muls0 f15, f7, f2
+    psq_l f1, 8(a), 0, 0
+    ps_madds1 f12, f8, f0, f12
+    psq_l f3, 24(a), 0, 0
+    ps_madds1 f14, f8, f2, f14
+    psq_l f10, 32(b), 0, 0
+    ps_madds1 f13, f9, f0, f13
+    psq_l f11, 40(b), 0, 0
+    ps_madds1 f15, f9, f2, f15
+    psq_l f4, 32(a), 0, 0
+    psq_l f5, 40(a), 0, 0
+    ps_madds0 f12, f10, f1, f12
+    ps_madds0 f13, f11, f1, f13
+    ps_madds0 f14, f10, f3, f14
+    ps_madds0 f15, f11, f3, f15
+    psq_st f12, 0(ab), 0, 0
+    ps_muls0 f2, f6, f4
+    ps_madds1 f13, f31, f1, f13
+    ps_muls0 f0, f7, f4
+    psq_st f14, 16(ab), 0, 0
+    ps_madds1 f15, f31, f3, f15
+    psq_st f13, 8(ab), 0, 0
+    ps_madds1 f2, f8, f4, f2
+    ps_madds1 f0, f9, f4, f0
+    ps_madds0 f2, f10, f5, f2
+    lfd f14, 8(r1)
+    psq_st f15, 24(ab), 0, 0
+    ps_madds0 f0, f11, f5, f0
+    psq_st f2, 32(ab), 0, 0
+    ps_madds1 f0, f31, f5, f0
+    lfd f15, 16(r1)
+    psq_st f0, 40(ab), 0, 0
+    lfd f31, 40(r1)
+    addi r1, r1, 64
+    blr
+}
+#endif
+
+void C_MTXConcatArray(const Mtx a, const Mtx* srcBase, Mtx* dstBase, u32 count) {
+    u32 i;
+
+    ASSERTMSGLINE(580, a != 0, "MTXConcatArray(): NULL MtxPtr 'a' ");
+    ASSERTMSGLINE(581, srcBase != 0, "MTXConcatArray(): NULL MtxPtr 'srcBase' ");
+    ASSERTMSGLINE(582, dstBase != 0, "MTXConcatArray(): NULL MtxPtr 'dstBase' ");
+    ASSERTMSGLINE(583, count > 1, "MTXConcatArray(): count must be greater than 1.");
+
+    for (i = 0; i < count; i++) {
+        C_MTXConcat(a, *srcBase, *dstBase);
+        srcBase++;
+        dstBase++;
+    }
+}
+
+#ifdef __MWERKS__
+#if DEBUG
+#pragma push
+#pragma optimization_level 1
+// This function will not compile at optimization level 0
+#endif
+void PSMTXConcatArray(const register Mtx a, const register Mtx* srcBase, register Mtx* dstBase, register u32 count) {
+    register f32 va0, va1, va2, va3, va4, va5;
+    register f32 vb0, vb1, vb2, vb3, vb4, vb5;
+    register f32 vd0, vd1, vd2, vd3, vd4, vd5;
+    register f32 u01;
+    register f32* u01Ptr = Unit01;
+
+    asm {
+        psq_l va0, 0(a), 0, 0;
+        psq_l va1, 8(a), 0, 0;
+        psq_l va2, 16(a), 0, 0;
+        psq_l va3, 24(a), 0, 0;
+        subi count, count, 1;
+        psq_l va4, 32(a), 0, 0;
+        psq_l va5, 40(a), 0, 0;
+        mtctr count;
+        psq_l u01, 0(u01Ptr), 0, 0;
+        psq_l vb0, 0(srcBase), 0, 0;
+        psq_l vb2, 16(srcBase), 0, 0;
+        ps_muls0 vd0, vb0, va0;
+        ps_muls0 vd2, vb0, va2;
+        ps_muls0 vd4, vb0, va4;
+        psq_l vb4, 32(srcBase), 0, 0;
+        ps_madds1 vd0, vb2, va0, vd0;
+        ps_madds1 vd2, vb2, va2, vd2;
+        ps_madds1 vd4, vb2, va4, vd4;
+        psq_l vb1, 8(srcBase), 0, 0;
+        ps_madds0 vd0, vb4, va1, vd0;
+        ps_madds0 vd2, vb4, va3, vd2;
+        ps_madds0 vd4, vb4, va5, vd4;
+        psq_l vb3, 24(srcBase), 0, 0;
+        psq_st vd0, 0(dstBase), 0, 0;
+        ps_muls0 vd1, vb1, va0;
+        ps_muls0 vd3, vb1, va2;
+        ps_muls0 vd5, vb1, va4;
+        psq_l vb5, 40(srcBase), 0, 0;
+        psq_st vd2, 16(dstBase), 0, 0;
+        ps_madds1 vd1, vb3, va0, vd1;
+        ps_madds1 vd3, vb3, va2, vd3;
+        ps_madds1 vd5, vb3, va4, vd5;
+    _loop:
+        addi srcBase, srcBase, sizeof(Mtx);
+        ps_madds0 vd1, vb5, va1, vd1;
+        ps_madds0 vd3, vb5, va3, vd3;
+        ps_madds0 vd5, vb5, va5, vd5;
+        psq_l vb0, 0(srcBase), 0, 0;
+        psq_st vd4, 32(dstBase), 0, 0;
+        ps_madd vd1, u01, va1, vd1;
+        ps_madd vd3, u01, va3, vd3;
+        ps_madd vd5, u01, va5, vd5;
+        psq_l vb2, 16(srcBase), 0, 0;
+        psq_st vd1, 8(dstBase), 0, 0;
+        ps_muls0 vd0, vb0, va0;
+        ps_muls0 vd2, vb0, va2;
+        ps_muls0 vd4, vb0, va4;
+        psq_l vb4, 32(srcBase), 0, 0;
+        psq_st vd3, 24(dstBase), 0, 0;
+        ps_madds1 vd0, vb2, va0, vd0;
+        ps_madds1 vd2, vb2, va2, vd2;
+        ps_madds1 vd4, vb2, va4, vd4;
+        psq_l vb1, 8(srcBase), 0, 0;
+        psq_st vd5, 40(dstBase), 0, 0;
+        addi dstBase, dstBase, sizeof(Mtx);
+        ps_madds0 vd0, vb4, va1, vd0;
+        ps_madds0 vd2, vb4, va3, vd2;
+        ps_madds0 vd4, vb4, va5, vd4;
+        psq_l vb3, 24(srcBase), 0, 0;
+        psq_st vd0, 0(dstBase), 0, 0;
+        ps_muls0 vd1, vb1, va0;
+        ps_muls0 vd3, vb1, va2;
+        ps_muls0 vd5, vb1, va4;
+        psq_l vb5, 40(srcBase), 0, 0;
+        psq_st vd2, 16(dstBase), 0, 0;
+        ps_madds1 vd1, vb3, va0, vd1;
+        ps_madds1 vd3, vb3, va2, vd3;
+        ps_madds1 vd5, vb3, va4, vd5;
+        bdnz _loop;
+        psq_st vd4, 32(dstBase), 0, 0;
+        ps_madds0 vd1, vb5, va1, vd1;
+        ps_madds0 vd3, vb5, va3, vd3;
+        ps_madds0 vd5, vb5, va5, vd5;
+        ps_madd vd1, u01, va1, vd1;
+        ps_madd vd3, u01, va3, vd3;
+        ps_madd vd5, u01, va5, vd5;
+        psq_st vd1, 8(dstBase), 0, 0;
+        psq_st vd3, 24(dstBase), 0, 0;
+        psq_st vd5, 40(dstBase), 0, 0;
+    }
+}
+#if DEBUG
+#pragma pop
+#endif
+#endif
+
+void C_MTXTranspose(const Mtx src, Mtx xPose) {
+    Mtx mTmp;
+    MtxP m;
+
+    ASSERTMSGLINE(851, src, "MTXTranspose():  NULL MtxPtr 'src' ");
+    ASSERTMSGLINE(852, xPose, "MTXTranspose():  NULL MtxPtr 'xPose' ");
+
+    if (src == xPose) {
+        m = mTmp;
+    } else {
+        m = xPose;
+    }
+
+    m[0][0] = src[0][0];
+    m[0][1] = src[1][0];
+    m[0][2] = src[2][0];
+    m[0][3] = 0;
+    m[1][0] = src[0][1];
+    m[1][1] = src[1][1];
+    m[1][2] = src[2][1];
+    m[1][3] = 0;
+    m[2][0] = src[0][2];
+    m[2][1] = src[1][2];
+    m[2][2] = src[2][2];
+    m[2][3] = 0;
+    if (m == mTmp) {
+        C_MTXCopy(mTmp, xPose);
+    }
+}
+
+#ifdef __MWERKS__
+void PSMTXTranspose(const register Mtx src, register Mtx xPose) {
+    register f32 c_zero = 0;
+    register f32 row0a;
+    register f32 row1a;
+    register f32 row0b;
+    register f32 row1b;
+    register f32 trns0;
+    register f32 trns1;
+    register f32 trns2;
+
+    asm {
+        psq_l row0a, 0(src), 0, 0
+    }
+    xPose[2][3] = c_zero;
+    asm {
+        psq_l row1a, 16(src), 0, 0
+        ps_merge00 trns0, row0a, row1a
+        psq_l row0b, 8(src), 1, 0
+        ps_merge11 trns1, row0a, row1a
+        psq_l row1b, 24(src), 1, 0
+        psq_st trns0, 0(xPose), 0, 0
+        psq_l row0a, 32(src), 0, 0
+        ps_merge00 trns2, row0b, row1b
+        psq_st trns1, 16(xPose), 0, 0
+        ps_merge00 trns0, row0a, c_zero
+        psq_st trns2, 32(xPose), 0, 0
+        ps_merge10 trns1, row0a, c_zero
+        psq_st trns0, 8(xPose), 0, 0
+    }
+    row0b = src[2][2];
+    asm {
+        psq_st trns1, 24(xPose), 0, 0
+    }
+    xPose[2][2] = row0b;
+}
+#endif
+
+u32 C_MTXInverse(const Mtx src, Mtx inv) {
+    Mtx mTmp;
+    MtxP m;
+    f32 det;
+
+    ASSERTMSGLINE(950, src, "MTXInverse():  NULL MtxPtr 'src' ");
+    ASSERTMSGLINE(951, inv, "MTXInverse():  NULL MtxPtr 'inv' ");
+
+    if (src == inv) {
+        m = mTmp;
+    } else {
+        m = inv;
+    }
+    det = ((((src[2][1] * (src[0][2] * src[1][0]))
+          + ((src[2][2] * (src[0][0] * src[1][1]))
+           + (src[2][0] * (src[0][1] * src[1][2]))))
+           - (src[0][2] * (src[2][0] * src[1][1])))
+           - (src[2][2] * (src[1][0] * src[0][1])))
+           - (src[1][2] * (src[0][0] * src[2][1]));
+    if (0 == det) {
+        return 0;
+    }
+    det = 1 / det;
+    m[0][0] = (det * +((src[1][1] * src[2][2]) - (src[2][1] * src[1][2])));
+    m[0][1] = (det * -((src[0][1] * src[2][2]) - (src[2][1] * src[0][2])));
+    m[0][2] = (det * +((src[0][1] * src[1][2]) - (src[1][1] * src[0][2])));
+
+    m[1][0] = (det * -((src[1][0] * src[2][2]) - (src[2][0] * src[1][2])));
+    m[1][1] = (det * +((src[0][0] * src[2][2]) - (src[2][0] * src[0][2])));
+    m[1][2] = (det * -((src[0][0] * src[1][2]) - (src[1][0] * src[0][2])));
+
+    m[2][0] = (det * +((src[1][0] * src[2][1]) - (src[2][0] * src[1][1])));
+    m[2][1] = (det * -((src[0][0] * src[2][1]) - (src[2][0] * src[0][1])));
+    m[2][2] = (det * +((src[0][0] * src[1][1]) - (src[1][0] * src[0][1])));
+
+    m[0][3] = ((-m[0][0] * src[0][3]) - (m[0][1] * src[1][3])) - (m[0][2] * src[2][3]);
+    m[1][3] = ((-m[1][0] * src[0][3]) - (m[1][1] * src[1][3])) - (m[1][2] * src[2][3]);
+    m[2][3] = ((-m[2][0] * src[0][3]) - (m[2][1] * src[1][3])) - (m[2][2] * src[2][3]);
+
+    if (m == mTmp) {
+        C_MTXCopy(mTmp, inv);
+    }
+    return 1;
+}
+
+#ifdef __MWERKS__
+asm u32 PSMTXInverse(const register Mtx src, register Mtx inv) {
+    psq_l f0, 0(src), 1, 0
+    psq_l f1, 4(src), 0, 0
+    psq_l f2, 16(src), 1, 0
+    ps_merge10 f6, f1, f0
+    psq_l f3, 20(src), 0, 0
+    psq_l f4, 32(src), 1, 0
+    ps_merge10 f7, f3, f2
+    psq_l f5, 36(src), 0, 0
+    ps_mul f11, f3, f6
+    ps_mul f13, f5, f7
+    ps_merge10 f8, f5, f4
+    ps_msub f11, f1, f7, f11
+    ps_mul f12, f1, f8
+    ps_msub f13, f3, f8, f13
+    ps_mul f10, f3, f4
+    ps_msub f12, f5, f6, f12
+    ps_mul f9, f0, f5
+    ps_mul f8, f1, f2
+    ps_sub f6, f6, f6
+    ps_msub f10, f2, f5, f10
+    ps_mul f7, f0, f13
+    ps_msub f9, f1, f4, f9
+    ps_madd f7, f2, f12, f7
+    ps_msub f8, f0, f3, f8
+    ps_madd f7, f4, f11, f7
+    ps_cmpo0 cr0, f7, f6
+    bne skip_return
+    li r3, 0
+    blr
+skip_return:
+    fres f0, f7
+    ps_add f6, f0, f0
+    ps_mul f5, f0, f0
+    ps_nmsub f0, f7, f5, f6
+    lfs f1, 12(src)
+    ps_muls0 f13, f13, f0
+    lfs f2, 28(src)
+    ps_muls0 f12, f12, f0
+    lfs f3, 44(src)
+    ps_muls0 f11, f11, f0
+    ps_merge00 f5, f13, f12
+    ps_muls0 f10, f10, f0
+    ps_merge11 f4, f13, f12
+    ps_muls0 f9, f9, f0
+    psq_st f5, 0(inv), 0, 0
+    ps_mul f6, f13, f1
+    psq_st f4, 16(inv), 0, 0
+    ps_muls0 f8, f8, f0
+    ps_madd f6, f12, f2, f6
+    psq_st f10, 32(inv), 1, 0
+    ps_nmadd f6, f11, f3, f6
+    psq_st f9, 36(inv), 1, 0
+    ps_mul f7, f10, f1
+    ps_merge00 f5, f11, f6
+    psq_st f8, 40(inv), 1, 0
+    ps_merge11 f4, f11, f6
+    psq_st f5, 8(inv), 0, 0
+    ps_madd f7, f9, f2, f7
+    psq_st f4, 24(inv), 0, 0
+    ps_nmadd f7, f8, f3, f7
+    li r3, 1
+    psq_st f7, 44(inv), 1, 0
+}
+#endif
+
+u32 C_MTXInvXpose(const Mtx src, Mtx invX) {
+    Mtx mTmp;
+    MtxP m;
+    f32 det;
+
+    ASSERTMSGLINE(1185, src, "MTXInvXpose(): NULL MtxPtr 'src' ");
+    ASSERTMSGLINE(1186, invX, "MTXInvXpose(): NULL MtxPtr 'invX' ");
+
+    if (src == invX) {
+        m = mTmp;
+    } else {
+        m = invX;
+    }
+    det = ((((src[2][1] * (src[0][2] * src[1][0]))
+          + ((src[2][2] * (src[0][0] * src[1][1]))
+          +  (src[2][0] * (src[0][1] * src[1][2]))))
+          -  (src[0][2] * (src[2][0] * src[1][1])))
+          -  (src[2][2] * (src[1][0] * src[0][1])))
+          -  (src[1][2] * (src[0][0] * src[2][1]));
+    if (0 == det) {
+        return 0;
+    }
+    det = 1 / det;
+    m[0][0] = (det * +((src[1][1] * src[2][2]) - (src[2][1] * src[1][2])));
+    m[0][1] = (det * -((src[1][0] * src[2][2]) - (src[2][0] * src[1][2])));
+    m[0][2] = (det * +((src[1][0] * src[2][1]) - (src[2][0] * src[1][1])));
+
+    m[1][0] = (det * -((src[0][1] * src[2][2]) - (src[2][1] * src[0][2])));
+    m[1][1] = (det * +((src[0][0] * src[2][2]) - (src[2][0] * src[0][2])));
+    m[1][2] = (det * -((src[0][0] * src[2][1]) - (src[2][0] * src[0][1])));
+
+    m[2][0] = (det * +((src[0][1] * src[1][2]) - (src[1][1] * src[0][2])));
+    m[2][1] = (det * -((src[0][0] * src[1][2]) - (src[1][0] * src[0][2])));
+    m[2][2] = (det * +((src[0][0] * src[1][1]) - (src[1][0] * src[0][1])));
+
+    m[0][3] = 0;
+    m[1][3] = 0;
+    m[2][3] = 0;
+
+    if (m == mTmp) {
+        C_MTXCopy(mTmp, invX);
+    }
+    return 1;
+}
+
+#ifdef __MWERKS__
+asm u32 PSMTXInvXpose(const register Mtx src, register Mtx invX) {
+	psq_l f0, 0(src), 1, 0
+	psq_l f1, 4(src), 0, 0
+	psq_l f2, 16(src), 1, 0
+	ps_merge10 f6, f1, f0
+	psq_l f3, 20(src), 0, 0
+	psq_l f4, 32(src), 1, 0
+	ps_merge10 f7, f3, f2
+	psq_l f5, 36(src), 0, 0
+	ps_mul f11, f3, f6
+	ps_merge10 f8, f5, f4
+	ps_mul f13, f5, f7
+	ps_msub f11, f1, f7, f11
+	ps_mul f12, f1, f8
+	ps_msub f13, f3, f8, f13
+	ps_msub f12, f5, f6, f12
+	ps_mul f10, f3, f4
+	ps_mul f9, f0, f5
+	ps_mul f8, f1, f2
+	ps_msub f10, f2, f5, f10
+	ps_msub f9, f1, f4, f9
+	ps_msub f8, f0, f3, f8
+	ps_mul f7, f0, f13
+	ps_sub f1, f1, f1
+	ps_madd f7, f2, f12, f7
+	ps_madd f7, f4, f11, f7
+	ps_cmpo0 cr0, f7, f1
+	bne skip_return
+	li r3, 0
+	blr
+skip_return:
+	fres f0, f7
+	psq_st f1, 12(invX), 1, 0
+	ps_add f6, f0, f0
+	ps_mul f5, f0, f0
+	psq_st f1, 28(invX), 1, 0
+	ps_nmsub f0, f7, f5, f6
+	psq_st f1, 44(invX), 1, 0
+	ps_muls0 f13, f13, f0
+	ps_muls0 f12, f12, f0
+	ps_muls0 f11, f11, f0
+    psq_st f13, 0(invX), 0, 0
+	psq_st f12, 16(invX), 0, 0
+	ps_muls0 f10, f10, f0
+    ps_muls0 f9, f9, f0
+	psq_st f11, 32(invX), 0, 0
+	psq_st f10, 8(invX), 1, 0
+	ps_muls0 f8, f8, f0
+	li r3, 1
+    psq_st f9, 24(invX), 1, 0
+	psq_st f8, 40(invX), 1, 0
+}
+#endif
+
+void C_MTXRotRad(Mtx m, u8 axis, f32 rad) {
+    f32 sinA;
+    f32 cosA;
+
+    ASSERTMSGLINE(1447, m, "MTXRotRad():  NULL MtxPtr 'm' ");
+    sinA = sinf(rad);
+    cosA = cosf(rad);
+    C_MTXRotTrig(m, axis, sinA, cosA);
+}
+
+#ifdef __MWERKS__
+void PSMTXRotRad(Mtx m, u8 axis, f32 rad) {
+    f32 sinA, cosA;
+    sinA = sinf(rad);
+    cosA = cosf(rad);
+    PSMTXRotTrig(m, axis, sinA, cosA);
+}
+#endif
+
+void C_MTXRotTrig(Mtx m, u8 axis, f32 sinA, f32 cosA) {
+    ASSERTMSGLINE(1502, m, "MTXRotTrig():  NULL MtxPtr 'm' ");
+    switch(axis) {
+        case 'x':
+        case 'X':
+            m[0][0] = 1;
+            m[0][1] = 0;
+            m[0][2] = 0;
+            m[0][3] = 0;
+            m[1][0] = 0;
+            m[1][1] = cosA;
+            m[1][2] = -sinA;
+            m[1][3] = 0;
+            m[2][0] = 0;
+            m[2][1] = sinA;
+            m[2][2] = cosA;
+            m[2][3] = 0;
+            break;
+        case 'y':
+        case 'Y':
+            m[0][0] = cosA;
+            m[0][1] = 0;
+            m[0][2] = sinA;
+            m[0][3] = 0;
+            m[1][0] = 0;
+            m[1][1] = 1;
+            m[1][2] = 0;
+            m[1][3] = 0;
+            m[2][0] = -sinA;
+            m[2][1] = 0;
+            m[2][2] = cosA;
+            m[2][3] = 0;
+            break;
+        case 'z':
+        case 'Z':
+            m[0][0] = cosA;
+            m[0][1] = -sinA;
+            m[0][2] = 0;
+            m[0][3] = 0;
+            m[1][0] = sinA;
+            m[1][1] = cosA;
+            m[1][2] = 0;
+            m[1][3] = 0;
+            m[2][0] = 0;
+            m[2][1] = 0;
+            m[2][2] = 1;
+            m[2][3] = 0;
+            break;
+        default:
+            ASSERTMSGLINE(1529, FALSE, "MTXRotTrig():  invalid 'axis' value ");
+            break;
+    }
+}
+
+#ifdef __MWERKS__
+void PSMTXRotTrig(register Mtx m, register u8 axis, register f32 sinA, register f32 cosA) {
+    register f32 fc0, fc1, nsinA;
+    register f32 fw0, fw1, fw2, fw3;
+
+    asm {
+        frsp        sinA, sinA
+        frsp        cosA, cosA
+    }
+
+    fc0 = 0.0f;
+    fc1 = 1.0f;
+
+    asm {
+        ori         axis, axis, 0x20
+        ps_neg      nsinA, sinA
+        cmplwi      axis, 'x'
+        beq         _case_x
+        cmplwi      axis, 'y'
+        beq         _case_y
+        cmplwi      axis, 'z'
+        beq         _case_z
+        b           _end
+
+    _case_x:
+        psq_st      fc1,  0(m), 1, 0
+        psq_st      fc0,  4(m), 0, 0
+        ps_merge00  fw0, sinA, cosA
+        psq_st      fc0, 12(m), 0, 0
+        ps_merge00  fw1, cosA, nsinA
+        psq_st      fc0, 28(m), 0, 0
+        psq_st      fc0, 44(m), 1, 0
+        psq_st      fw0, 36(m), 0, 0
+        psq_st      fw1, 20(m), 0, 0
+        b           _end;
+
+    _case_y:
+        ps_merge00  fw0, cosA, fc0
+        ps_merge00  fw1, fc0, fc1
+        psq_st      fc0, 24(m), 0, 0
+        psq_st      fw0,  0(m), 0, 0
+        ps_merge00  fw2, nsinA, fc0
+        ps_merge00  fw3, sinA, fc0
+        psq_st      fw0, 40(m), 0, 0;
+        psq_st      fw1, 16(m), 0, 0;
+        psq_st      fw3,  8(m), 0, 0;
+        psq_st      fw2, 32(m), 0, 0;
+        b           _end;
+
+    _case_z:
+        psq_st      fc0,  8(m), 0, 0
+        ps_merge00  fw0, sinA, cosA
+        ps_merge00  fw2, cosA, nsinA
+        psq_st      fc0, 24(m), 0, 0
+        psq_st      fc0, 32(m), 0, 0
+        ps_merge00  fw1, fc1, fc0
+        psq_st      fw0, 16(m), 0, 0
+        psq_st      fw2,  0(m), 0, 0
+        psq_st      fw1, 40(m), 0, 0
+
+    _end:
+    }
+}
+#endif
+
+void C_MTXRotAxisRad(Mtx m, const Vec* axis, f32 rad) {
+    Vec vN;
+    f32 s;
+    f32 c;
+    f32 t;
+    f32 x;
+    f32 y;
+    f32 z;
+    f32 xSq;
+    f32 ySq;
+    f32 zSq;
+
+    ASSERTMSGLINE(1677, m, "MTXRotAxisRad():  NULL MtxPtr 'm' ");
+    ASSERTMSGLINE(1678, axis, "MTXRotAxisRad():  NULL VecPtr 'axis' ");
+
+    s = sinf(rad);
+    c = cosf(rad);
+    t = 1 - c;
+    C_VECNormalize(axis, &vN);
+    x = vN.x;
+    y = vN.y;
+    z = vN.z;
+    xSq = (x * x);
+    ySq = (y * y);
+    zSq = (z * z);
+    m[0][0] = (c + (t * xSq));
+    m[0][1] = (y * (t * x)) - (s * z);
+    m[0][2] = (z * (t * x)) + (s * y);
+    m[0][3] = 0;
+    m[1][0] = ((y * (t * x)) + (s * z));
+    m[1][1] = (c + (t * ySq));
+    m[1][2] = ((z * (t * y)) - (s * x));
+    m[1][3] = 0;
+    m[2][0] = ((z * (t * x)) - (s * y));
+    m[2][1] = ((z * (t * y)) + (s * x));
+    m[2][2] = (c + (t * zSq));
+    m[2][3] = 0;
+}
+
+#ifdef __MWERKS__
+static void __PSMTXRotAxisRadInternal(register Mtx m, const register Vec* axis, register f32 sT, register f32 cT) {
+    register f32 tT, fc0;
+    register f32 tmp0, tmp1, tmp2, tmp3, tmp4;
+    register f32 tmp5, tmp6, tmp7, tmp8, tmp9;
+    tmp9 = 0.5f;
+    tmp8 = 3.0f;
+
+    asm {
+        frsp cT, cT;
+        psq_l tmp0, 0(axis), 0, 0;
+        frsp sT, sT;
+        lfs tmp1, 8(axis);
+        ps_mul tmp2, tmp0, tmp0;
+        fadds tmp7, tmp9, tmp9;
+        ps_madd tmp3, tmp1, tmp1, tmp2;
+        fsubs fc0, tmp9, tmp9;
+        ps_sum0 tmp4, tmp3, tmp1, tmp2;
+        fsubs tT, tmp7, cT;
+        frsqrte tmp5, tmp4;
+        fmuls tmp2, tmp5, tmp5;
+        fmuls tmp3, tmp5, tmp9;
+        fnmsubs tmp2, tmp2, tmp4, tmp8;
+        fmuls tmp5, tmp2, tmp3;
+        ps_merge00  cT, cT, cT;
+        ps_muls0 tmp0, tmp0, tmp5;
+        ps_muls0 tmp1, tmp1, tmp5;
+        ps_muls0 tmp4, tmp0, tT;
+        ps_muls0 tmp9, tmp0, sT;
+        ps_muls0 tmp5, tmp1, tT;
+        ps_muls1 tmp3, tmp4, tmp0;
+        ps_muls0 tmp2, tmp4, tmp0;
+        ps_muls0 tmp4, tmp4, tmp1;
+        fnmsubs tmp6, tmp1, sT, tmp3;
+        fmadds tmp7, tmp1, sT, tmp3;
+        ps_neg tmp0, tmp9;
+        ps_sum0 tmp8, tmp4, fc0, tmp9;
+        ps_sum0 tmp2, tmp2, tmp6, cT;
+        ps_sum1 tmp3, cT, tmp7, tmp3;
+        ps_sum0 tmp6, tmp0, fc0, tmp4;
+        psq_st tmp8, 8(m), 0, 0;
+        ps_sum0 tmp0, tmp4, tmp4, tmp0;
+        psq_st tmp2, 0(m), 0, 0;
+        ps_muls0 tmp5, tmp5, tmp1;
+        psq_st tmp3, 16(m), 0, 0;
+        ps_sum1 tmp4, tmp9, tmp0, tmp4;
+        psq_st tmp6, 24(m), 0, 0;
+        ps_sum0 tmp5, tmp5, fc0, cT;
+        psq_st tmp4, 32(m), 0, 0;
+        psq_st tmp5, 40(m), 0, 0;
+    }
+}
+
+void PSMTXRotAxisRad(Mtx m, const Vec* axis, f32 rad) {
+    f32 sinT, cosT;
+
+    sinT = sinf(rad);
+    cosT = cosf(rad);
+
+    __PSMTXRotAxisRadInternal(m, axis, sinT, cosT);
+}
+#endif
+
+void C_MTXTrans(Mtx m, f32 xT, f32 yT, f32 zT) {
+    ASSERTMSGLINE(1866, m, "MTXTrans():  NULL MtxPtr 'm' ");
+    m[0][0] = 1;
+    m[0][1] = 0;
+    m[0][2] = 0;
+    m[0][3] = xT;
+    m[1][0] = 0;
+    m[1][1] = 1;
+    m[1][2] = 0;
+    m[1][3] = yT;
+    m[2][0] = 0;
+    m[2][1] = 0;
+    m[2][2] = 1;
+    m[2][3] = zT;
+}
+
+#ifdef __MWERKS__
+void PSMTXTrans(register Mtx m, register f32 xT, register f32 yT, register f32 zT) {
+    register f32 c0 = 0.0f;
+    register f32 c1 = 1.0f;
+
+    asm {
+        stfs xT, 12(m)
+        stfs yT, 28(m)
+        psq_st c0, 4(m), 0, 0
+        psq_st c0, 32(m), 0, 0
+        stfs c0, 16(m)
+        stfs c1, 20(m)
+        stfs c0, 24(m)
+        stfs c1, 40(m)
+        stfs zT, 44(m)
+        stfs c1, 0(m)
+    }
+}
+#endif
+
+void C_MTXTransApply(const Mtx src, Mtx dst, f32 xT, f32 yT, f32 zT) {
+    ASSERTMSGLINE(1933, src, "MTXTransApply(): NULL MtxPtr 'src' ");
+    ASSERTMSGLINE(1934, dst, "MTXTransApply(): NULL MtxPtr 'src' "); //! wrong assert string
+
+    if (src != dst) {
+        dst[0][0] = src[0][0];
+        dst[0][1] = src[0][1];
+        dst[0][2] = src[0][2];
+        dst[1][0] = src[1][0];
+        dst[1][1] = src[1][1];
+        dst[1][2] = src[1][2];
+        dst[2][0] = src[2][0];
+        dst[2][1] = src[2][1];
+        dst[2][2] = src[2][2];
+    }
+
+    dst[0][3] = (src[0][3] + xT);
+    dst[1][3] = (src[1][3] + yT);
+    dst[2][3] = (src[2][3] + zT);
+}
+
+#ifdef __MWERKS__
+asm void PSMTXTransApply(const register Mtx src, register Mtx dst, register f32 xT, register f32 yT, register f32 zT) {
+    nofralloc
+    psq_l fp4, 0(src), 0, 0
+    frsp xT, xT
+    psq_l fp5, 8(src), 0, 0
+    frsp yT, yT
+    psq_l fp7, 24(src), 0, 0
+    frsp zT, zT
+    psq_l fp8, 40(src), 0, 0
+    psq_st fp4, 0(dst),  0, 0
+    ps_sum1 fp5, xT, fp5, fp5
+    psq_l fp6, 16(src), 0, 0
+    psq_st fp5, 8(dst), 0, 0
+    ps_sum1 fp7, yT, fp7, fp7
+    psq_l fp9, 32(src), 0, 0
+    psq_st fp6, 16(dst), 0, 0
+    ps_sum1 fp8, zT, fp8, fp8
+    psq_st fp7, 24(dst), 0, 0
+    psq_st fp9, 32(dst), 0, 0
+    psq_st fp8, 40(dst), 0, 0
+    blr
+}
+#endif
+
+void C_MTXScale(Mtx m, f32 xS, f32 yS, f32 zS) {
+    ASSERTMSGLINE(2008, m, "MTXScale():  NULL MtxPtr 'm' ");
+    m[0][0] = xS;
+    m[0][1] = 0;
+    m[0][2] = 0;
+    m[0][3] = 0;
+    m[1][0] = 0;
+    m[1][1] = yS;
+    m[1][2] = 0;
+    m[1][3] = 0;
+    m[2][0] = 0;
+    m[2][1] = 0;
+    m[2][2] = zS;
+    m[2][3] = 0;
+}
+
+#ifdef __MWERKS__
+void PSMTXScale(register Mtx m, register f32 xS, register f32 yS, register f32 zS) {
+    register f32 c0 = 0.0f;
+
+    asm {
+        stfs xS, 0(m)
+        psq_st c0, 4(m), 0, 0
+        psq_st c0, 12(m), 0, 0
+        stfs yS, 20(m)
+        psq_st c0, 24(m), 0, 0
+        psq_st c0, 32(m), 0, 0
+        stfs zS, 40(m)
+        stfs c0, 44(m)
+    }
+}
+#endif
+
+void C_MTXScaleApply(const Mtx src, Mtx dst, f32 xS, f32 yS, f32 zS) {
+    ASSERTMSGLINE(2070, src, "MTXScaleApply(): NULL MtxPtr 'src' ");
+    ASSERTMSGLINE(2071, dst, "MTXScaleApply(): NULL MtxPtr 'dst' ");
+    dst[0][0] = (src[0][0] * xS);
+    dst[0][1] = (src[0][1] * xS);
+    dst[0][2] = (src[0][2] * xS);
+    dst[0][3] = (src[0][3] * xS);
+    dst[1][0] = (src[1][0] * yS);
+    dst[1][1] = (src[1][1] * yS);
+    dst[1][2] = (src[1][2] * yS);
+    dst[1][3] = (src[1][3] * yS);
+    dst[2][0] = (src[2][0] * zS);
+    dst[2][1] = (src[2][1] * zS);
+    dst[2][2] = (src[2][2] * zS);
+    dst[2][3] = (src[2][3] * zS);
+}
+
+#ifdef __MWERKS__
+asm void PSMTXScaleApply(const register Mtx src, register Mtx dst, register f32 xS, register f32 yS, register f32 zS) {
+    nofralloc
+    frsp xS, xS
+    psq_l fp4, 0(src), 0, 0
+    frsp yS, yS
+    psq_l fp5, 8(src), 0, 0
+    frsp zS, zS
+    ps_muls0 fp4, fp4, xS
+    psq_l fp6, 16(src), 0, 0
+    ps_muls0 fp5, fp5, xS
+    psq_l fp7, 24(src), 0, 0
+    ps_muls0 fp6, fp6, yS
+    psq_l fp8, 32(src), 0, 0
+    psq_st fp4, 0(dst), 0, 0
+    ps_muls0 fp7, fp7, yS
+    psq_l fp2, 40(src), 0, 0
+    psq_st fp5, 8(dst), 0, 0
+    ps_muls0 fp8, fp8, zS
+    psq_st fp6, 16(dst), 0, 0
+    ps_muls0 fp2, fp2, zS
+    psq_st fp7, 24(dst), 0, 0
+    psq_st fp8, 32(dst), 0, 0
+    psq_st fp2, 40(dst), 0, 0
+    blr
+}
+#endif
+
+void C_MTXQuat(Mtx m, const Quaternion* q) {
+    f32 s;
+    f32 xs;
+    f32 ys;
+    f32 zs;
+    f32 wx;
+    f32 wy;
+    f32 wz;
+    f32 xx;
+    f32 xy;
+    f32 xz;
+    f32 yy;
+    f32 yz;
+    f32 zz;
+
+    ASSERTMSGLINE(2145, m, "MTXQuat():  NULL MtxPtr 'm' ");
+    ASSERTMSGLINE(2146, q, "MTXQuat():  NULL QuaternionPtr 'q' ");
+    ASSERTMSGLINE(2147, q->x || q->y || q->z || q->w, "MTXQuat():  zero-value quaternion ");
+    s = 2 / ((q->w * q->w) + ((q->z * q->z) + ((q->x * q->x) + (q->y * q->y))));
+    xs = q->x * s;
+    ys = q->y * s;
+    zs = q->z * s;
+    wx = q->w * xs;
+    wy = q->w * ys;
+    wz = q->w * zs;
+    xx = q->x * xs;
+    xy = q->x * ys;
+    xz = q->x * zs;
+    yy = q->y * ys;
+    yz = q->y * zs;
+    zz = q->z * zs;
+    m[0][0] = (1 - (yy + zz));
+    m[0][1] = (xy - wz);
+    m[0][2] = (xz + wy);
+    m[0][3] = 0;
+    m[1][0] = (xy + wz);
+    m[1][1] = (1 - (xx + zz));
+    m[1][2] = (yz - wx);
+    m[1][3] = 0;
+    m[2][0] = (xz - wy);
+    m[2][1] = (yz + wx);
+    m[2][2] = (1 - (xx + yy));
+    m[2][3] = 0;
+}
+
+#ifdef __MWERKS__
+void PSMTXQuat(register Mtx m, const register Quaternion* q) {
+    register f32 c_zero, c_one, c_two, scale;
+    register f32 tmp0, tmp1, tmp2, tmp3, tmp4;
+    register f32 tmp5, tmp6, tmp7, tmp8, tmp9;
+    c_one = 1.0f;
+
+    asm {
+        psq_l tmp0, 0(q), 0, 0
+        psq_l tmp1, 8(q), 0, 0
+        fsubs c_zero, c_one, c_one
+        fadds c_two, c_one, c_one
+        ps_mul tmp2, tmp0, tmp0
+        ps_merge10 tmp5, tmp0, tmp0
+        ps_madd tmp4, tmp1, tmp1, tmp2
+        ps_mul tmp3, tmp1, tmp1
+        ps_sum0 scale, tmp4, tmp4, tmp4
+        ps_muls1 tmp7, tmp5, tmp1
+        fres tmp9, scale
+        ps_sum1 tmp4, tmp3, tmp4, tmp2
+        ps_nmsub scale, scale, tmp9, c_two
+        ps_muls1 tmp6, tmp1, tmp1
+        ps_mul scale, tmp9, scale
+        ps_sum0 tmp2, tmp2, tmp2, tmp2
+        fmuls scale, scale, c_two
+        ps_madd tmp8, tmp0, tmp5, tmp6
+        ps_msub tmp6, tmp0, tmp5, tmp6
+        psq_st c_zero, 12(m), 1, 0
+        ps_nmsub tmp2, tmp2, scale, c_one
+        ps_nmsub tmp4, tmp4, scale, c_one
+        psq_st c_zero, 44(m), 1, 0
+        ps_mul tmp8, tmp8, scale
+        ps_mul tmp6, tmp6, scale
+        psq_st tmp2, 40(m), 1, 0
+        ps_madds0 tmp5, tmp0, tmp1, tmp7
+        ps_merge00 tmp1, tmp8, tmp4
+        ps_nmsub tmp7, tmp7, c_two, tmp5
+        ps_merge10 tmp0, tmp4, tmp6
+        psq_st tmp1, 16(m), 0, 0
+        ps_mul tmp5, tmp5, scale
+        ps_mul tmp7, tmp7, scale
+        psq_st tmp0, 0(m), 0, 0
+        psq_st tmp5, 8(m), 1, 0
+        ps_merge10 tmp3, tmp7, c_zero
+        ps_merge01 tmp9, tmp7, tmp5
+        psq_st tmp3, 24(m), 0, 0
+        psq_st tmp9, 32(m), 0, 0
+    }
+}
+#endif
+
+void C_MTXLookAt(Mtx m, const Point3d* camPos, const Vec* camUp, const Point3d* target) {
+    Vec vLook;
+    Vec vRight;
+    Vec vUp;
+
+    ASSERTMSGLINE(2438, m, "MTXLookAt():  NULL MtxPtr 'm' ");
+    ASSERTMSGLINE(2439, camPos, "MTXLookAt():  NULL VecPtr 'camPos' ");
+    ASSERTMSGLINE(2440, camUp, "MTXLookAt():  NULL VecPtr 'camUp' ");
+    ASSERTMSGLINE(2441, target, "MTXLookAt():  NULL Point3dPtr 'target' ");
+
+    vLook.x = camPos->x - target->x;
+    vLook.y = camPos->y - target->y;
+    vLook.z = camPos->z - target->z;
+    VECNormalize(&vLook, &vLook);
+    VECCrossProduct(camUp, &vLook, &vRight);
+    VECNormalize(&vRight, &vRight);
+    VECCrossProduct(&vLook, &vRight, &vUp);
+    m[0][0] = vRight.x;
+    m[0][1] = vRight.y;
+    m[0][2] = vRight.z;
+    m[0][3] = -((camPos->z * vRight.z) + ((camPos->x * vRight.x) + (camPos->y * vRight.y)));
+    m[1][0] = vUp.x;
+    m[1][1] = vUp.y;
+    m[1][2] = vUp.z;
+    m[1][3] = -((camPos->z * vUp.z) + ((camPos->x * vUp.x) + (camPos->y * vUp.y)));
+    m[2][0] = vLook.x;
+    m[2][1] = vLook.y;
+    m[2][2] = vLook.z;
+    m[2][3] = -((camPos->z * vLook.z) + ((camPos->x * vLook.x) + (camPos->y * vLook.y)));
+}
+
+void C_MTXLightFrustum(Mtx m, f32 t, f32 b, f32 l, f32 r, f32 n, f32 scaleS, f32 scaleT, f32 transS, f32 transT) {
+    f32 tmp;
+
+    ASSERTMSGLINE(2541, m, "MTXLightFrustum():  NULL MtxPtr 'm' ");
+    ASSERTMSGLINE(2542, (t != b), "MTXLightFrustum():  't' and 'b' clipping planes are equal ");
+    ASSERTMSGLINE(2543, (l != r), "MTXLightFrustum():  'l' and 'r' clipping planes are equal ");
+
+    tmp = 1 / (r - l);
+    m[0][0] = (scaleS * (2 * n * tmp));
+    m[0][1] = 0;
+    m[0][2] = (scaleS * (tmp * (r + l))) - transS;
+    m[0][3] = 0;
+    tmp = 1 / (t - b);
+    m[1][0] = 0;
+    m[1][1] = (scaleT * (2 * n * tmp));
+    m[1][2] = (scaleT * (tmp * (t + b))) - transT;
+    m[1][3] = 0;
+    m[2][0] = 0;
+    m[2][1] = 0;
+    m[2][2] = -1;
+    m[2][3] = 0;
+}
+
+void C_MTXLightPerspective(Mtx m, f32 fovY, f32 aspect, f32 scaleS, f32 scaleT, f32 transS, f32 transT) {
+    f32 angle;
+    f32 cot;
+
+    ASSERTMSGLINE(2605, m, "MTXLightPerspective():  NULL MtxPtr 'm' ");
+    ASSERTMSGLINE(2606, (fovY > 0.0) && (fovY < 180.0), "MTXLightPerspective():  'fovY' out of range ");
+    ASSERTMSGLINE(2607, 0 != aspect, "MTXLightPerspective():  'aspect' is 0 ");
+
+    angle = (0.5f * fovY);
+    angle = MTXDegToRad(angle);
+    cot = 1 / tanf(angle);
+    m[0][0] = (scaleS * (cot / aspect));
+    m[0][1] = 0;
+    m[0][2] = -transS;
+    m[0][3] = 0;
+    m[1][0] = 0;
+    m[1][1] = (cot * scaleT);
+    m[1][2] = -transT;
+    m[1][3] = 0;
+    m[2][0] = 0;
+    m[2][1] = 0;
+    m[2][2] = -1;
+    m[2][3] = 0;
+}
+
+void C_MTXLightOrtho(Mtx m, f32 t, f32 b, f32 l, f32 r, f32 scaleS, f32 scaleT, f32 transS, f32 transT) {
+    f32 tmp;
+
+    ASSERTMSGLINE(2673, m, "MTXLightOrtho():  NULL MtxPtr 'm' ");
+    ASSERTMSGLINE(2674, (t != b), "MTXLightOrtho():  't' and 'b' clipping planes are equal ");
+    ASSERTMSGLINE(2675, (l != r), "MTXLightOrtho():  'l' and 'r' clipping planes are equal ");
+    tmp = 1 / (r - l);
+    m[0][0] = (2 * tmp * scaleS);
+    m[0][1] = 0;
+    m[0][2] = 0;
+    m[0][3] = (transS + (scaleS * (tmp * -(r + l))));
+    tmp = 1/ (t - b);
+    m[1][0] = 0;
+    m[1][1] = (2 * tmp * scaleT);
+    m[1][2] = 0;
+    m[1][3] = (transT + (scaleT * (tmp * -(t + b))));
+    m[2][0] = 0;
+    m[2][1] = 0;
+    m[2][2] = 0;
+    m[2][3] = 1;
+}
diff --git a/src/dolphin/mtx/mtx44.c b/src/dolphin/mtx/mtx44.c
new file mode 100644
index 00000000..3b2ac282
--- /dev/null
+++ b/src/dolphin/mtx/mtx44.c
@@ -0,0 +1,92 @@
+
+#include <dolphin/mtx/mtx.h>
+#include <dolphin/mtx/mtx44.h>
+#include <dolphin/os/OS.h>
+#include <math.h>
+
+void C_MTXFrustum(Mtx44 m, f32 t, f32 b, f32 l, f32 r, f32 n, f32 f) {
+    f32 tmp;
+
+    ASSERTMSGLINE(105, m, "MTXFrustum():  NULL Mtx44Ptr 'm' ");
+    ASSERTMSGLINE(106, t != b, "MTXFrustum():  't' and 'b' clipping planes are equal ");
+    ASSERTMSGLINE(107, l != r, "MTXFrustum():  'l' and 'r' clipping planes are equal ");
+    ASSERTMSGLINE(108, n != f, "MTXFrustum():  'n' and 'f' clipping planes are equal ");
+    tmp = 1 / (r - l);
+    m[0][0] = (2 * n * tmp);
+    m[0][1] = 0;
+    m[0][2] = (tmp * (r + l));
+    m[0][3] = 0;
+    tmp = 1 / (t - b);
+    m[1][0] = 0;
+    m[1][1] = (2 * n * tmp);
+    m[1][2] = (tmp * (t + b));
+    m[1][3] = 0;
+    m[2][0] = 0;
+    m[2][1] = 0;
+    tmp = 1 / (f - n);
+    m[2][2] = (-n * tmp);
+    m[2][3] = (tmp * -(f * n));
+    m[3][0] = 0;
+    m[3][1] = 0;
+    m[3][2] = -1;
+    m[3][3] = 0;
+}
+
+void C_MTXPerspective(Mtx44 m, f32 fovY, f32 aspect, f32 n, f32 f) {
+    f32 angle;
+    f32 cot;
+    f32 tmp;
+
+    ASSERTMSGLINE(179, m, "MTXPerspective():  NULL Mtx44Ptr 'm' ");
+    ASSERTMSGLINE(180, (fovY > 0.0) && (fovY < 180.0), "MTXPerspective():  'fovY' out of range ");
+    ASSERTMSGLINE(181, 0.0f != aspect, "MTXPerspective():  'aspect' is 0 ");
+
+    angle = (0.5f * fovY);
+    angle = MTXDegToRad(angle);
+    cot = 1 / tanf(angle);
+    m[0][0] = (cot / aspect);
+    m[0][1] = 0;
+    m[0][2] = 0;
+    m[0][3] = 0;
+    m[1][0] = 0;
+    m[1][1] = (cot);
+    m[1][2] = 0;
+    m[1][3] = 0;
+    m[2][0] = 0;
+    m[2][1] = 0;
+    tmp = 1 / (f - n);
+    m[2][2] = (-n * tmp);
+    m[2][3] = (tmp * -(f * n));
+    m[3][0] = 0;
+    m[3][1] = 0;
+    m[3][2] = -1;
+    m[3][3] = 0;
+}
+
+void C_MTXOrtho(Mtx44 m, f32 t, f32 b, f32 l, f32 r, f32 n, f32 f) {
+    f32 tmp;
+
+    ASSERTMSGLINE(254, m, "MTXOrtho():  NULL Mtx44Ptr 'm' ");
+    ASSERTMSGLINE(255, t != b, "MTXOrtho():  't' and 'b' clipping planes are equal ");
+    ASSERTMSGLINE(256, l != r, "MTXOrtho():  'l' and 'r' clipping planes are equal ");
+    ASSERTMSGLINE(257, n != f, "MTXOrtho():  'n' and 'f' clipping planes are equal ");
+    tmp = 1 / (r - l);
+    m[0][0] = 2 * tmp;
+    m[0][1] = 0;
+    m[0][2] = 0;
+    m[0][3] = (tmp * -(r + l));
+    tmp = 1 / (t - b);
+    m[1][0] = 0;
+    m[1][1] = 2 * tmp;
+    m[1][2] = 0;
+    m[1][3] = (tmp * -(t + b));
+    m[2][0] = 0;
+    m[2][1] = 0;
+    tmp = 1 / (f - n);
+    m[2][2] = (-1 * tmp);
+    m[2][3] = (-f * tmp);
+    m[3][0] = 0;
+    m[3][1] = 0;
+    m[3][2] = 0;
+    m[3][3] = 1;
+}
diff --git a/src/dolphin/mtx/mtxvec.c b/src/dolphin/mtx/mtxvec.c
new file mode 100644
index 00000000..d414db32
--- /dev/null
+++ b/src/dolphin/mtx/mtxvec.c
@@ -0,0 +1,212 @@
+
+#include <dolphin/mtx/mtxvec.h>
+#include <dolphin/os/OS.h>
+
+void C_MTXMultVec(const Mtx m, const Vec* src, Vec* dst) {
+    Vec vTmp;
+
+    ASSERTMSGLINE(66, m, "MTXMultVec():  NULL MtxPtr 'm' ");
+    ASSERTMSGLINE(67, src, "MTXMultVec():  NULL VecPtr 'src' ");
+    ASSERTMSGLINE(68, dst, "MTXMultVec():  NULL VecPtr 'dst' ");
+
+    vTmp.x = m[0][3] + ((m[0][2] * src->z) + ((m[0][0] * src->x) + (m[0][1] * src->y)));
+    vTmp.y = m[1][3] + ((m[1][2] * src->z) + ((m[1][0] * src->x) + (m[1][1] * src->y)));
+    vTmp.z = m[2][3] + ((m[2][2] * src->z) + ((m[2][0] * src->x) + (m[2][1] * src->y)));
+    dst->x = vTmp.x;
+    dst->y = vTmp.y;
+    dst->z = vTmp.z;
+}
+
+#ifdef __MWERKS__
+asm void PSMTXMultVec(const register Mtx m, const register Vec* src, register Vec* dst) {
+    nofralloc
+    psq_l f0, Vec.x(src), 0, 0
+    psq_l f2, 0(m), 0, 0
+    psq_l f1, Vec.z(src), 1, 0
+    ps_mul f4, f2, f0
+    psq_l f3, 8(m), 0, 0
+    ps_madd f5, f3, f1, f4
+    psq_l f8, 16(m), 0, 0
+    ps_sum0 f6, f5, f6, f5
+    psq_l f9, 24(m), 0, 0
+    ps_mul f10, f8, f0
+    psq_st f6, Vec.x(dst), 1, 0
+    ps_madd f11, f9, f1, f10
+    psq_l f2, 32(m), 0, 0
+    ps_sum0 f12, f11, f12, f11
+    psq_l f3, 40(m), 0, 0
+    ps_mul f4, f2, f0
+    psq_st f12, Vec.y(dst), 1, 0
+    ps_madd f5, f3, f1, f4
+    ps_sum0 f6, f5, f6, f5
+    psq_st f6, Vec.z(dst), 1, 0
+    blr
+}
+#endif
+
+void C_MTXMultVecArray(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count) {
+    u32 i;
+    Vec vTmp;
+
+    ASSERTMSGLINE(168, m, "MTXMultVecArray():  NULL MtxPtr 'm' ");
+    ASSERTMSGLINE(169, srcBase, "MTXMultVecArray():  NULL VecPtr 'srcBase' ");
+    ASSERTMSGLINE(170, dstBase, "MTXMultVecArray():  NULL VecPtr 'dstBase' ");
+    ASSERTMSGLINE(171, count > 1, "MTXMultVecArray():  count must be greater than 1.");
+
+    for(i = 0; i < count; i++) {
+        vTmp.x = m[0][3] + ((m[0][2] * srcBase->z) + ((m[0][0] * srcBase->x) + (m[0][1] * srcBase->y)));
+        vTmp.y = m[1][3] + ((m[1][2] * srcBase->z) + ((m[1][0] * srcBase->x) + (m[1][1] * srcBase->y)));
+        vTmp.z = m[2][3] + ((m[2][2] * srcBase->z) + ((m[2][0] * srcBase->x) + (m[2][1] * srcBase->y)));
+        dstBase->x = vTmp.x;
+        dstBase->y = vTmp.y;
+        dstBase->z = vTmp.z;
+        srcBase++;
+        dstBase++;
+    }
+}
+
+#ifdef __MWERKS__
+asm void PSMTXMultVecArray(const register Mtx m, const register Vec* srcBase, register Vec* dstBase, register u32 count) {
+    nofralloc
+    psq_l f13, 0x0(m), 0, 0
+    psq_l f12, 0x10(m), 0, 0
+    subi count, count, 0x1
+    psq_l f11, 0x8(m), 0, 0
+    ps_merge00 f0, f13, f12
+    subi dstBase, dstBase, 0x4
+    psq_l f10, 0x18(m), 0, 0
+    ps_merge11 f1, f13, f12
+    mtctr count
+    psq_l f4, 0x20(m), 0, 0
+    ps_merge00 f2, f11, f10
+    psq_l f5, 0x28(m), 0, 0
+    ps_merge11 f3, f11, f10
+    psq_l f6, 0x0(srcBase), 0, 0
+    psq_lu f7, 0x8(srcBase), 1, 0
+    ps_madds0 f8, f0, f6, f3
+    ps_mul f9, f4, f6
+    ps_madds1 f8, f1, f6, f8
+    ps_madd f10, f5, f7, f9
+L_000003C4:
+    psq_lu f6, 0x4(srcBase), 0, 0
+    ps_madds0 f12, f2, f7, f8
+    psq_lu f7, 0x8(srcBase), 1, 0
+    ps_sum0 f13, f10, f9, f10
+    ps_madds0 f8, f0, f6, f3
+    ps_mul f9, f4, f6
+    psq_stu f12, 0x4(dstBase), 0, 0
+    ps_madds1 f8, f1, f6, f8
+    psq_stu f13, 0x8(dstBase), 1, 0
+    ps_madd f10, f5, f7, f9
+    bdnz L_000003C4
+    ps_madds0 f12, f2, f7, f8
+    ps_sum0 f13, f10, f9, f10
+    psq_stu f12, 0x4(dstBase), 0, 0
+    psq_stu f13, 0x8(dstBase), 1, 0
+    blr
+}
+#endif
+
+void C_MTXMultVecSR(const Mtx m, const Vec* src, Vec* dst) {
+    Vec vTmp;
+
+    ASSERTMSGLINE(313, m, "MTXMultVecSR():  NULL MtxPtr 'm' ");
+    ASSERTMSGLINE(314, src, "MTXMultVecSR():  NULL VecPtr 'src' ");
+    ASSERTMSGLINE(315, dst, "MTXMultVecSR():  NULL VecPtr 'dst' ");
+
+    vTmp.x = (m[0][2] * src->z) + ((m[0][0] * src->x) + (m[0][1] * src->y));
+    vTmp.y = (m[1][2] * src->z) + ((m[1][0] * src->x) + (m[1][1] * src->y));
+    vTmp.z = (m[2][2] * src->z) + ((m[2][0] * src->x) + (m[2][1] * src->y));
+    dst->x = vTmp.x;
+    dst->y = vTmp.y;
+    dst->z = vTmp.z;
+}
+
+#ifdef __MWERKS__
+asm void PSMTXMultVecSR(const register Mtx m, const register Vec* src, register Vec* dst) {
+    nofralloc
+    psq_l f0, 0x0(m), 0, 0
+    psq_l f6, 0x0(src), 0, 0
+    psq_l f2, 0x10(m), 0, 0
+    ps_mul f8, f0, f6
+    psq_l f4, 0x20(m), 0, 0
+    ps_mul f10, f2, f6
+    psq_l f7, 0x8(src), 1, 0
+    ps_mul f12, f4, f6
+    psq_l f3, 0x18(m), 0, 0
+    ps_sum0 f8, f8, f8, f8
+    psq_l f5, 0x28(m), 0, 0
+    ps_sum0 f10, f10, f10, f10
+    psq_l f1, 0x8(m), 0, 0
+    ps_sum0 f12, f12, f12, f12
+    ps_madd f9, f1, f7, f8
+    psq_st f9, 0x0(dst), 1, 0
+    ps_madd f11, f3, f7, f10
+    psq_st f11, 0x4(dst), 1, 0
+    ps_madd f13, f5, f7, f12
+    psq_st f13, 0x8(dst), 1, 0
+    blr
+}
+#endif
+
+void C_MTXMultVecArraySR(const Mtx m, const Vec* srcBase, Vec* dstBase, u32 count) {
+    u32 i;
+    Vec vTmp;
+
+    ASSERTMSGLINE(410, m, "MTXMultVecArraySR():  NULL MtxPtr 'm' ");
+    ASSERTMSGLINE(411, srcBase, "MTXMultVecArraySR():  NULL VecPtr 'srcBase' ");
+    ASSERTMSGLINE(412, dstBase, "MTXMultVecArraySR():  NULL VecPtr 'dstBase' ");
+    ASSERTMSGLINE(413, count > 1, "MTXMultVecArraySR():  count must be greater than 1.");
+
+    for(i = 0; i < count; i++) {
+        vTmp.x = (m[0][2] * srcBase->z) + ((m[0][0] * srcBase->x) + (m[0][1] * srcBase->y));
+        vTmp.y = (m[1][2] * srcBase->z) + ((m[1][0] * srcBase->x) + (m[1][1] * srcBase->y));
+        vTmp.z = (m[2][2] * srcBase->z) + ((m[2][0] * srcBase->x) + (m[2][1] * srcBase->y));
+        dstBase->x = vTmp.x;
+        dstBase->y = vTmp.y;
+        dstBase->z = vTmp.z;
+        srcBase++;
+        dstBase++;
+    }
+}
+
+#ifdef __MWERKS__
+asm void PSMTXMultVecArraySR(const register Mtx m, const register Vec* srcBase, register Vec* dstBase, register u32 count) {
+    nofralloc
+    psq_l f13, 0x0(m), 0, 0
+    psq_l f12, 0x10(m), 0, 0
+    subi count, count, 0x1
+    psq_l f11, 0x8(m), 1, 0
+    ps_merge00 f0, f13, f12
+    subi dstBase, dstBase, 0x4
+    psq_l f10, 0x18(m), 1, 0
+    ps_merge11 f1, f13, f12
+    mtctr count
+    psq_l f3, 0x20(m), 0, 0
+    ps_merge00 f2, f11, f10
+    psq_l f4, 0x28(m), 1, 0
+    psq_l f6, 0x0(srcBase), 0, 0
+    psq_lu f7, 0x8(srcBase), 1, 0
+    ps_muls0 f8, f0, f6
+    ps_mul f9, f3, f6
+    ps_madds1 f8, f1, f6, f8
+    ps_madd f10, f4, f7, f9
+L_000007D0:
+    psq_lu f6, 0x4(srcBase), 0, 0
+    ps_madds0 f12, f2, f7, f8
+    psq_lu f7, 0x8(srcBase), 1, 0
+    ps_sum0 f13, f10, f9, f9
+    ps_muls0 f8, f0, f6
+    ps_mul f9, f3, f6
+    psq_stu f12, 0x4(dstBase), 0, 0
+    ps_madds1 f8, f1, f6, f8
+    psq_stu f13, 0x8(dstBase), 1, 0
+    ps_madd f10, f4, f7, f9
+    bdnz L_000007D0
+    ps_madds0 f12, f2, f7, f8
+    ps_sum0 f13, f10, f9, f9
+    psq_stu f12, 0x4(dstBase), 0, 0
+    psq_stu f13, 0x8(dstBase), 1, 0
+    blr
+}
+#endif
diff --git a/src/dolphin/mtx/quat.c b/src/dolphin/mtx/quat.c
new file mode 100644
index 00000000..eb5f0656
--- /dev/null
+++ b/src/dolphin/mtx/quat.c
@@ -0,0 +1,224 @@
+
+#include <dolphin/mtx/quat.h>
+#include <dolphin/os/OS.h>
+#include <math.h>
+
+void C_QUATMultiply(const Quaternion* p, const Quaternion* q, Quaternion* pq) {
+    Quaternion* r;
+    Quaternion pqTmp;
+
+    ASSERTMSGLINE(193, p, "QUATMultiply():  NULL QuaternionPtr 'p' ");
+    ASSERTMSGLINE(194, q, "QUATMultiply():  NULL QuaternionPtr 'q' ");
+    ASSERTMSGLINE(195, pq, "QUATMultiply():  NULL QuaternionPtr 'pq' ");
+
+    if (p == pq || q == pq){
+        r = &pqTmp;
+    } else {
+        r = pq;
+    }
+
+    r->w = (p->w * q->w) - (p->x * q->x) - (p->y * q->y) - (p->z * q->z);
+    r->x = (p->w * q->x) + (p->x * q->w) + (p->y * q->z) - (p->z * q->y);
+    r->y = (p->w * q->y) + (p->y * q->w) + (p->z * q->x) - (p->x * q->z);
+    r->z = (p->w * q->z) + (p->z * q->w) + (p->x * q->y) - (p->y * q->x);
+
+    if (r == &pqTmp) {
+        *pq = pqTmp;
+    }
+}
+
+#ifdef __MWERKS__
+void PSQUATMultiply(const register Quaternion* p, const register Quaternion* q, register Quaternion* pq) {
+    register f32 pxy, pzw;
+    register f32 qxy, qzw;
+    register f32 pnxy, pnzw, pnxny, pnznw;
+    register f32 rxy, rzw;
+    register f32 sxy, szw;
+
+    asm {
+        psq_l pxy, 0x0(p), 0, 0
+        psq_l pzw, 0x8(p), 0, 0
+        psq_l qxy, 0x0(q), 0, 0
+        ps_neg pnxny, pxy
+        psq_l qzw, 0x8(q), 0, 0
+        ps_neg pnznw, pzw
+        ps_merge01 pnxy, pnxny, pxy
+        ps_muls0 rxy, pzw, qxy
+        ps_muls0 rzw, pnxny, qxy
+        ps_merge01 pnzw, pnznw, pzw
+        ps_muls1 szw, pnxy, qxy
+        ps_madds0 rxy, pnxy, qzw, rxy
+        ps_muls1 sxy, pnzw, qxy
+        ps_madds0 rzw, pnzw, qzw, rzw
+        ps_madds1 szw, pnznw, qzw, szw
+        ps_merge10 rxy, rxy, rxy
+        ps_madds1 sxy, pxy, qzw, sxy
+        ps_merge10 rzw, rzw, rzw
+        ps_add rxy, rxy, sxy
+        psq_st rxy, 0x0(pq), 0, 0
+        ps_sub rzw, rzw, szw
+        psq_st rzw, 0x8(pq), 0, 0
+    }
+}
+#endif
+
+void C_QUATNormalize(const Quaternion* src, Quaternion* unit) {
+    f32 mag;
+    ASSERTMSGLINE(407, src, "QUATNormalize():  NULL QuaternionPtr 'src' ");
+    ASSERTMSGLINE(408, unit, "QUATNormalize():  NULL QuaternionPtr 'unit' ");
+
+    mag = (src->x * src->x) + (src->y * src->y) + (src->z * src->z) + (src->w * src->w);
+    if (mag >= 0.00001f) {
+        mag = 1.0f / sqrtf(mag);
+
+        unit->x = src->x * mag;
+        unit->y = src->y * mag;
+        unit->z = src->z * mag;
+        unit->w = src->w * mag;
+    } else {
+        unit->x = unit->y = unit->z = unit->w = 0.0f;
+    }
+}
+
+#ifdef __MWERKS__
+void PSQUATNormalize(const register Quaternion* src, register Quaternion* unit) {
+    register f32 sxy, szw;
+    register f32 mag, rsqmag;
+    register f32 diff;
+    register f32 c_zero;
+    register f32 nwork0, nwork1;
+
+    register f32 epsilon = 0.00001f;
+    register f32 c_half = 0.5f;
+    register f32 c_three = 3.0f;
+
+    asm {
+        psq_l sxy, 0x0(src), 0, 0
+        ps_mul mag, sxy, sxy
+        psq_l szw, 0x8(src), 0, 0
+        ps_sub c_zero, epsilon, epsilon
+        ps_madd mag, szw, szw, mag
+        ps_sum0 mag, mag, mag, mag
+        frsqrte rsqmag, mag
+        ps_sub diff, mag, epsilon
+        fmul nwork0, rsqmag, rsqmag
+        fmul nwork1, rsqmag, c_half
+        fnmsub nwork0, nwork0, mag, c_three
+        fmul rsqmag, nwork0, nwork1
+        ps_sel rsqmag, diff, rsqmag, c_zero
+        ps_muls0 sxy, sxy, rsqmag
+        ps_muls0 szw, szw, rsqmag
+        psq_st sxy, 0x0(unit), 0, 0
+        psq_st szw, 0x8(unit), 0, 0
+    }
+}
+#endif
+
+void C_QUATInverse(const Quaternion* src, Quaternion* inv) {
+    f32 mag, norminv;
+    ASSERTMSGLINE(498, src, "QUATInverse():  NULL QuaternionPtr 'src' ");
+    ASSERTMSGLINE(499, inv, "QUATInverse():  NULL QuaternionPtr 'inv' ");
+
+    mag = (src->x * src->x) + (src->y * src->y) + (src->z * src->z) + (src->w * src->w);
+    if (mag == 0.0f) {
+        mag = 1.0f;
+    }
+
+    norminv = 1.0f / mag;
+    inv->x = -src->x * norminv;
+    inv->y = -src->y * norminv;
+    inv->z = -src->z * norminv;
+    inv->w =  src->w * norminv;
+}
+
+#ifdef __MWERKS__
+void PSQUATInverse(const register Quaternion* src, register Quaternion* inv) {
+    register f32 sxy, szw;
+    register f32 izz, iww;
+    register f32 mag, nmag;
+    register f32 norminv, nninv;
+    register f32 nwork0;
+    register f32 c_two;
+    register f32 c_zero;
+    register f32 c_one = 1.0f;
+
+    asm {
+        psq_l sxy, 0x0(src), 0, 0
+        ps_mul mag, sxy, sxy
+        ps_sub c_zero, c_one, c_one
+        psq_l szw, 0x8(src), 0, 0
+        ps_madd mag, szw, szw, mag
+        ps_add c_two, c_one, c_one
+        ps_sum0 mag, mag, mag, mag
+        fcmpu cr0, mag, c_zero
+        beq L_00000948
+        fres norminv, mag
+        ps_neg nmag, mag
+        ps_nmsub nwork0, mag, norminv, c_two
+        ps_mul norminv, norminv, nwork0
+        b L_0000094C
+    L_00000948:
+        fmr norminv, c_one
+    L_0000094C:
+        ps_neg nninv, norminv
+        ps_muls1 iww, norminv, szw
+        ps_muls0 sxy, sxy, nninv
+        psq_st iww, 0xc(inv), 1, 0
+        ps_muls0 izz, szw, nninv
+        psq_st sxy, 0x0(inv), 0, 0
+        psq_st izz, 0x8(inv), 1, 0
+    }
+}
+#endif
+
+void C_QUATRotAxisRad(Quaternion* r, const Vec* axis, f32 rad) {
+    f32 half, sh, ch;
+    Vec nAxis;
+
+    ASSERTMSGLINE(758, r, "QUATRotAxisRad():  NULL QuaternionPtr 'r' ");
+    ASSERTMSGLINE(759, axis, "QUATRotAxisRad():  NULL VecPtr 'axis' ");
+
+    VECNormalize(axis, &nAxis);
+
+    half = rad * 0.5f;
+    sh = sinf(half);
+    ch = cosf(half);
+
+    r->x = sh * nAxis.x;
+    r->y = sh * nAxis.y;
+    r->z = sh * nAxis.z;
+    r->w = ch;
+}
+
+void C_QUATSlerp(const Quaternion* p, const Quaternion* q, Quaternion* r, f32 t) {
+    f32 theta, sin_th, cos_th;
+    f32 tp, tq;
+
+    ASSERTMSGLINE(869, p, "QUATSlerp():  NULL QuaternionPtr 'p' ");
+    ASSERTMSGLINE(870, q, "QUATSlerp():  NULL QuaternionPtr 'q' ");
+    ASSERTMSGLINE(871, r, "QUATSlerp():  NULL QuaternionPtr 'r' ");
+    
+    cos_th = p->x * q->x + p->y * q->y + p->z * q->z + p->w * q->w;
+    tq = 1.0f;
+
+    if (cos_th < 0.0f) {
+        cos_th = -cos_th;
+        tq = -tq;
+    }
+
+    if (cos_th <= 0.99999f) {
+        theta = acosf(cos_th);
+        sin_th = sinf(theta);
+
+        tp = sinf((1.0f - t) * theta) / sin_th;
+        tq *= sinf(t * theta) / sin_th;
+    } else {
+        tp = 1.0f - t;
+        tq *= t;
+    }
+
+    r->x = (tp * p->x) + (tq * q->x);
+    r->y = (tp * p->y) + (tq * q->y);
+    r->z = (tp * p->z) + (tq * q->z);
+    r->w = (tp * p->w) + (tq * q->w);
+}
diff --git a/src/dolphin/mtx/vec.c b/src/dolphin/mtx/vec.c
new file mode 100644
index 00000000..c9b42ccc
--- /dev/null
+++ b/src/dolphin/mtx/vec.c
@@ -0,0 +1,365 @@
+
+#include <dolphin/mtx/vec.h>
+#include <dolphin/os/OS.h>
+#include <math.h>
+
+void C_VECAdd(const Vec* a, const Vec* b, Vec* ab) {
+    ASSERTMSGLINE(108, a, "VECAdd():  NULL VecPtr 'a' ");
+    ASSERTMSGLINE(109, b, "VECAdd():  NULL VecPtr 'b' ");
+    ASSERTMSGLINE(110, ab, "VECAdd():  NULL VecPtr 'ab' ");
+    ab->x = a->x + b->x;
+    ab->y = a->y + b->y;
+    ab->z = a->z + b->z;
+}
+
+#ifdef __MWERKS__
+asm void PSVECAdd(const register Vec* a, const register Vec* b, register Vec* ab) {
+    psq_l f2, Vec.x(a), 0, 0
+    psq_l f4, Vec.x(b), 0, 0
+    ps_add f6, f2, f4
+    psq_st f6, Vec.x(ab), 0, 0
+    psq_l f3, Vec.z(a), 1, 0
+    psq_l f5, Vec.z(b), 1, 0
+    ps_add f7, f3, f5
+    psq_st f7, Vec.z(ab), 1, 0
+}
+#endif
+
+void C_VECSubtract(const Vec* a, const Vec* b, Vec* a_b) {
+    ASSERTMSGLINE(177, a, "VECSubtract():  NULL VecPtr 'a' ");
+    ASSERTMSGLINE(178, b, "VECSubtract():  NULL VecPtr 'b' ");
+    ASSERTMSGLINE(179, a_b, "VECSubtract():  NULL VecPtr 'a_b' ");
+    a_b->x = a->x - b->x;
+    a_b->y = a->y - b->y;
+    a_b->z = a->z - b->z;
+}
+
+#ifdef __MWERKS__
+asm void PSVECSubtract(const register Vec* a, const register Vec* b, register Vec* a_b) {
+    psq_l f2, Vec.x(a), 0, 0
+    psq_l f4, Vec.x(b), 0, 0
+    ps_sub f6, f2, f4
+    psq_st f6, Vec.x(a_b), 0, 0
+    psq_l f3, Vec.z(a), 1, 0
+    psq_l f5, Vec.z(b), 1, 0
+    ps_sub f7, f3, f5
+    psq_st f7, Vec.z(a_b), 1, 0
+}
+#endif
+
+void C_VECScale(const Vec* src, Vec* dst, f32 scale) {
+    ASSERTMSGLINE(247, src, "VECScale():  NULL VecPtr 'src' ");
+    ASSERTMSGLINE(248, dst, "VECScale():  NULL VecPtr 'dst' ");
+    dst->x = (src->x * scale);
+    dst->y = (src->y * scale);
+    dst->z = (src->z * scale);
+}
+
+#ifdef __MWERKS__
+void PSVECScale(const register Vec* src, register Vec* dst, register f32 scale) {
+    register f32 vxy, vz, rxy, rz;
+
+    asm {
+        psq_l vxy, 0x0(src), 0, 0
+        psq_l vz, 0x8(src), 1, 0
+        ps_muls0 rxy, vxy, scale
+        psq_st rxy, 0x0(dst), 0, 0
+        ps_muls0 rz, vz, scale
+        psq_st rz, 0x8(dst), 1, 0
+    }
+}
+#endif
+
+void C_VECNormalize(const Vec* src, Vec* unit) {
+    f32 mag;
+
+    ASSERTMSGLINE(315, src, "VECNormalize():  NULL VecPtr 'src' ");
+    ASSERTMSGLINE(316, unit, "VECNormalize():  NULL VecPtr 'unit' ");
+
+    mag = (src->z * src->z) + ((src->x * src->x) + (src->y * src->y));
+    ASSERTMSGLINE(321, 0.0f != mag, "VECNormalize():  zero magnitude vector ");
+
+    mag = 1.0f / sqrtf(mag);
+    unit->x = src->x * mag;
+    unit->y = src->y * mag;
+    unit->z = src->z * mag;
+}
+
+#ifdef __MWERKS__
+void PSVECNormalize(const register Vec* src, register Vec* unit) {
+    register float c_half = 0.5f;
+    register float c_three = 3.0f;
+    register float v1_xy;
+    register float v1_z;
+    register float xx_zz;
+    register float xx_yy;
+    register float sqsum;
+    register float rsqrt;
+    register float nwork0;
+    register float nwork1;
+
+    asm {
+        psq_l v1_xy, 0x0(src), 0, 0
+        ps_mul xx_yy, v1_xy, v1_xy
+        psq_l v1_z, 0x8(src), 1, 0
+        ps_madd xx_zz, v1_z, v1_z, xx_yy
+        ps_sum0 sqsum, xx_zz, v1_z, xx_yy
+        frsqrte rsqrt, sqsum
+        fmuls nwork0, rsqrt, rsqrt
+        fmuls nwork1, rsqrt, c_half
+        fnmsubs nwork0, nwork0, sqsum, c_three
+        fmuls rsqrt, nwork0, nwork1
+        ps_muls0 v1_xy, v1_xy, rsqrt
+        psq_st v1_xy, 0x0(unit), 0, 0
+        ps_muls0 v1_z, v1_z, rsqrt
+        psq_st v1_z, 0x8(unit), 1, 0
+    }
+}
+#endif
+
+f32 C_VECSquareMag(const Vec* v) {
+    f32 sqmag;
+
+    ASSERTMSGLINE(405, v, "VECMag():  NULL VecPtr 'v' ");
+
+    sqmag = v->z * v->z + ((v->x * v->x) + (v->y * v->y));
+    return sqmag;
+}
+
+#ifdef __MWERKS__
+f32 PSVECSquareMag(const register Vec* v) {
+    register f32 vxy, vzz, sqmag;
+
+    asm {
+        psq_l vxy, 0x0(v), 0, 0
+        ps_mul vxy, vxy, vxy
+        lfs vzz, 0x8(v)
+        ps_madd sqmag, vzz, vzz, vxy
+        ps_sum0 sqmag, sqmag, vxy, vxy
+    }
+
+    return sqmag;
+}
+#endif
+
+f32 C_VECMag(const Vec* v) {
+    return sqrtf(C_VECSquareMag(v));
+}
+
+#ifdef __MWERKS__
+f32 PSVECMag(const register Vec* v) {
+    register f32 vxy, vzz;
+    register f32 sqmag, rmag;
+    register f32 nwork0, nwork1;
+    register f32 c_three, c_half, c_zero;
+
+    c_half = 0.5f;
+
+    asm {
+        psq_l vxy, 0x0(v), 0, 0
+        ps_mul vxy, vxy, vxy
+        lfs vzz, 0x8(v)
+        fsubs c_zero, c_half, c_half
+        ps_madd sqmag, vzz, vzz, vxy
+        ps_sum0 sqmag, sqmag, vxy, vxy
+        fcmpu cr0, sqmag, c_zero
+        beq L_000005F0
+        frsqrte rmag, sqmag
+    }
+
+    c_three = 3.0f;
+
+    asm {
+        fmuls nwork0, rmag, rmag
+        fmuls nwork1, rmag, c_half
+        fnmsubs nwork0, nwork0, sqmag, c_three
+        fmuls rmag, nwork0, nwork1
+        fmuls sqmag, sqmag, rmag
+    L_000005F0:
+    }
+
+    return sqmag;
+}
+#endif
+
+f32 C_VECDotProduct(const Vec* a, const Vec* b) {
+    f32 dot;
+
+    ASSERTMSGLINE(540, a, "VECDotProduct():  NULL VecPtr 'a' ");
+    ASSERTMSGLINE(541, b, "VECDotProduct():  NULL VecPtr 'b' ");
+    dot = (a->z * b->z) + ((a->x * b->x) + (a->y * b->y));
+    return dot;
+}
+
+#ifdef __MWERKS__
+asm f32 PSVECDotProduct(const register Vec* a, const register Vec* b) {
+    psq_l f2, Vec.y(a), 0, 0
+    psq_l f3, Vec.y(b), 0, 0
+    ps_mul f2, f2, f3
+    psq_l f5, Vec.x(a), 0, 0
+    psq_l f4, Vec.x(b), 0, 0
+    ps_madd f3, f5, f4, f2
+    ps_sum0 f1, f3, f2, f2
+}
+#endif
+
+void C_VECCrossProduct(const Vec* a, const Vec* b, Vec* axb) {
+    Vec vTmp;
+
+    ASSERTMSGLINE(602, a, "VECCrossProduct():  NULL VecPtr 'a' ");
+    ASSERTMSGLINE(603, b, "VECCrossProduct():  NULL VecPtr 'b' ");
+    ASSERTMSGLINE(604, axb, "VECCrossProduct():  NULL VecPtr 'axb' ");
+
+    vTmp.x = (a->y * b->z) - (a->z * b->y);
+    vTmp.y = (a->z * b->x) - (a->x * b->z);
+    vTmp.z = (a->x * b->y) - (a->y * b->x);
+    axb->x = vTmp.x;
+    axb->y = vTmp.y;
+    axb->z = vTmp.z;
+}
+
+#ifdef __MWERKS__
+asm void PSVECCrossProduct(const register Vec* a, const register Vec* b, register Vec* axb) {
+    psq_l f1, Vec.x(b), 0, 0
+    lfs f2, Vec.z(a)
+    psq_l f0, Vec.x(a), 0, 0
+    ps_merge10 f6, f1, f1
+    lfs f3, Vec.z(b)
+    ps_mul f4, f1, f2
+    ps_muls0 f7, f1, f0
+    ps_msub f5, f0, f3, f4
+    ps_msub f8, f0, f6, f7
+    ps_merge11 f9, f5, f5
+    ps_merge01 f10, f5, f8
+    psq_st f9, Vec.x(axb), 1, 0
+    ps_neg f10, f10
+    psq_st f10, Vec.y(axb), 0, 0
+}
+#endif
+
+void C_VECHalfAngle(const Vec* a, const Vec* b, Vec* half) {
+    Vec aTmp;
+    Vec bTmp;
+    Vec hTmp;
+
+    ASSERTMSGLINE(707, a, "VECHalfAngle():  NULL VecPtr 'a' ");
+    ASSERTMSGLINE(708, b, "VECHalfAngle():  NULL VecPtr 'b' ");
+    ASSERTMSGLINE(709, half, "VECHalfAngle():  NULL VecPtr 'half' ");
+
+    aTmp.x = -a->x;
+    aTmp.y = -a->y;
+    aTmp.z = -a->z;
+    bTmp.x = -b->x;
+    bTmp.y = -b->y;
+    bTmp.z = -b->z;
+
+    VECNormalize(&aTmp, &aTmp);
+    VECNormalize(&bTmp, &bTmp);
+    VECAdd(&aTmp, &bTmp, &hTmp);
+
+    if (VECDotProduct(&hTmp, &hTmp) > 0.0f) {
+        VECNormalize(&hTmp, half);
+        return;
+    }
+    *half = hTmp;
+}
+
+void C_VECReflect(const Vec* src, const Vec* normal, Vec* dst) {
+    f32 cosA;
+    Vec uI;
+    Vec uN;
+
+    ASSERTMSGLINE(763, src, "VECReflect():  NULL VecPtr 'src' ");
+    ASSERTMSGLINE(764, normal, "VECReflect():  NULL VecPtr 'normal' ");
+    ASSERTMSGLINE(765, dst, "VECReflect():  NULL VecPtr 'dst' ");
+
+    uI.x = -src->x;
+    uI.y = -src->y;
+    uI.z = -src->z;
+
+    VECNormalize(&uI, &uI);
+    VECNormalize(normal, &uN);
+
+    cosA = VECDotProduct(&uI, &uN);
+    dst->x = (2.0f * uN.x * cosA) - uI.x;
+    dst->y = (2.0f * uN.y * cosA) - uI.y;
+    dst->z = (2.0f * uN.z * cosA) - uI.z;
+    VECNormalize(dst, dst);
+}
+
+f32 C_VECSquareDistance(const Vec* a, const Vec* b) {
+    Vec diff;
+
+    diff.x = a->x - b->x;
+    diff.y = a->y - b->y;
+    diff.z = a->z - b->z;
+    return (diff.z * diff.z) + ((diff.x * diff.x) + (diff.y * diff.y));
+}
+
+#ifdef __MWERKS__
+f32 PSVECSquareDistance(const register Vec* a, const register Vec* b) {
+    register f32 v0yz, v1yz, v0xy, v1xy, dyz, dxy;
+    register f32 sqdist;
+
+    asm {
+        psq_l v0yz, 0x4(a), 0, 0
+        psq_l v1yz, 0x4(b), 0, 0
+        ps_sub dyz, v0yz, v1yz
+        psq_l v0xy, 0x0(a), 0, 0
+        psq_l v1xy, 0x0(b), 0, 0
+        ps_mul dyz, dyz, dyz
+        ps_sub dxy, v0xy, v1xy
+        ps_madd sqdist, dxy, dxy, dyz
+        ps_sum0 sqdist, sqdist, dyz, dyz
+    }
+
+    return sqdist;
+}
+#endif
+
+f32 C_VECDistance(const Vec* a, const Vec* b) {
+    return sqrtf(C_VECSquareDistance(a, b));
+}
+
+#ifdef __MWERKS__
+f32 PSVECDistance(const register Vec* a, const register Vec* b) {
+    register f32 v0yz, v1yz, v0xy, v1xy, dyz, dxy;
+    register f32 sqdist, rdist;
+    register f32 nwork0, nwork1;
+    register f32 c_half, c_three, c_zero;
+
+    asm {
+        psq_l v0yz, 0x4(a), 0, 0
+        psq_l v1yz, 0x4(b), 0, 0
+        ps_sub dyz, v0yz, v1yz
+        psq_l v0xy, 0x0(a), 0, 0
+        psq_l v1xy, 0x0(b), 0, 0
+        ps_mul dyz, dyz, dyz
+        ps_sub dxy, v0xy, v1xy
+    }
+
+    c_half  = 0.5f;
+
+    asm {
+        ps_madd sqdist, dxy, dxy, dyz
+        fsubs c_zero, c_half, c_half
+        ps_sum0 sqdist, sqdist, dyz, dyz
+        fcmpu cr0, c_zero, sqdist
+        beq L_00000CBC
+    }
+
+    c_three = 3.0f;
+
+    asm {
+        frsqrte rdist, sqdist
+        fmuls nwork0, rdist, rdist
+        fmuls nwork1, rdist, c_half
+        fnmsubs nwork0, nwork0, sqdist, c_three
+        fmuls rdist, nwork0, nwork1
+        fmuls sqdist, sqdist, rdist
+    L_00000CBC:
+    }
+
+    return sqdist;
+}
+#endif