path: root/src/JSystem/J3DGraphBase/J3DTransform.cpp

//
// Generated by dtk
// Translation Unit: J3DTransform.cpp
//

#include "JSystem/JSystem.h" // IWYU pragma: keep

#include "JSystem/J3DGraphBase/J3DTransform.h"
#include "JSystem/J3DGraphBase/J3DStruct.h"
#include "JSystem/JMath/JMATrigonometric.h"

// This symbol needs to go in .data, but as it's only 8 bytes long, it would normally go in .sdata or .sdata2.
// But if the array doesn't have a size specified in its declaration, and its definition comes *after* it gets used,
// that forces it into .data instead because the size is unknown when it's used. Even const is ignored.
f32 PSMulUnit01[2] = { 0.0f, -1.0f };

const J3DTransformInfo j3dDefaultTransformInfo = {
    { 1.0f, 1.0f, 1.0f },
    { 0, 0, 0 },
    { 0.0f, 0.0f, 0.0f },
};

const Vec j3dDefaultScale = { 1.0f, 1.0f, 1.0f };

const Mtx j3dDefaultMtx = {
    { 1.0f, 0.0f, 0.0f, 0.0f },
    { 0.0f, 1.0f, 0.0f, 0.0f },
    { 0.0f, 0.0f, 1.0f, 0.0f },
};

/* 802DA0A8-802DA0B0       .text __MTGQR7__FUl */
void __MTGQR7(register u32 v) {
#ifdef __MWERKS__
asm {
        mtspr GQR7, v
    }
#endif
}

/* 802DA0B0-802DA0E8       .text J3DGQRSetup7__FUlUlUlUl */
void J3DGQRSetup7(u32 r0, u32 r1, u32 r2, u32 r3) {
    u32 v = (((r0 << 8) + r1) << 16) | ((r2 << 8) + r3);
    __MTGQR7(v);
}

// Enabling scheduling optimization seems to be necessary but just for this
// function.
#pragma scheduling on
/* 802DA0E8-802DA120       .text J3DCalcZValue__FPA4_f3Vec */
f32 J3DCalcZValue(MtxP m, Vec v) {
    return m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3];
}
#pragma scheduling reset

/* 802DA120-802DA2E0       .text J3DCalcBBoardMtx__FPA4_f */
void J3DCalcBBoardMtx(Mtx mtx) {
    f32 sx = std::sqrtf(mtx[0][0]*mtx[0][0] + mtx[1][0]*mtx[1][0] + mtx[2][0]*mtx[2][0]);
    f32 sy = std::sqrtf(mtx[0][1]*mtx[0][1] + mtx[1][1]*mtx[1][1] + mtx[2][1]*mtx[2][1]);
    f32 sz = std::sqrtf(mtx[0][2]*mtx[0][2] + mtx[1][2]*mtx[1][2] + mtx[2][2]*mtx[2][2]);

    mtx[0][0] = sx;
    mtx[0][1] = 0.0f;
    mtx[0][2] = 0.0f;

    mtx[1][0] = 0.0f;
    mtx[1][1] = sy;
    mtx[1][2] = 0.0f;

    mtx[2][0] = 0.0f;
    mtx[2][1] = 0.0f;
    mtx[2][2] = sz;
}

/* 802DA2E0-802DA584       .text J3DCalcYBBoardMtx__FPA4_f */
void J3DCalcYBBoardMtx(Mtx mtx) {
    f32 sx = std::sqrtf(mtx[0][0]*mtx[0][0] + mtx[1][0]*mtx[1][0] + mtx[2][0]*mtx[2][0]);
    f32 sy = std::sqrtf(mtx[0][1]*mtx[0][1] + mtx[1][1]*mtx[1][1] + mtx[2][1]*mtx[2][1]);
    f32 sz = std::sqrtf(mtx[0][2]*mtx[0][2] + mtx[1][2]*mtx[1][2] + mtx[2][2]*mtx[2][2]);

    Vec axisX, axisY, axisZ;

    axisX.x = 1.0f;
    axisX.y = 0.0f;
    axisX.z = 0.0f;

    axisY.x = mtx[0][1];
    axisY.y = mtx[1][1];
    axisY.z = mtx[2][1];

    VECCrossProduct(&axisX, &axisY, &axisZ);
    VECNormalize(&axisY, &axisY);
    VECNormalize(&axisZ, &axisZ);

    mtx[0][0] = axisX.x * sx;
    mtx[0][1] = axisY.x * sy;
    mtx[0][2] = axisZ.x * sz;

    mtx[1][0] = axisX.y * sx;
    mtx[1][1] = axisY.y * sy;
    mtx[1][2] = axisZ.y * sz;

    mtx[2][0] = axisX.z * sx;
    mtx[2][1] = axisY.z * sy;
    mtx[2][2] = axisZ.z * sz;
}

