Wii MatrixStack class

#ifndef _MATRIX_STACK_H_
#define _MATRIX_STACK_H_

#include <stack>
#include <cstring>
#include <gccore.h>

struct Mtx_s {
    Mtx data;
}__attribute__ ((packed));

class MatrixStack
{
public:
    MatrixStack() {Identity();}

    void Identity()
    {
        guMtxIdentity(current_matrix.data);
    }

    void Translate(const guVector &v)
    {
        Mtx tmp;
        guMtxTrans(tmp, v.x, v.y, v.z);
        guMtxConcat(current_matrix.data, tmp, current_matrix.data);
    }

    void Rotate(guVector &v, float angle)
    {
        guMtxRotAxisRad(current_matrix.data, &v, angle);
    }

    void Rotate_X(float angle)
    {
        Mtx tmp;
        guMtxRotRad(tmp, 'x', angle);
        guMtxConcat(current_matrix.data, tmp, current_matrix.data);
    }

    void Rotate_Y(float angle)
    {
        Mtx tmp;
        guMtxRotRad(tmp, 'y', angle);
        guMtxConcat(current_matrix.data, tmp, current_matrix.data);
    }

    void Rotate_Z(float angle)
    {
        Mtx tmp;
        guMtxRotRad(tmp, 'z', angle);
        guMtxConcat(current_matrix.data, tmp, current_matrix.data);
    }

    void Scale(const guVector &v)
    {
        guMtxScale(current_matrix.data, v.x, v.y, v.z);
    }

    struct Mtx_s Top()
    {
        return current_matrix;
    }

    void copyMatrix(Mtx m)
    {
        memcpy(current_matrix.data, m, sizeof(Mtx));
    }

    void Push()
    {
        stack_matrix.push(current_matrix);
    }

    void Pop()
    {
        current_matrix = stack_matrix.top();
        stack_matrix.pop();
    }

    void setMatrix(u32 pnidx)
    {
        GX_LoadPosMtxImm(current_matrix.data, pnidx);
    }

    MtxP const value_ptr()
    {
        return (MtxP)current_matrix.data;
    }

private:
    struct Mtx_s current_matrix;
    std::stack<struct Mtx_s> stack_matrix;
};



#endif