#include <stdio.h>
#include <math.h>

#include "math_matrix.h"
#include "math_vector3D.h"

Cmatrix::Cmatrix()
{
    Zeros();
}

Cmatrix::Cmatrix(float e11, float e12, float e13, 
                 float e21, float e22, float e23, 
                 float e31, float e32, float e33)
{
    element[0][0] = e11;
    element[0][1] = e12;
    element[0][2] = e13;

    element[1][0] = e21;
    element[1][1] = e22;
    element[1][2] = e23;

    element[2][0] = e31;
    element[2][1] = e32;
    element[2][2] = e33;
}


Cmatrix::~Cmatrix()
{
}


Cmatrix Cmatrix::operator+ (const Cmatrix &m)
{
	Cmatrix mnew;

    for (int j=0; j<3; j++)
    {
        for (int i=0; i<3; i++)
        {
            mnew.element[i][j] = this->element[i][j] + m.element[i][j];
        }
    }

	return mnew;
}

Cmatrix Cmatrix::operator* (const Cmatrix &m)
{
    Cmatrix mnew;

    for (int j=0; j<3; j++)
    {
        for (int i=0; i<3; i++)
        {
            for (int k=0; k<3; k++)
            {
                mnew.element[i][j] += this->element[i][k] * m.element[k][j];
            }
        }
    }

	return mnew;
}

Cvector Cmatrix::mul_vec(Cvector &v)
{
	Cvector v2;

	v2.x = element[0][0] * v.x + element[0][1] * v.y + element[0][2] * v.z;
	v2.y = element[1][0] * v.x + element[1][1] * v.y + element[1][2] * v.z;
	v2.z = element[2][0] * v.x + element[2][1] * v.y + element[2][2] * v.z;

	return v2;
}

void Cmatrix::Zeros()
{
    for (int j=0; j<3; j++)
        for (int i=0; i<3; i++)
            element[i][j] = 0;
}

void Cmatrix::Ones()
{
    for (int j=0; j<3; j++)
        for (int i=0; i<3; i++)
            element[i][j] = 1;
}

void Cmatrix::Identity()
{
    for (int j=0; j<3; j++)
        for (int i=0; i<3; i++)
            if (i==j)
                element[i][j] = 1;
            else
                element[i][j] = 0;            
}

void Cmatrix::PrintMatrix()
{
    for (int i=0; i<3; i++)
        printf("%f   %f   %f\n", element[i][0], element[i][1], element[i][2]);
    printf("\n");
}

void Cmatrix::set_rot_matrix(float rot_x, float rot_y, float rot_z)
{
	Cmatrix rot_matrix;

	float c_rx = cos(rot_x);
	float c_ry = cos(rot_y);
	float c_rz = cos(rot_z);
	float s_rx = sin(rot_x);
	float s_ry = sin(rot_y);
	float s_rz = sin(rot_z);

	Cmatrix rot_matrix_X(	1,	0,		0, 
							0,	c_rx,	-s_rx, 
							0,	s_rx,	c_rx);

	Cmatrix rot_matrix_Y(	c_ry,	0,		s_ry, 
							0,		1,		0, 
							-s_ry,	0,		c_ry);

	Cmatrix rot_matrix_Z(	c_rz,	-s_rz,	0, 
							s_rz,	c_rz,	0, 
							0,		0,		1);

	rot_matrix = rot_matrix_X * rot_matrix_Y * rot_matrix_Z;

    for (int j=0; j<3; j++)
        for (int i=0; i<3; i++)
            element[i][j] = rot_matrix.element[i][j];
}