/* 802DA584-802DA64C       .text J3DPSCalcInverseTranspose__FPA4_fPA3_f */
ASM void J3DPSCalcInverseTranspose(register Mtx src, register Mtx33 dst) {
    #ifdef __MWERKS__
        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      non_zero_determinant
        li       r3, 0
        blr

    non_zero_determinant:
        ps_res     f0, f7
        ps_add   f6, f0, f0
        ps_mul   f5, f0, f0
        ps_nmsub f0, f7, f5, f6
        ps_add   f6, f0, f0
        ps_mul   f5, f0, f0
        ps_nmsub f0, f7, f5, f6
        ps_muls0 f13, f13, f0
        ps_muls0 f12, f12, f0
        psq_st   f13, 0(dst), 0, 0
        ps_muls0 f11, f11, f0
        psq_st   f12, 12(dst), 0, 0
        ps_muls0 f10, f10, f0
        psq_st   f11, 24(dst), 0, 0
        ps_muls0 f9, f9, f0
        psq_st   f10, 8(dst), 1, 0
        ps_muls0 f8, f8, f0
        psq_st   f9, 20(r4), 1, 0
        li       r3, 1
        psq_st   f8, 32(r4), 1, 0
        blr
    #endif
}

/* 802DA64C-802DA724       .text J3DGetTranslateRotateMtx__FRC16J3DTransformInfoPA4_f */
void J3DGetTranslateRotateMtx(const J3DTransformInfo& tx, Mtx dst) {
    f32 sx = JMASSin(tx.mRotation.x), cx = JMASCos(tx.mRotation.x);
    f32 sy = JMASSin(tx.mRotation.y), cy = JMASCos(tx.mRotation.y);
    f32 sz = JMASSin(tx.mRotation.z), cz = JMASCos(tx.mRotation.z);

    dst[2][0] = -sy;
    dst[0][0] = cz * cy;
    dst[1][0] = sz * cy;
    dst[2][1] = cy * sx;
    dst[2][2] = cy * cx;

    f32 cxsz = cx * sz;
    f32 sxcz = sx * cz;
    dst[0][1] = sxcz * sy - cxsz;
    dst[1][2] = cxsz * sy - sxcz;

    f32 sxsz = sx * sz;
    f32 cxcz = cx * cz;
    dst[0][2] = cxcz * sy + sxsz;
    dst[1][1] = sxsz * sy + cxcz;

    dst[0][3] = tx.mTranslate.x;
    dst[1][3] = tx.mTranslate.y;
    dst[2][3] = tx.mTranslate.z;
}

/* 802DA724-802DA7E4       .text J3DGetTranslateRotateMtx__FsssfffPA4_f */
void J3DGetTranslateRotateMtx(s16 rx, s16 ry, s16 rz, f32 tx, f32 ty, f32 tz, Mtx dst) {
    f32 sx = JMASSin(rx), cx = JMASCos(rx);
    f32 sy = JMASSin(ry), cy = JMASCos(ry);
    f32 sz = JMASSin(rz), cz = JMASCos(rz);

    dst[2][0] = -sy;
    dst[0][0] = cz * cy;
    dst[1][0] = sz * cy;
    dst[2][1] = cy * sx;
    dst[2][2] = cy * cx;

    f32 cxsz = cx * sz;
    f32 sxcz = sx * cz;
    dst[0][1] = sxcz * sy - cxsz;
    dst[1][2] = cxsz * sy - sxcz;

    f32 sxsz = sx * sz;
    f32 cxcz = cx * cz;
    dst[0][2] = cxcz * sy + sxsz;
    dst[1][1] = sxsz * sy + cxcz;

    dst[0][3] = tx;
    dst[1][3] = ty;
    dst[2][3] = tz;
}

