Citro3D testing

#include <iostream>
#include <cmath>
#include <cstring>

using namespace std;


 #include <stdbool.h>
 #include <stdint.h>
 #define M_PI 3.14159265358979323846 /* pi */
 typedef uint8_t u8;
 typedef uint32_t u32;

 typedef u32 C3D_IVec;

 static inline C3D_IVec IVec_Pack(u8 x, u8 y, u8 z, u8 w)
 {
     return (u32)x | ((u32)y << 8) | ((u32)z << 16) | ((u32)w << 24);
 }

 typedef union
 {
     struct
     {
         float w;
         float z;
         float y;
         float x;
     };

     struct
     {
         float r;
         float k;
         float j;
         float i;
     };

     float c[4];
 } C3D_FVec;

 typedef C3D_FVec C3D_FQuat;

 typedef union
 {
     C3D_FVec r[4];
     float m[4*4];
 } C3D_Mtx;


 static inline C3D_FVec FVec4_New(float x, float y, float z, float w)
 {
     return (C3D_FVec){{ w, z, y, x }};
 }

 static inline C3D_FVec FVec3_New(float x, float y, float z)
 {
     return FVec4_New(x, y, z, 0.0f);
 }

 static inline float FVec4_Dot(C3D_FVec lhs, C3D_FVec rhs)
 {
     // A∙B = sum of component-wise products
     return lhs.x*rhs.x + lhs.y*rhs.y + lhs.z*rhs.z + lhs.w*rhs.w;
 }

 static inline float FVec3_Dot(C3D_FVec lhs, C3D_FVec rhs)
 {
     // A∙B = sum of component-wise products
     return lhs.x*rhs.x + lhs.y*rhs.y + lhs.z*rhs.z;
 }

 static inline float FVec3_Magnitude(C3D_FVec v)
 {
     // ‖v‖ = √(v∙v)
     return sqrtf(FVec3_Dot(v,v));
 }

 static inline C3D_FVec FVec3_Normalize(C3D_FVec v)
 {
     // get vector magnitude
     float m = FVec3_Magnitude(v);

     // scale by inverse magnitude to get a unit vector
     return FVec3_New(v.x/m, v.y/m, v.z/m);
 }

 void Mtx_Translate(C3D_Mtx* mtx, float x, float y, float z, bool bRightSide)
 {

     C3D_FVec v = FVec4_New(x, y, z, 1.0f);
     int i, j;

     if (bRightSide)
     {
         for (i = 0; i < 4; ++i)
             mtx->r[i].w = FVec4_Dot(mtx->r[i], v);
     }
     else
     {
         for (j = 0; j < 3; ++j)
             for (i = 0; i < 4; ++i)
                 mtx->r[j].c[i] += mtx->r[3].c[i] * v.c[3-j];
     }

 }

 void gl_print_matrix(const C3D_Mtx *m) {
    for (int i = 0; i < 16; i+=4) {
        fprintf(stderr, "%f\t%f\t%f\t%f\n", m->m[i+3], m->m[i+2], m->m[i+1], m->m[i]);
    }
    fprintf(stderr, "\n");
}

 void Mtx_Transpose(C3D_Mtx* out)
 {
     float swap;
     for (int i = 0; i <= 2; i++)
     {
         for (int j = 2-i; j >= 0; j--)
         {
             swap = out->r[i].c[j];
             out->r[i].c[j] = out->r[3-j].c[3-i];
             out->r[3-j].c[3-i] = swap;
         }
     }
 }

 void Mtx_Rotate(C3D_Mtx* mtx, C3D_FVec axis, float angle, bool bRightSide)
 {
     size_t  i;
     C3D_Mtx om;

     float s = sinf(angle);
     float c = cosf(angle);
     float t = 1.0f - c;

     axis = FVec3_Normalize(axis);

     float x = axis.x;
     float y = axis.y;
     float z = axis.z;
     float w;

     om.r[0].x = t*x*x + c;
     om.r[1].x = t*x*y + s*z;
     om.r[2].x = t*x*z - s*y;
     om.r[3].x = 0.0f; //optimized out

     om.r[0].y = t*y*x - s*z;
     om.r[1].y = t*y*y + c;
     om.r[2].y = t*y*z + s*x;
     om.r[3].y = 0.0f; //optimized out

     om.r[0].z = t*z*x + s*y;
     om.r[1].z = t*z*y - s*x;
     om.r[2].z = t*z*z + c;
     om.r[3].z = 0.0f; //optimized out
      /* optimized out*/
     om.r[0].w = 0.0f;
     om.r[1].w = 0.0f;
     om.r[2].w = 0.0f;
     om.r[3].w = 1.0f;


     if (bRightSide)
     {
         for (i = 0; i < 4; ++i)
         {
             x = mtx->r[i].x*om.r[0].x + mtx->r[i].y*om.r[1].x + mtx->r[i].z*om.r[2].x;
             y = mtx->r[i].x*om.r[0].y + mtx->r[i].y*om.r[1].y + mtx->r[i].z*om.r[2].y;
             z = mtx->r[i].x*om.r[0].z + mtx->r[i].y*om.r[1].z + mtx->r[i].z*om.r[2].z;

             mtx->r[i].x = x;
             mtx->r[i].y = y;
             mtx->r[i].z = z;
         }
     }
     else
     {
         for (i = 0; i < 3; ++i)
         {
             x = mtx->r[0].x*om.r[i].x + mtx->r[1].x*om.r[i].y + mtx->r[2].x*om.r[i].z;
             y = mtx->r[0].y*om.r[i].x + mtx->r[1].y*om.r[i].y + mtx->r[2].y*om.r[i].z;
             z = mtx->r[0].z*om.r[i].x + mtx->r[1].z*om.r[i].y + mtx->r[2].z*om.r[i].z;
             w = mtx->r[0].w*om.r[i].x + mtx->r[1].w*om.r[i].y + mtx->r[2].w*om.r[i].z;

             om.r[i].x = x;
             om.r[i].y = y;
             om.r[i].z = z;
             om.r[i].w = w;
         }

         for (i = 0; i < 3; ++i)
             mtx->r[i] = om.r[i];
     }
 }

 void Mtx_RotateZ(C3D_Mtx* mtx, float angle, bool bRightSide)
 {
     float  a, b;
     float  cosAngle = cosf(angle);
     float  sinAngle = sinf(angle);
     size_t i;

     if (bRightSide)
     {
         for (i = 0; i < 4; ++i)
         {
             a = mtx->r[i].x*cosAngle + mtx->r[i].y*sinAngle;
             b = mtx->r[i].y*cosAngle - mtx->r[i].x*sinAngle;
             mtx->r[i].x = a;
             mtx->r[i].y = b;
         }
     }
     else
     {
         for (i = 0; i < 4; ++i)
         {
             a = mtx->r[0].c[i]*cosAngle - mtx->r[1].c[i]*sinAngle;
             b = mtx->r[1].c[i]*cosAngle + mtx->r[0].c[i]*sinAngle;
             mtx->r[0].c[i] = a;
             mtx->r[1].c[i] = b;
         }
     }
 }

int main() {
    C3D_Mtx mtx = {12,8,4,0,13,9,5,1,14,10,6,2,15,11,7,3};
    gl_print_matrix(&mtx);

    C3D_Mtx mtx2 = {12,8,4,0,13,9,5,1,14,10,6,2,15,11,7,3};
    Mtx_Translate(&mtx2, 10, 20, 30, true);
    gl_print_matrix(&mtx2);

    C3D_Mtx mtx3 = {12,8,4,0,13,9,5,1,14,10,6,2,15,11,7,3};
    C3D_FVec vec = FVec3_New(0.5,0.8,1);
    float angle = (float)(30 * (float)M_PI / 180.0);
    Mtx_Rotate(&mtx3, vec, angle, true);
    gl_print_matrix(&mtx3);

    C3D_Mtx mtx4 = {12,8,4,0,13,9,5,1,14,10,6,2,15,11,7,3};
    Mtx_RotateZ(&mtx4, angle, true);
    gl_print_matrix(&mtx4);


    return 0;

}