#include "matrices.H"

Matrix::Matrix(void)
{
    column = new pov_vector<DBL>[4];
    column[0] = pov_vector<DBL>(4, 0.0);
    column[1] = column[0];
    column[2] = column[0];
    column[3] = column[0];
}

Matrix::Matrix(const Matrix& n)
{
    column = new pov_vector<DBL>[4];
    column[0] = n.column[0];
    column[1] = n.column[1];
    column[2] = n.column[2];
    column[3] = n.column[3];
}

Matrix::~Matrix()
{
    delete[] column;
}

void Matrix::Zero(void)
{
    register int i, j;

    for (i = 0; i < 4; i++)
	for (j = 0; j < 4; j++)
	    column[i][j] = 0.0;
}

void Matrix::Identity(void)
{
    register int i, j;

    this->Zero();
    column[0][0] = 1.0;
    column[1][1] = 1.0;
    column[2][2] = 1.0;
    column[3][3] = 1.0;
}

pov_vector<DBL>& Matrix::operator[](int n) const
{
    return (pov_vector<DBL>&)(column[n]);
}

Matrix& Matrix::operator=(const Matrix& matrix1)
{
    register int i;

    for (i = 0; i < 4; i++)
	column[i] = matrix1.column[i];
    return *this;
}

Matrix Matrix::operator*(Matrix& matrix2) const
{
    register int i, j, k;
    Matrix temp_matrix;

    for (i = 0 ; i < 4 ; i++)
    {
	for (j = 0 ; j < 4 ; j++)
	{
	    temp_matrix[i][j] = 0.0;

	    for (k = 0 ; k < 4 ; k++)
	    {
		temp_matrix[i][j] += column[i][k] * matrix2[k][j];
	    }
	}
    }
    return temp_matrix;
}

Matrix& Matrix::operator*=(Matrix& matrix2)
{
    register int i, j, k;
    Matrix temp_matrix;

    for (i = 0; i < 4; i++)
	for (j = 0; j < 4; j++)
	{
	    temp_matrix[i][j] = 0.0;

	    for (k = 0; k < 4; k++)
		temp_matrix[i][j] += column[i][k] * matrix2[k][j];
	}

    for (i = 0; i < 4; i++)
	for (j = 0; j < 4; j++)
	    column[i][j] = temp_matrix[i][j];

    return *this;
}

Matrix Transpose(const Matrix& matrix1)
{
    register int i, j;
    Matrix temp_matrix;

    for (i = 0 ; i < 4 ; i++)
	for (j = 0 ; j < 4 ; j++)
	    temp_matrix[i][j] = matrix1[j][i];

    return temp_matrix;
}