/* 802DA7E4-802DA8A8       .text J3DGetTextureMtx__FRC17J3DTextureSRTInfo3VecPA4_f */
void J3DGetTextureMtx(const J3DTextureSRTInfo& srt, Vec center, Mtx dst) {
    f32 sr = JMASSin(srt.mRotation), cr = JMASCos(srt.mRotation);

    f32 cx = srt.mScaleX * cr;
    f32 sx = srt.mScaleX * sr;
    f32 sy = srt.mScaleY * sr;
    f32 cy = srt.mScaleY * cr;

    dst[0][0] = cx;
    dst[0][1] = -sx;
    dst[0][2] = (-cx * center.x + sx * center.y) + center.x + srt.mTranslationX;

    dst[1][0] = sy;
    dst[1][1] = cy;
    dst[1][2] = (-sy * center.x - cy * center.y) + center.y + srt.mTranslationY;

    dst[2][3] = 0.0f;
    dst[2][1] = 0.0f;
    dst[2][0] = 0.0f;
    dst[1][3] = 0.0f;
    dst[0][3] = 0.0f;
    dst[2][2] = 1.0f;
}

/* 802DA8A8-802DA96C       .text J3DGetTextureMtxOld__FRC17J3DTextureSRTInfo3VecPA4_f */
void J3DGetTextureMtxOld(const J3DTextureSRTInfo& srt, Vec center, Mtx dst) {
    f32 sr = JMASSin(srt.mRotation), cr = JMASCos(srt.mRotation);

    f32 cx = srt.mScaleX * cr;
    f32 sx = srt.mScaleX * sr;
    f32 sy = srt.mScaleY * sr;
    f32 cy = srt.mScaleY * cr;

    dst[0][0] = cx;
    dst[0][1] = -sx;
    dst[0][3] = (-cx * center.x + sx * center.y) + center.x + srt.mTranslationX;

    dst[1][0] = sy;
    dst[1][1] = cy;
    dst[1][3] = (-sy * center.x - cy * center.y) + center.y + srt.mTranslationY;

    dst[2][3] = 0.0f;
    dst[2][1] = 0.0f;
    dst[2][0] = 0.0f;
    dst[1][2] = 0.0f;
    dst[0][2] = 0.0f;
    dst[2][2] = 1.0f;
}

/* 802DA96C-802DAA38       .text J3DGetTextureMtxMaya__FRC17J3DTextureSRTInfoPA4_f */
void J3DGetTextureMtxMaya(const J3DTextureSRTInfo& srt, Mtx dst) {
    f32 sr = JMASSin(srt.mRotation), cr = JMASCos(srt.mRotation);
    f32 tx = srt.mTranslationX - 0.5f;
    f32 ty = srt.mTranslationY - 0.5f;

    dst[0][0] = srt.mScaleX * cr;
    dst[0][1] = srt.mScaleY * sr;
    dst[0][2] = tx * cr - sr * (ty + srt.mScaleY) + 0.5f;

    dst[1][0] = -srt.mScaleX * sr;
    dst[1][1] = srt.mScaleY * cr;
    dst[1][2] = -tx * sr - cr * (ty + srt.mScaleY) + 0.5f;

    dst[2][3] = 0.0f;
    dst[2][1] = 0.0f;
    dst[2][0] = 0.0f;
    dst[1][3] = 0.0f;
    dst[0][3] = 0.0f;
    dst[2][2] = 1.0f;
}

/* 802DAA38-802DAB04       .text J3DGetTextureMtxMayaOld__FRC17J3DTextureSRTInfoPA4_f */
void J3DGetTextureMtxMayaOld(const J3DTextureSRTInfo& srt, Mtx dst) {
    f32 sr = JMASSin(srt.mRotation), cr = JMASCos(srt.mRotation);
    f32 tx = srt.mTranslationX - 0.5f;
    f32 ty = srt.mTranslationY - 0.5f;

    dst[0][0] = srt.mScaleX * cr;
    dst[0][1] = srt.mScaleY * sr;
    dst[0][3] = tx * cr - sr * (ty + srt.mScaleY) + 0.5f;

    dst[1][0] = -srt.mScaleX * sr;
    dst[1][1] = srt.mScaleY * cr;
    dst[1][3] = -tx * sr - cr * (ty + srt.mScaleY) + 0.5f;

    dst[2][3] = 0.0f;
    dst[2][1] = 0.0f;
    dst[2][0] = 0.0f;
    dst[1][2] = 0.0f;
    dst[0][2] = 0.0f;
    dst[2][2] = 1.0f;
}

