Matrix and Vector functions

void IdentityMatrix(float *m)
{
    m[0] = 1.0f; m[4] = 0.0f; m[8] =  0.0f; m[12] =  0.0f;
    m[1] = 0.0f; m[5] = 1.0f; m[9] =  0.0f; m[13] =  0.0f;
    m[2] = 0.0f; m[6] = 0.0f; m[10] = 1.0f; m[14] =  0.0f;
    m[3] = 0.0f; m[7] = 0.0f; m[11] = 0.0f; m[15] =  1.0f;
}

void TranslationMatrix(float *m, float x, float y, float z)
{
    m[0] = 1.0f; m[4] = 0.0f; m[8] =  0.0f;  m[12] =  x;
    m[1] = 0.0f; m[5] = 1.0f; m[9] =  0.0f;  m[13] =  y;
    m[2] = 0.0f; m[6] = 0.0f; m[10] =  1.0f; m[14] =  z;
    m[3] = 0.0f; m[7] = 0.0f; m[11] =  0.0f; m[15] =  1.0f;
}

void RotationMatrix_X(float *m, float r)
{
    float c = cosf(r);
    float s = sinf(r);
    m[0] = 1.0f; m[4] = 0.0f; m[8] =  0.0f;  m[12] =  0.0f;
    m[1] = 0.0f; m[5] = c;    m[9] =  -s;    m[13] =  0.0f;
    m[2] = 0.0f; m[6] = s;    m[10] =  c;    m[14] =  0.0f;
    m[3] = 0.0f; m[7] = 0.0f; m[11] =  0.0f; m[15] =  1.0f;
}

void RotationMatrix_Y(float *m, float r)
{
    float c = cosf(r);
    float s = sinf(r);
    m[0] = c;    m[4] = 0.0f; m[8] =  s;     m[12] =  0.0f;
    m[1] = 0.0f; m[5] = 1.0f; m[9] =  0.0f;  m[13] =  0.0f;
    m[2] = -s;   m[6] = 0.0f; m[10] =  c;    m[14] =  0.0f;
    m[3] = 0.0f; m[7] = 0.0f; m[11] =  0.0f; m[15] =  1.0f;
}

void RotationMatrix_Z(float *m, float r)
{
    float c = cosf(r);
    float s = sinf(r);
    m[0] = c;    m[4] = -s;   m[8] =  0.0f;  m[12] =  0.0f;
    m[1] = s;    m[5] = c;    m[9] =  0.0f;  m[13] =  0.0f;
    m[2] = 0.0f; m[6] = 0.0f; m[10] =  1.0f; m[14] =  0.0f;
    m[3] = 0.0f; m[7] = 0.0f; m[11] =  0.0f; m[15] =  1.0f;
}

void MultiplyMatrix(float *_dest, float *_m1, float *_m2)
{
    float (*dest)[4] = (float (*)[4])_dest;
    float (*m1)[4]   = (float (*)[4])_m1;
    float (*m2)[4]   = (float (*)[4])_m2;
    dest[0][0] = m1[0][0] * m2[0][0] + m1[0][1] * m2[1][0] + m1[0][2] * m2[2][0] + m1[0][3] * m2[3][0];
    dest[0][1] = m1[0][0] * m2[0][1] + m1[0][1] * m2[1][1] + m1[0][2] * m2[2][1] + m1[0][3] * m2[3][1];
    dest[0][2] = m1[0][0] * m2[0][2] + m1[0][1] * m2[1][2] + m1[0][2] * m2[2][2] + m1[0][3] * m2[3][2];
    dest[0][3] = m1[0][0] * m2[0][3] + m1[0][1] * m2[1][3] + m1[0][2] * m2[2][3] + m1[0][3] * m2[3][3];

    dest[1][0] = m1[1][0] * m2[0][0] + m1[1][1] * m2[1][0] + m1[1][2] * m2[2][0] + m1[1][3] * m2[3][0];
    dest[1][1] = m1[1][0] * m2[0][1] + m1[1][1] * m2[1][1] + m1[1][2] * m2[2][1] + m1[1][3] * m2[3][1];
    dest[1][2] = m1[1][0] * m2[0][2] + m1[1][1] * m2[1][2] + m1[1][2] * m2[2][2] + m1[1][3] * m2[3][2];
    dest[1][3] = m1[1][0] * m2[0][3] + m1[1][1] * m2[1][3] + m1[1][2] * m2[2][3] + m1[1][3] * m2[3][3];

    dest[2][0] = m1[2][0] * m2[0][0] + m1[2][1] * m2[1][0] + m1[2][2] * m2[2][0] + m1[2][3] * m2[3][0];
    dest[2][1] = m1[2][0] * m2[0][1] + m1[2][1] * m2[1][1] + m1[2][2] * m2[2][1] + m1[2][3] * m2[3][1];
    dest[2][2] = m1[2][0] * m2[0][2] + m1[2][1] * m2[1][2] + m1[2][2] * m2[2][2] + m1[2][3] * m2[3][2];
    dest[2][3] = m1[2][0] * m2[0][3] + m1[2][1] * m2[1][3] + m1[2][2] * m2[2][3] + m1[2][3] * m2[3][3];

    dest[3][0] = m1[3][0] * m2[0][0] + m1[3][1] * m2[1][0] + m1[3][2] * m2[2][0] + m1[3][3] * m2[3][0];
    dest[3][1] = m1[3][0] * m2[0][1] + m1[3][1] * m2[1][1] + m1[3][2] * m2[2][1] + m1[3][3] * m2[3][1];
    dest[3][2] = m1[3][0] * m2[0][2] + m1[3][1] * m2[1][2] + m1[3][2] * m2[2][2] + m1[3][3] * m2[3][2];
    dest[3][3] = m1[3][0] * m2[0][3] + m1[3][1] * m2[1][3] + m1[3][2] * m2[2][3] + m1[3][3] * m2[3][3];

}

void CopyMatrix(float *src, float *dst)
{
    memcpy(dst, src, sizeof(float) * 16);
}

void CameraMatrix(float *m, float *right, float *up, float *forward)
{
    m[0] = right[0]; m[4] = up[0]; m[8] =  forward[0]; m[12] =  0.0f;
    m[1] = right[1]; m[5] = up[1]; m[9] =  forward[1]; m[13] =  0.0f;
    m[2] = right[2]; m[6] = up[2]; m[10] = forward[2]; m[14] =  0.0f;
    m[3] = 0.0f;     m[7] = 0.0f;  m[11] = 0.0f;       m[15] =  1.0f;
}

void LookUpMatrix(float *m, float *eye, float *center, float *up)
{
    float forward[3] = {center[0] - eye[0], center[1] - eye[1], center[2] - eye[2]};
    float right[3];
    CrossProductVector(right, up, forward);
    CameraMatrix(m, right, up, forward);
}

void CrossProductVector(float *dest, float *v1, float *v2)
{
    dest[0] = v1[1] * v2[2] - v1[2] * v2[1];
    dest[1] = v1[2] * v2[0] - v1[0] * v2[2];
    dest[2] = v1[0] * v2[1] - v1[1] * v2[0];
}

float DotProductVector(float *v1, float *v2)
{
    return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}

float LengthVector(float *v)
{
    return sqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
}

void NormalizeVector(float *src, float *dst)
{
    float length = LengthVector(src);
    dst[0] = dst[0]/length;
    dst[1] = dst[1]/length;
    dst[2] = dst[2]/length;
}