Matrix Invert(const Matrix &m)
{
    DBL d00, d01, d02, d03;
    DBL D;
    Matrix r;

    d00 = (m.column[1][1] * m.column[2][2] * m.column[3][3]
	   + m.column[1][2] * m.column[2][3] * m.column[3][1]
	   + m.column[1][3] * m.column[2][1] * m.column[3][2]
	   - m.column[3][1] * m.column[2][2] * m.column[1][3]
	   - m.column[3][2] * m.column[2][3] * m.column[1][1]
	   - m.column[3][3] * m.column[2][1] * m.column[1][2]);
    d01 = -(m.column[1][0] * m.column[2][2] * m.column[3][3]
	    + m.column[1][2] * m.column[2][3] * m.column[3][0]
	    + m.column[1][3] * m.column[2][0] * m.column[3][2]
	    - m.column[3][0] * m.column[2][2] * m.column[1][3]
	    - m.column[3][2] * m.column[2][3] * m.column[1][0]
	    - m.column[3][3] * m.column[2][0] * m.column[1][2]);
    d02 = (m.column[1][0] * m.column[2][1] * m.column[3][3]
	   + m.column[1][1] * m.column[2][3] * m.column[3][0]
	   + m.column[1][3] * m.column[2][0] * m.column[3][1]
	   - m.column[3][0] * m.column[2][1] * m.column[1][3]
	   - m.column[3][1] * m.column[2][3] * m.column[1][0]
	   - m.column[3][3] * m.column[2][0] * m.column[1][1]);
    d03 = -(m.column[1][0] * m.column[2][1] * m.column[3][2]
	    + m.column[1][1] * m.column[2][2] * m.column[3][0]
	    + m.column[1][2] * m.column[2][0] * m.column[3][1]
	    - m.column[3][0] * m.column[2][1] * m.column[1][2]
	    - m.column[3][1] * m.column[2][2] * m.column[1][0]
	    - m.column[3][2] * m.column[2][0] * m.column[1][1]);

    D = m.column[0][0] * d00 + m.column[0][1] * d01
	+ m.column[0][2] * d02 + m.column[0][3] * d03;

    if (D == 0.0)
    {
	/* this is not correct, but it allows compilation for now */
	return r;
    }

    r[0][0] = d00/D;
    r[1][0] = d01/D;
    r[2][0] = d02/D;
    r[3][0] = d03/D;

    r[0][1] = -(m.column[0][1] * m.column[2][2] * m.column[3][3]
		+ m.column[0][2] * m.column[2][3] * m.column[3][1]
		+ m.column[0][3] * m.column[2][1] * m.column[3][2]
		- m.column[3][1] * m.column[2][2] * m.column[0][3]
		- m.column[3][2] * m.column[2][3] * m.column[0][1]
		- m.column[3][3] * m.column[2][1] * m.column[0][2]) / D;
    r[1][1] = (m.column[0][0] * m.column[2][2] * m.column[3][3]
	       + m.column[0][2] * m.column[2][3] * m.column[3][0]
	       + m.column[0][3] * m.column[2][0] * m.column[3][2]
	       - m.column[3][0] * m.column[2][2] * m.column[0][3]
	       - m.column[3][2] * m.column[2][3] * m.column[0][0]
	       - m.column[3][3] * m.column[2][0] * m.column[0][2]) / D;
    r[2][1] = -(m.column[0][0] * m.column[2][1] * m.column[3][3]
		+ m.column[0][1] * m.column[2][3] * m.column[3][0]
		+ m.column[0][3] * m.column[2][0] * m.column[3][1]
		- m.column[3][0] * m.column[2][1] * m.column[0][3]
		- m.column[3][1] * m.column[2][3] * m.column[0][0]
		- m.column[3][3] * m.column[2][0] * m.column[0][1]) / D;
    r[3][1] = (m.column[0][0] * m.column[2][1] * m.column[3][2]
	       + m.column[0][1] * m.column[2][2] * m.column[3][0]
	       + m.column[0][2] * m.column[2][0] * m.column[3][1]
	       - m.column[3][0] * m.column[2][1] * m.column[0][2]
	       - m.column[3][1] * m.column[2][2] * m.column[0][0]
	       - m.column[3][2] * m.column[2][0] * m.column[0][1]) / D;

    r[0][2] = (m.column[0][1] * m.column[1][2] * m.column[3][3]
	       + m.column[0][2] * m.column[1][3] * m.column[3][1]
	       + m.column[0][3] * m.column[1][1] * m.column[3][2]
	       - m.column[3][1] * m.column[1][2] * m.column[0][3]
	       - m.column[3][2] * m.column[1][3] * m.column[0][1]
	       - m.column[3][3] * m.column[1][1] * m.column[0][2]) / D;
    r[1][2] = -(m.column[0][0] * m.column[1][2] * m.column[3][3]
		+ m.column[0][2] * m.column[1][3] * m.column[3][0]
		+ m.column[0][3] * m.column[1][0] * m.column[3][2]
		- m.column[3][0] * m.column[1][2] * m.column[0][3]
		- m.column[3][2] * m.column[1][3] * m.column[0][0]
		- m.column[3][3] * m.column[1][0] * m.column[0][2]) / D;
    r[2][2] = (m.column[0][0] * m.column[1][1] * m.column[3][3]
	       + m.column[0][1] * m.column[1][3] * m.column[3][0]
	       + m.column[0][3] * m.column[1][0] * m.column[3][1]
	       - m.column[3][0] * m.column[1][1] * m.column[0][3]
	       - m.column[3][1] * m.column[1][3] * m.column[0][0]
	       - m.column[3][3] * m.column[1][0] * m.column[0][1]) / D;
    r[3][2] = -(m.column[0][0] * m.column[1][1] * m.column[3][2]
		+ m.column[0][1] * m.column[1][2] * m.column[3][0]
		+ m.column[0][2] * m.column[1][0] * m.column[3][1]
		- m.column[3][0] * m.column[1][1] * m.column[0][2]
		- m.column[3][1] * m.column[1][2] * m.column[0][0]
		- m.column[3][2] * m.column[1][0] * m.column[0][1]) / D;

    r[0][3] = -(m.column[0][1] * m.column[1][2] * m.column[2][3]
		+ m.column[0][2] * m.column[1][3] * m.column[2][1]
		+ m.column[0][3] * m.column[1][1] * m.column[2][2]
		- m.column[2][1] * m.column[1][2] * m.column[0][3]
		- m.column[2][2] * m.column[1][3] * m.column[0][1]
		- m.column[2][3] * m.column[1][1] * m.column[0][2]) / D;
    r[1][3] = (m.column[0][0] * m.column[1][2] * m.column[2][3]
	       + m.column[0][2] * m.column[1][3] * m.column[2][0]
	       + m.column[0][3] * m.column[1][0] * m.column[2][2]
	       - m.column[2][0] * m.column[1][2] * m.column[0][3]
	       - m.column[2][2] * m.column[1][3] * m.column[0][0]
	       - m.column[2][3] * m.column[1][0] * m.column[0][2]) / D;
    r[2][3] = -(m.column[0][0] * m.column[1][1] * m.column[2][3]
		+ m.column[0][1] * m.column[1][3] * m.column[2][0]
		+ m.column[0][3] * m.column[1][0] * m.column[2][1]
		- m.column[2][0] * m.column[1][1] * m.column[0][3]
		- m.column[2][1] * m.column[1][3] * m.column[0][0]
		- m.column[2][3] * m.column[1][0] * m.column[0][1]) / D;
    r[3][3] = (m.column[0][0] * m.column[1][1] * m.column[2][2]
	       + m.column[0][1] * m.column[1][2] * m.column[2][0]
	       + m.column[0][2] * m.column[1][0] * m.column[2][1]
	       - m.column[2][0] * m.column[1][1] * m.column[0][2]
	       - m.column[2][1] * m.column[1][2] * m.column[0][0]
	       - m.column[2][2] * m.column[1][0] * m.column[0][1]) / D;
    return r;
}