/* 802DAB04-802DAB68       .text J3DScaleNrmMtx__FPA4_fRC3Vec */
void J3DScaleNrmMtx(register Mtx mtx, const register Vec& scl) {
    register f32 mtx_xy, mtx_z_, scl_xy, scl_z_;

#ifdef __MWERKS__
asm {
        /* Row 0 */
        psq_l  scl_xy, 0(scl), 0, 0
        psq_l  mtx_xy, 0(mtx), 0, 0
        lfs    scl_z_, 8(scl)
        lfs    mtx_z_, 8(mtx)
        ps_mul f4, mtx_xy, scl_xy
        psq_st f4, 0(mtx), 0, 0
        fmuls  f4, mtx_z_, scl_z_
        stfs   f4, 8(mtx)

        /* Row 1 */
        psq_l  scl_xy, 0(scl), 0, 0
        psq_l  mtx_xy, 16(mtx), 0, 0
        lfs    scl_z_, 8(scl)
        lfs    mtx_z_, 24(mtx)
        ps_mul f4, mtx_xy, scl_xy
        psq_st f4, 16(mtx), 0, 0
        fmuls  f4, mtx_z_, scl_z_
        stfs   f4, 24(mtx)

        /* Row 2 */
        psq_l  scl_xy, 0(scl), 0, 0
        psq_l  mtx_xy, 32(mtx), 0, 0
        lfs    scl_z_, 8(scl)
        lfs    mtx_z_, 40(mtx)
        ps_mul f4, mtx_xy, scl_xy
        psq_st f4, 32(mtx), 0, 0
        fmuls  f4, mtx_z_, scl_z_
        stfs   f4, 40(mtx)
    }
#endif
}

/* 802DAB68-802DABBC       .text J3DScaleNrmMtx33__FPA3_fRC3Vec */
void J3DScaleNrmMtx33(register Mtx33 mtx, const register Vec& scl) {
    register f32 mtx0_xy, mtx0_z_;
    register f32 mtx1_xy, mtx1_z_;
    register f32 mtx2_xy, mtx2_z_;
    register f32 scl__xy, scl__z_;

#ifdef __MWERKS__
asm {
        psq_l  mtx0_xy, 0(mtx), 0, 0
        psq_l  scl__xy, 0(scl), 0, 0
        lfs    mtx0_z_, 8(mtx)
        lfs    scl__z_, 8(scl)
        ps_mul mtx0_xy, mtx0_xy, scl__xy
        psq_l  mtx1_xy, 12(mtx), 0, 0
        fmuls  mtx0_z_, mtx0_z_, scl__z_
        lfs    mtx1_z_, 20(mtx)
        ps_mul mtx1_xy, mtx1_xy, scl__xy
        psq_l  mtx2_xy, 24(mtx), 0, 0
        fmuls  mtx1_z_, mtx1_z_, scl__z_
        lfs    mtx2_z_, 32(mtx)
        ps_mul mtx2_xy, mtx2_xy, scl__xy
        psq_st mtx0_xy, 0(mtx), 0, 0
        fmuls  mtx2_z_, mtx2_z_, scl__z_
        stfs   mtx0_z_, 8(mtx)
        psq_st mtx1_xy, 12(mtx), 0, 0
        stfs   mtx1_z_, 20(mtx)
        psq_st mtx2_xy, 24(mtx), 0, 0
        stfs   mtx2_z_, 32(mtx)
    }
#endif
}

