#include <math.h>
#include "Matrix.H"
#include "Transform.H"

Matrix Translate(double dx, double dy)
{
   return(Matrix(3,3,
		 1.0,0.0,0.0,
		 0.0,1.0,0.0,
		 dx, dy, 1.0));
}

Matrix Scale(double a, double b)
{
   return(Matrix(3,3,
		 a  ,0.0,0.0,
		 0.0,b  ,0.0,
		 0.0,0.0,1.0));
}

Matrix Rotate(double t, double x, double y)
{
   return(Matrix(3,3,
		 cos(t),		  sin(t),		   0.0,
		 -sin(t),		  cos(t),		   0.0,
		 x*(1.0-cos(t))+y*sin(t), y*(1.0-cos(t))-x*sin(t), 1.0));
}


Matrix Translate(double x, double y, double z)
{
   return(Matrix(4,4,
		 1.0,0.0,0.0,0.0,
		 0.0,1.0,0.0,0.0,
		 0.0,0.0,1.0,0.0,
		 x,y,z,1.0));
}

Matrix Scale(double x, double y, double z)
{
   return(Matrix(4,4,
		 x  ,0.0,0.0,0.0,
		 0.0,y  ,0.0,0.0,
		 0.0,0.0,z  ,0.0,
		 0.0,0.0,0.0,1.0));
}


Matrix RotateX(double t, double x, double y, double z)
{
   return(Matrix(4,4,
		 1.0,0.0,0.0,0.0,
		 0.0,cos(t),sin(t),0.0,
		 0.0,-sin(t),cos(t),0.0,
		 0.0,y*(1-cos(t))+z*sin(t),z*(1-cos(t))-y*sin(t),1.0));
}

Matrix RotateY(double t, double x, double y, double z)
{
   return(Matrix(4,4,
		 cos(t),0.0,-sin(t),0.0,
		 0.0,1.0,0.0,0.0,
		 sin(t),0.0,cos(t),0.0,
		 x*(1-cos(t))-z*sin(t),0.0,z*(1-cos(t))+x*sin(t),1.0));
}

Matrix RotateZ(double t, double x, double y, double z)
{
   return(Matrix(4,4,
		 cos(t),sin(t),0.0,0.0,
		 -sin(t),cos(t),0.0,0.0,
		 0.0,0.0,1.0,0.0,
		 x*(1-cos(t))+y*sin(t),y*(1-cos(t))-x*sin(t),0.0,1.0));
}

Matrix Translate(Matrix& p, int i)
   // Returns an N dimentional matrix to translate by the ith row of p,
   // or NAM.
{
     if (i > p.Rows())
	  return Matrix::NAM();
     
     Matrix m(p.Cols(), p.Cols());
     int j;

     m.Identify();
     for (j = 1; j < p.Cols(); j ++)
	  m(p.Cols(), j) = p(i, j);

     return m;
}