Transformation::Transformation(void)
    : ma(), in()
{
    ma.Identity();
    in.Identity();
}

Transformation::Transformation(const Transformation& t)
    : ma(t.ma), in(t.in)
{
}

Transformation::~Transformation()
{
}

pov_vector<DBL> Transformation::TransformPoint(pov_vector<DBL>& v)
{
    register int i;
    pov_vector<DBL> answer_array(3, 0.0);

    for (i = 0 ; i < 3 ; i++)
	answer_array[i] = (v[0] * ma[0][i])
	    + (v[1] * ma[1][i])
	    + (v[2] * ma[2][i])
	    + ma[3][i];

    return answer_array;
}

pov_vector<DBL> Transformation::InverseTransformPoint(pov_vector<DBL>& v)
{
    register int i;
    pov_vector<DBL> answer_array(3, 0.0);

    for (i = 0 ; i < 3 ; i++)
	answer_array[i] = (v[X] * in[0][i])
	    + (v[Y] * in[1][i])
	    + (v[Z] * in[2][i])
	    + in[3][i];

    return answer_array;
}

pov_vector<DBL> Transformation::TransformDirection(pov_vector<DBL>& v)
{
    register int i;
    pov_vector<DBL> answer_array(3, 0.0);

    for (i = 0 ; i < 3 ; i++)
	answer_array[i] = (v[X] * ma[0][i])
	    + (v[Y] * ma[1][i])
	    + (v[Z] * ma[2][i]);

    return answer_array;
}

pov_vector<DBL> Transformation::InverseTransformDirection(pov_vector<DBL>& v)
{
    register int i;
    pov_vector<DBL> answer_array(3, 0.0);

    for (i = 0 ; i < 3 ; i++)
	answer_array[i] = (v[X] * in[0][i])
	    + (v[Y] * in[1][i])
	    + (v[Z] * in[2][i]);

    return answer_array;
}

pov_vector<DBL> Transformation::TransformNormal(pov_vector<DBL>& v)
{
    register int i;
    pov_vector<DBL> answer_array(3, 0.0);

    for (i = 0 ; i < 3 ; i++)
	answer_array[i] = (v[X] * in[i][0])
	    + (v[Y] * in[i][1])
	    + (v[Z] * in[i][2]);

    return answer_array;
}

pov_vector<DBL> Transformation::InverseTransformNormal(pov_vector<DBL>& v)
{
    register int i;
    pov_vector<DBL> answer_array(3, 0.0);

    for (i = 0 ; i < 3 ; i++)
	answer_array[i] = (v[X] * ma[i][0])
	    + (v[Y] * ma[i][1])
	    + (v[Z] * ma[i][2]);

    return answer_array;
}