/* 802DABBC-802DACE0       .text J3DMtxProjConcat__FPA4_fPA4_fPA4_f */
void J3DMtxProjConcat(register Mtx a, register Mtx b, register Mtx dst) {
    /* Nonmatching */
    #ifdef __MWERKS__
    asm {
        psq_l      f2, 0(a), 0, 0
        psq_l      f3, 8(a), 0, 0
        ps_merge00 f6, f2, f2
        ps_merge11 f7, f2, f2
        ps_merge00 f8, f3, f3
        ps_merge11 f9, f3, f3
        psq_l      f10, 0(b), 0, 0
        psq_l      f11, 0x10(b), 0, 0
        psq_l      f12, 0x20(b), 0, 0
        psq_l      f13, 0x30(b), 0, 0
        ps_mul     f0, f6, f10
        ps_madd    f0, f7, f11, f0
        ps_madd    f0, f8, f12, f0
        ps_madd    f0, f9, f13, f0
        // dest[0][0] and dest[0][1]
        psq_st     f0, 0(dst), 0, 0
        psq_l      f10, 0x8(b), 0, 0
        psq_l      f11, 0x18(b), 0, 0
        psq_l      f12, 0x28(b), 0, 0
        psq_l      f13, 0x38(b), 0, 0
        ps_mul     f0, f6, f10
        ps_madd    f0, f7, f11, f0
        ps_madd    f0, f8, f12, f0
        ps_madd    f0, f9, f13, f0
        // dest[0][2] and dest[0][3]
        psq_st     f0, 0x8(dst), 0, 0
        psq_l      f2, 0x10(a), 0, 0
        psq_l      f3, 0x18(a), 0, 0
        ps_merge00 f6, f2, f2
        ps_merge11 f7, f2, f2
        ps_merge00 f8, f3, f3
        ps_merge11 f9, f3, f3
        psq_l      f10, 0(b), 0, 0
        psq_l      f11, 0x10(b), 0, 0
        psq_l      f12, 0x20(b), 0, 0
        psq_l      f13, 0x30(b), 0, 0
        ps_mul     f0, f6, f10
        ps_madd    f0, f7, f11, f0
        ps_madd    f0, f8, f12, f0
        ps_madd    f0, f9, f13, f0
        // dest[1][0] and dest[1][1]
        psq_st     f0, 0x10(dst), 0, 0
        psq_l      f10, 0x8(b), 0, 0
        psq_l      f11, 0x18(b), 0, 0
        psq_l      f12, 0x28(b), 0, 0
        psq_l      f13, 0x38(b), 0, 0
        ps_mul     f0, f6, f10
        ps_madd    f0, f7, f11, f0
        ps_madd    f0, f8, f12, f0
        ps_madd    f0, f9, f13, f0
        // dest[1][2] and dest[1][3]
        psq_st     f0, 0x18(dst), 0, 0
        psq_l      f2, 0x20(a), 0, 0
        psq_l      f3, 0x28(a), 0, 0
        ps_merge00 f6, f2, f2
        ps_merge11 f7, f2, f2
        ps_merge00 f8, f3, f3
        ps_merge11 f9, f3, f3
        psq_l      f10, 0(b), 0, 0
        psq_l      f11, 0x10(b), 0, 0
        psq_l      f12, 0x20(b), 0, 0
        psq_l      f13, 0x30(b), 0, 0
        ps_mul     f0, f6, f10
        ps_madd    f0, f7, f11, f0
        ps_madd    f0, f8, f12, f0
        ps_madd    f0, f9, f13, f0
        // dest[2][0] and dest[2][1]
        psq_st     f0, 0x20(dst), 0, 0
        psq_l      f10, 0x8(b), 0, 0
        psq_l      f11, 0x18(b), 0, 0
        psq_l      f12, 0x28(b), 0, 0
        psq_l      f13, 0x38(b), 0, 0
        ps_mul     f0, f6, f10
        ps_madd    f0, f7, f11, f0
        ps_madd    f0, f8, f12, f0
        ps_madd    f0, f9, f13, f0
        // dest[2][2] and dest[2][3]
        psq_st     f0, 0x28(dst), 0, 0
        }
    #endif
}

/* 802DACE0-802DAD0C       .text J3DPSMtx33Copy__FPA3_fPA3_f */
void J3DPSMtx33Copy(register Mtx3P src, register Mtx3P dst) {
    register f32 x1_y1;
    register f32 z1_x2;
    register f32 y2_z2;
    register f32 x3_y3;
    register f32 z3;

#ifdef __MWERKS__
asm {
        psq_l x1_y1, 0(src), 0, 0
        psq_l z1_x2, 8(src), 0, 0
        psq_l y2_z2, 16(src), 0, 0
        psq_l x3_y3, 24(src), 0, 0
        lfs z3, 32(src)
        psq_st x1_y1, 0(dst), 0, 0
        psq_st z1_x2, 8(dst), 0, 0
        psq_st y2_z2, 16(dst), 0, 0
        psq_st x3_y3, 24(dst), 0, 0
        stfs z3, 32(dst)
    }
#endif
}

