//Clases basicas de geometria y algebra

#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "Macros.h"
#include "vtkAlgebra.h"



// Clase Punto -------------------------------------------
//--------------------------------------------------------

Punto::Punto(float xx, float yy, float zz, float ww)
//Constructor por defecto
{
	x=xx; y=yy; z=zz; w=ww; esUnitario=0;
};

Punto Punto::operator +(Punto p)
//Suma de dos vectores 
{
	Punto suma;
	
	suma.x=x+p.x; suma.y=y+p.y; suma.z=z+p.z;
	return suma;
};

Punto Punto::operator -(Punto p)
//Diferencia de dos vectores
{
	Punto resta;
	resta.x=x - p.x; resta.y=y-p.y; resta.z=z-p.z;
	return resta;

};

Punto Punto::operator *(float k)
//Producto por un escalar POR LA DERECHA
{
	Punto multi;
	multi.x=x*k; multi.y=y*k; multi.z=z*k;
	return multi;
};

float Punto::operator [](int i)
//Tratamiento como vector por sobrecarga del operador []
{
	switch(i)
	{
	case 0:
		return(x);
		break;
	case 1:
		return(y);
		break;
	case 2:
		return(z);
		break;
	case 3:
		return(w);
		break;
	default:
		std::cerr << "GB:Indice de coordenada fuera de rango\n";
	};
	exit(1);
	return 0.0f;
};

Punto Punto::operator *(Matriz m)
//Producto por una matriz POR LA DERECHA (P*M)
{
	int i;
	Punto resul;

	resul.w = 0.0f;
	// ¿O w debe valer 1 siempre en el punto resultante?
	for (i=0; i<4; i++)
	{
		resul.x += (*this)[i]*m.e[i][0];
		resul.y += (*this)[i]*m.e[i][1];
		resul.z += (*this)[i]*m.e[i][2];
		resul.w += (*this)[i]*m.e[i][3];
	}
	return resul;

};

Punto Punto::transformar(Transformacion &t)
//Transforma considerando el punto como vector columna
//es decir, aplica la transformacion por la izquierda
{
	return (t*(*this));
};

Punto Punto::negado()
//Devuelve un nuevo vector con el signo cambiado
{
	return Punto(-x,-y,-z,w);
};

float Punto::modulo()
//Devuelve el modulo del vector
{
	float modu;
	modu = cuad(x) + cuad(y) + cuad(z);
	modu = (float)sqrt(modu);
	if(modu == 0.0f) modu = 0.0001f;
	return modu;

};

Punto Punto::normalizar()
//Normaliza el vector receptor y se devuelve a si mismo
{
	float temp;

	if (esUnitario == 0) {
		esUnitario = 1;
		temp = modulo();
		x = x/temp;
		y = y/temp;
		z = z/temp;
		}
	return *this;
};

Punto Punto::unitario()
//Devuelve el vector unitario en esa direccion
{
	Punto norm;
	float temp;

	if (esUnitario == 0) {
		temp = modulo();
		norm.x = x/temp;
		norm.y = y/temp;
		norm.z = z/temp;
		norm.w = 1.0f;
		}
	else {
		norm.x = x; norm.y = y; norm.z = z; norm.w=1.0f;
	}
	norm.esUnitario = 1;
	return norm;
};

float Punto::longitud()
//Calcula la longitud (0..360º) esferica del vector en grados
{
	float longi;
	float hipot;
	float coseno;

	hipot = (float)sqrt(cuad(x)+cuad(z));
	if (hipot != 0) 
	{
		coseno = z/(hipot);
		longi= (float)acos(coseno);
		if (x<0) longi= -longi;
	}
	else
		longi = 0.0f;
	
	return longi*180/PI;
	
};

float Punto::latitud()
//Calcula la latitud (-90º..90º) esférica del vector en grados
{
	float lat;
	lat= (float)acos(sqrt(x*x+z*z)/modulo());		
	if(y<0) lat= -lat;
	return lat*180/PI;
};

float Punto::operator *(Punto p)
//Calcula el producto escalar
{
	int i;
	float tot = 0.0f;
	for (i=0; i<3; i++)
		tot += (*this)[i]*p[i];

	return tot;
}

Punto Punto::operator ^(Punto p)
//Calcula el producto vectorial
{
	Punto resul;

	resul.x = y*p.z - z*p.y;
	resul.y = z*p.x - x*p.z;
	resul.z = x*p.y - y*p.x;

	return resul;
}

Punto Punto::homogeneo()
//Devuelve el punto con w=1 (dividiendo por w)
{
	Punto resul;
	
	resul.x = x/w;
	resul.y = y/w;
	resul.z = z/w;
	resul.w = 1.0f;

	return resul;
}

void Punto::escribir()
//Escribe las coordenadas del punto en la consola
{
	std::cout << x <<","<< y << ","<< z <<","<< w;
	std::cout << std::endl;
};