void Transformation::MatrixTransform(Matrix& matrix)
{
    ma = matrix;
    in = Invert(ma);
}

void Transformation::ScalingTransform(pov_vector<DBL>& v)
{
    ma.Identity();
    ma[0][0] = v[X];
    ma[1][1] = v[Y];
    ma[2][2] = v[Z];

    in.Identity();
    in[0][0] = 1.0 / v[X];
    in[1][1] = 1.0 / v[Y];
    in[2][2] = 1.0 / v[Z];
}

void Transformation::TranslationTransform(pov_vector<DBL>& v)
{
    ma.Identity();
    ma[3][0] = v[X];
    ma[3][1] = v[Y];
    ma[3][2] = v[Z];

    in.Identity();
    in[3][0] = -v[X];
    in[3][1] = -v[Y];
    in[3][2] = -v[Z];
}

void Transformation::RotationTransform(const pov_vector<DBL>& v)
{
    register DBL cosx, cosy, cosz, sinx, siny, sinz;
    Matrix matrix;
    pov_vector<DBL> Radian_Vector(3, 0.0);

    Radian_Vector = v * M_PI_180;

    ma.Identity();

    cosx = cos(Radian_Vector[0]);
    sinx = sin(Radian_Vector[0]);
    cosy = cos(Radian_Vector[1]);
    siny = sin(Radian_Vector[1]);
    cosz = cos(Radian_Vector[2]);
    sinz = sin(Radian_Vector[2]);

    ma[1][1] = cosx;
    ma[2][2] = cosx;
    ma[1][2] = sinx;
    ma[2][1] = 0.0 - sinx;

    in = Transpose(ma);

    matrix.Identity();

    matrix[0][0] = cosy;
    matrix[2][2] = cosy;
    matrix[0][2] = 0.0 - siny;
    matrix[2][0] = siny;

    ma *= matrix;
    in = Transpose(matrix) * in;

    matrix.Identity();

    matrix[0][0] = cosz;
    matrix[1][1] = cosz;
    matrix[0][1] = sinz;
    matrix[1][0] = 0.0 - sinz;

    ma *= matrix;
    in = Transpose(matrix) * in;
}

void Transformation::Compose(Transformation& t)
{
    ma *= t.ma;
    in = t.in * in;
}

void Transformation::AxisRotationTransform(const pov_vector<DBL>& v, DBL angle)
{
    DBL cosx, sinx;
    pov_vector<DBL> vec = v;

    vec.Normalize();
    ma.Identity();

    cosx = cos(angle);
    sinx = sin(angle);

    ma[0][0] = vec[X] * vec[X] + cosx * (1.0 - vec[X] * vec[X]);
    ma[0][1] = vec[X] * vec[Y] * (1.0 - cosx) + vec[Z] * sinx;
    ma[0][2] = vec[X] * vec[Z] * (1.0 - cosx) - vec[Y] * sinx;

    ma[1][0] = vec[X] * vec[Y] * (1.0 - cosx) - vec[Z] * sinx;
    ma[1][1] = vec[Y] * vec[Y] + cosx * (1.0 - vec[Y] * vec[Y]);
    ma[1][2] = vec[Y] * vec[Z] * (1.0 - cosx) + vec[X] * sinx;

    ma[2][0] = vec[X] * vec[Z] * (1.0 - cosx) + vec[Y] * sinx;
    ma[2][1] = vec[Y] * vec[Z] * (1.0 - cosx) - vec[X] * sinx;
    ma[2][2] = vec[Z] * vec[Z] + cosx * (1.0 - vec[Z] * vec[Z]);

    in = Transpose(ma);
}

void Transformation::CoordinateTransform(pov_vector<DBL>& origin,
					 pov_vector<DBL>& up,
					 DBL radius,
					 DBL length)
{
    Transformation trans2;
    pov_vector<DBL> tmpv;

    tmpv[X] = tmpv[Y] = radius; tmpv[Z] = length;
    this->ScalingTransform(tmpv);

    if (fabs(up[2]) > 1.0 - EPSILON)
    {
	tmpv[X] = 1.0; tmpv[Y] = tmpv[Z] = 0.0;
	up[Z] = ((up[Z] < 0.0) ? -1.0 : 1.0);
    }
    else
    {
	tmpv[X] = -up[Y];
	tmpv[Y] = up[X];
	tmpv[Z] = 0.0;
    }

    trans2.AxisRotationTransform(tmpv, acos(up[Z]));
    this->Compose(trans2);
    trans2.TranslationTransform(origin);
    this->Compose(trans2);
}