/* 802DAD0C-802DAD40       .text J3DPSMtx33CopyFrom34__FPA4_fPA3_f */
void J3DPSMtx33CopyFrom34(register MtxP src, register Mtx3P dst) {
#ifdef __MWERKS__
asm {
        psq_l  f0, 0(src), 0, 0
        psq_st f0, 0(dst), 0, 0
        lfs    f1, 8(src)
        stfs   f1, 8(dst)
        psq_l  f2, 16(src), 0, 0
        psq_st f2, 12(dst), 0, 0
        lfs    f3, 24(src)
        stfs   f3, 20(dst)
        psq_l  f4, 32(src), 0, 0
        psq_st f4, 24(dst), 0, 0
        lfs    f5, 40(src)
        stfs   f5, 32(dst)
    }
#endif
}

static f32 Unit01[2] = { 0.0f, 1.0f };

/* 802DAD40-802DAE1C       .text J3DPSMtxArrayConcat__FPA4_fPA4_fPA4_fUl */
ASM void J3DPSMtxArrayConcat(register Mtx mA, register Mtx mB, register Mtx mAB, register u32 count) {
#ifdef __MWERKS__
    nofralloc

    stwu sp, -0x40(sp)
    stfd fp14, 0x08(sp)
    addis r7, 0, Unit01@ha
    stfd fp15, 0x10(sp)
    addi r7, r7, Unit01@l
    stfd fp31,  0x28(sp)
    subi mB,  mB, 0x08
    subi mAB, mAB, 0x08
    mtctr count
loop:
    psq_l fp0, 0x00(mA), 0, 0
    psq_l fp6, 0x08(mB), 0, 0
    psq_l fp7, 0x10(mB), 0, 0
    psq_l fp8, 0x18(mB), 0, 0
    ps_muls0 fp12, fp6, fp0
    psq_l fp2, 0x10(mA), 0, 0
    ps_muls0 fp13, fp7, fp0
    psq_l fp31, 0x00(r7), 0, 0
    ps_muls0 fp14, fp6, fp2
    psq_l fp9, 0x20(mB), 0, 0
    ps_muls0 fp15, fp7, fp2
    psq_l fp1, 0x08(mA), 0, 0
    ps_madds1 fp12, fp8, fp0, fp12
    psq_l fp3, 0x18(mA), 0, 0
    ps_madds1 fp14, fp8, fp2, fp14
    psq_l fp10, 0x28(mB), 0, 0
    ps_madds1 fp13, fp9, fp0, fp13
    psq_lu fp11, 0x30(mB), 0, 0
    ps_madds1 fp15, fp9, fp2, fp15
    psq_l fp4, 0x20(mA), 0, 0
    psq_l fp5, 0x28(mA), 0, 0
    ps_madds0 fp12, fp10, fp1, fp12
    ps_madds0 fp13, fp11, fp1, fp13
    ps_madds0 fp14, fp10, fp3, fp14
    ps_madds0 fp15, fp11, fp3, fp15
    psq_st fp12, 0x08(mAB), 0, 0

    ps_muls0 fp2, fp6, fp4
    ps_madds1 fp13, fp31, fp1, fp13
    ps_muls0 fp0, fp7, fp4
    psq_st fp14, 0x18(mAB), 0, 0
    ps_madds1 fp15, fp31, fp3, fp15

    psq_st fp13, 0x10(mAB), 0, 0

    ps_madds1 fp2, fp8, fp4, fp2
    ps_madds1 fp0, fp9, fp4, fp0
    ps_madds0 fp2, fp10, fp5, fp2
    psq_st fp15, 0x20(mAB), 0, 0
    ps_madds0 fp0, fp11, fp5, fp0
    psq_st fp2, 0x28(mAB), 0, 0
    ps_madds1 fp0, fp31, fp5, fp0
    psq_stu fp0, 0x30(mAB), 0, 0

    bdnz loop

    lfd fp14, 0x08(sp)
    lfd fp15, 0x10(sp)
    lfd fp31, 0x28(sp)
    addi sp, sp, 0x40
    blr
#endif
}