BOOL Punto::operator ==(Punto p)
//Compara puntos
{
	if ((x == p.x)&&(y == p.y)&&(z == p.z)&&(w == p.w))
		return TRUE;
	else
		return FALSE;
};


//Clase Matriz -------------------------------------------
//--------------------------------------------------------


Matriz::Matriz()
//Construye la identidad por defecto
{
	int i,j;
	for(i=0;i<4;i++)
		for(j=0;j<4;j++){
			if(j==i) e[i][j]=1.0f;
			else e[i][j]=0.0f;
		};
};

Matriz::Matriz(Punto e1,Punto e2,Punto e3,Punto e4)
//Construye la matriz por columnas
{
	int i;
	for(i=0;i<4;i++){
		e[i][0]=e1[i];
		e[i][1]=e2[i];
		e[i][2]=e3[i];
		e[i][3]=e4[i];
	};
};

Punto Matriz::fila(int i)
//Devuelve un punto formado por la fila i (0..3)
{
	if(i<4 && i>=0) return Punto(e[i][0],e[i][1],e[i][2],e[i][3]);
	else {		
		std::cerr << "GB:Indice de columna fuera de rango\n";
		exit(1);
	};
	return Punto();
};

Punto Matriz::columna(int i)
//Devuelve un punto con la columna i (0..3)
{
	if(i<4 && i>=0) return Punto(e[0][i],e[1][i],e[2][i],e[3][i]);
	else {		
		std::cerr << "GB:Indice de columna fuera de rango\n";
		exit(1);
	};
	return Punto();
};


Matriz Matriz::operator =(const Matriz &m)
//Asignacion de matrices
{
	int i,j;
	for(i=0;i<4;i++)
		for(j=0;j<4;j++) e[i][j]=m.e[i][j];
	return *this;
};

Matriz Matriz::operator *(Matriz m)
//Producto de matrices. Devuelve una nueva matriz
{
	Matriz producto;
	int i,j,k;

	for (i=0; i<4; i++)
		for (j=0; j<4; j++)
			producto.e[i][j] = 0.0f;

	for (i=0; i<4; i++) {
		for (j=0; j<4; j++){
			for (k=0; k<4; k++){
				producto.e[i][j] += e[i][k] * m.e[k][j];
			}
		}
	}
	return producto;
};

Punto Matriz::operator *(Punto p)
//Producto por un vector POR LA DERECHA (M*P)
{
	int j;
	Punto pr(0.0f,0.0f,0.0f,0.0f);

	for(j=0;j<4;j++){
		  pr.x= pr.x+e[0][j]*p[j];
		  pr.y= pr.y+e[1][j]*p[j];
		  pr.z= pr.z+e[2][j]*p[j];
		  pr.w= pr.w+e[3][j]*p[j];
	};
	return pr;
};

Matriz3d Matriz::operator *(const Matriz3d &m3d)
//Multiplica por capas (k cte.) y devuelve una matriz3d
{
	int i,j,k,n;
	Matriz3d mr;	//Por defecto toda a ceros

	for(k=0;k<4;k++) //fija la capa
		for(i=0;i<4;i++)
			for(j=0;j<4;j++)
				for(n=0;n<4;n++)
					mr.e[i][j][k]= mr.e[i][j][k] + e[i][n]*m3d.e[n][j][k];
	return mr;
};


Matriz Matriz::operator *(float k)
//Producto por un escalar (M*k). Devuelve una nueva matriz
{
	Matriz prod;
	int i,j;

	for (i=0; i<4; i++) {
		for (j=0; j<4; j++){
			prod.e[i][j] = e[i][j]*k;
		}
	}
	return prod;
};

Matriz Matriz::operator !()
//Devuelve la traspuesta en una nueva matriz. 
//Usar parentesis para asegurar precedencia (!M)
{
	Matriz trasp;
	int i,j;

	for (i=0; i<4; i++){
		for (j=0; j<4; j++){
			trasp.e[j][i] = e[i][j];
		}
	}
	return trasp;
};

void Matriz::escribir()
//Escribe la matriz por filas en la salida estandar
{
	int i,j;

	for(i=0;i<4;i++){
		std::cout << std::endl;
		for(j=0;j<4;j++)
			std::cout << e[i][j] << "  ";
	};
	std::cout << std::endl;
};


//Clase Transformacion (subclase de Matriz) -------------------------
//-------------------------------------------------------------------

void Transformacion::traslacion(Punto d)
//Acumula la traslacion propuesta (d es la traslación)
{
	Matriz t;
	int i;

	for (i=0;i<3;i++) t.e[i][3]=d[i];
	Matriz::operator=((*this)*t); // ===  (*this)=(*this)*t;
};

void Transformacion::escalado(float sx,float sy,float sz, Punto centro)
//Acumula el escalado sobre un punto generico
{
	Matriz s;
	
	s.e[0][0] = sx; s.e[1][1] = sy; s.e[2][2] = sz;
	if (centro.modulo() != 0) traslacion(centro);
	Matriz::operator=((*this)*s);
	if (centro.modulo() != 0) traslacion(centro.negado());

};


void Transformacion::giro(float ang, Punto vectorEje, Punto enRecta )
//Acumula el giro alrededor de un eje generico
{
	float lat,longit;

	lat = vectorEje.latitud();
	longit = vectorEje.longitud();

	traslacion(enRecta);
	giroY(longit);
	giroX(-lat);

	giroZ(ang); //El giro sobre el eje tumbado

	giroX(lat);
	giroY(-longit);
	traslacion(enRecta.negado());

};

//Acumulacion de giros específicos
void Transformacion::giroX(float ang)
//en X
{
	Matriz g;
	g.e[1][1]=(float)cos((double)ang/180*PI);
    g.e[1][2]=(float)sin((double)(-ang/180*PI));
	g.e[2][1]= -g.e[1][2];        
	g.e[2][2]=g.e[1][1];

	Matriz::operator=((*this)*g);
};
void Transformacion::giroY(float ang)
//en Y
{
	Matriz g;
	g.e[0][0]=(float)cos((double)ang/180*PI);
    g.e[2][0]=(float)sin((double)(-ang/180*PI));
	g.e[0][2]= -g.e[2][0];        
	g.e[2][2]=g.e[0][0];

	Matriz::operator=((*this)*g);
};
void Transformacion::giroZ(float ang)
//en Z
{
	Matriz g;
	g.e[0][0]=(float)cos((double)ang/180*PI);
    g.e[0][1]=(float)sin((double)(-ang/180*PI));
	g.e[1][0]= -g.e[0][1];        
	g.e[1][1]=g.e[0][0];

	Matriz::operator=((*this)*g);
};
void Transformacion::giro(Punto u, Punto v, Punto w)
//Acumula un cambio de base dado por los ejes u,v,w
{
	Matriz g;

	g = !(Matriz(u,v,w,Punto(0.0f,0.0f,0.0f,1.0f)));
	g.e[0][3] = 0.0f;
	g.e[1][3] = 0.0f;
	g.e[2][3] = 0.0f;

	Matriz::operator=((*this)*g);
}

void Transformacion::identidad()
{
	int i,j;
	for(i=0;i<4;i++)
		for(j=0;j<4;j++){
			if(j==i) e[i][j]=1.0f;
			else e[i][j]=0.0f;
		};

}


// Clase Matriz3d -------------------------------------------
//-----------------------------------------------------------

Matriz3d::Matriz3d()
//Constructor por defecto. Matriz toda a 0.0
{
	int i,j,k;
	for(i=0;i<4;i++)
		for(j=0;j<4;j++)
		for(k=0;k<4;k++) e[i][j][k]=0.0f;
};

Matriz3d::Matriz3d(Matriz c1, Matriz c2, Matriz c3, Matriz c4)
//Contruye la matriz3d con las matrices como columnas
{
	int i,k;

	for(i=0;i<4;i++)
		for(k=0;k<4;k++){
			e[i][0][k]=c1.e[k][i];
			e[i][1][k]=c2.e[k][i];
			e[i][2][k]=c3.e[k][i];
			e[i][3][k]=c4.e[k][i];
	};
};

Matriz3d Matriz3d::operator =(const Matriz3d &m)
//Asignacion de matrices 3d
{
	int i,j,k;
	for(i=0;i<4;i++)
		for(j=0;j<4;j++)
			for (k=0;k<4;k++) e[i][j][k]=m.e[i][j][k];
	return *this;
};

Matriz3d Matriz3d::operator *(const Matriz &m)
//Multiplica por capas y devuelve la nueva matriz 3d
{
	int i,j,k,n;
	Matriz3d mr;	//Por defecto toda a ceros

	for(k=0;k<4;k++) //fija la capa
		for(i=0;i<4;i++)
			for(j=0;j<4;j++)
				for(n=0;n<4;n++)
					mr.e[i][j][k]= mr.e[i][j][k] + e[i][n][k]*m.e[n][j];
	return mr;

};

Punto Matriz3d::fiCo(int i, int j)
//Devuelve el punto para esa fila,columna 
{
/********************* A IMPLEMENTAR ***********************/
	return Punto(e[i][j][0],e[i][j][1],e[i][j][2],e[i][j][3]);
};

void Matriz3d::escribir()
//escribe la matriz por la salida estandar por columnas
{
	int i,j,k;

	for(j=0;j<4;j++){
		std::cout << std::endl;
		for(i=0;i<4;i++){
			std::cout << std::endl;
			for(k=0;k<4;k++) std::cout << e[i][j][k] << "  ";
		};
	};
};

Matriz3d Matriz3d::ponerEn(int i, int j, Punto p)
//Mete un punto en la posicion i,j
{
	int k;
	for(k=0;k<4;k++)
		e[i][j][k]=p[k];
	return *this;
};