/**********************************************************************************************
 *4d.cpp
 *
 *Implementation file of a 4d visualization library.
 *
 *Josh McCoy
 *October 2003
 *
 **********************************************************************************************/
#include "4d.h"


/*void translate4d(GLdouble m[5][5], GLdouble d1, GLdouble d2, GLdouble d3, GLdouble d4)
 *
 *Translates along any of the four axes in the transformation matrix given by the
 *first argument.  The four remaining arguments are the distance of the translations
 *according to each axis.  The matrix argument is changed and holds the results of
 *the transformation.
 */
void translate4d(GLdouble m[5][5], GLdouble d1, GLdouble d2, GLdouble d3, GLdouble d4)
{
  m[0][4] += d1;
  m[1][4] += d2;
  m[2][4] += d3;
  m[3][4] += d4;
}

/*void translate4d(GLdouble m[5][5], GLdouble p[5])
 *
 *Similar to the other translate4d function. Instead of the translation
 *given in four GLdoubles, it is given in the form of a homogeneous
 *coordinate vector.  The matrix argument is changed and holds the resultant
 *transformation matrix.
 */
void translate4d(GLdouble m[5][5], GLdouble p[5])
{
  m[0][4] += p[0];
  m[1][4] += p[1];
  m[2][4] += p[2];
  m[3][4] += p[3];
}

/*void rotate4d(GLdouble m[5][5], GLdouble theta, GLint axis)
 *
 *Changes a tranformation matrix to reflect a rotation around an axis.  The
 *first argument is the matrix to be changed.  The second argument is the angle
 *of rotation from the axis in degrees.  The last argument is the axis to rotate
 *around in dimension order (0 rotates around the x-axis, 1 rotates around the y-axis,
 *2 rotates around the z-axis, and 3 rotates around the fourth dimensional axis).
 */
void rotate4d(GLdouble m[5][5], GLdouble theta, GLint axis)
{
  GLdouble r[5][5] = {
	  {1.0, 0.0, 0.0, 0.0, 0.0},
	  {0.0, 1.0, 0.0, 0.0, 0.0},
	  {0.0, 0.0, 1.0, 0.0, 0.0},
	  {0.0, 0.0, 0.0, 1.0, 0.0},
	  {0.0, 0.0, 0.0, 0.0, 1.0},
  };

  r[(axis+1)%4][(axis+1)%4] = cos(theta * TO_RADIANS);
  r[(axis+1)%4][(axis+2)%4] = -sin(theta * TO_RADIANS);
  r[(axis+2)%4][(axis+1)%4] = sin(theta * TO_RADIANS);
  r[(axis+2)%4][(axis+2)%4] = cos(theta * TO_RADIANS);
  ;
  multMatrix4d(m, r, m);
}


/*void multMatrix4d(GLdouble m1[5][5], GLdouble m2[5][5], GLdouble m3[5][5])
 *
 *Multiples the first matrix argument times the second matrix argument and
 *places the result in the thrid matrix agrument.
 */
void multMatrix4d(GLdouble m1[5][5], GLdouble m2[5][5], GLdouble m3[5][5])
{
  GLint i=0, j=0, k=0;

  GLdouble result[5][5] = {
	  {0.0, 0.0, 0.0, 0.0, 0.0},
	  {0.0, 0.0, 0.0, 0.0, 0.0},
	  {0.0, 0.0, 0.0, 0.0, 0.0},
	  {0.0, 0.0, 0.0, 0.0, 0.0},
	  {0.0, 0.0, 0.0, 0.0, 0.0},
  };

	for(i = 0; i < 5; ++i){
		for(j = 0; j < 5; ++j){			            
			for(k = 0; k < 5; ++k){
			  result[i][j] += m1[i][k] * m2[k][j];
              
			}
		}
	}
    
  for(i = 0; i < 5; ++i)
	for(j = 0; j < 5; ++j)
      m3[i][j] = result[i][j];
}

/*void perspectiveDivision(GLdouble p[5])
 *
 *Performs perspective division on the homogenous coordinate agrument.
 *The coordinate is changed and stores the result.
 */
void perspectiveDivision(GLdouble p[5])
{
  p[0] = p[0] / p[4];
  p[1] = p[1] / p[4];
  p[2] = p[2] / p[4];
  p[3] = p[3] / p[4];
  p[4] = p[4] / p[4];
}

/*void matrixVectorProduct(GLdouble m[5][5], GLdouble p[5])
 *
 *Multiples the matrix agrument by the homogeneous coordinate.
 *The result goes in the coordinate specified by the third argument
 */
void matrixCoordinateProduct(GLdouble m[5][5], GLdouble p[5], GLdouble r[5])
{
  GLdouble result[5] = {0.0, 0.0, 0.0, 0.0, 0.0};

  for(unsigned int i = 0; i < 5; ++i)
	for(unsigned int j = 0; j < 5; ++j)
	  result[i] += m[i][j] *  p[j];

  for(i = 0; i<5; ++i)
    r[i] = result[i];
}

/*GLdouble& matrixVectorProduct(GLdouble m[5][5], GLdouble p[5])
 *
 *Multiples the matrix agrument by the homogeneous coordinate.
 *The return value is the resultant coordinate
 */
/*GLdouble[5][5] matrixCoordinateProduct(GLdouble m[5][5], GLdouble p[5])
{
  GLdouble result[5] = {0.0, 0.0, 0.0, 0.0, 0.0};

  for(unsigned int i = 0; i < 5; ++i)
	for(unsigned int j = 0; j < 5; ++j)
	  result[i] += m[i][j] *  p[j];

  return result;
}
*/

/*void printCoordinate(GLdouble p[5])
 *
 *Prints the given coordinate to stdout.
 */
void printCoordinate(GLdouble p[5])
{
  cout.precision(4);
  cout << p[0] << ' ' 
    << p[1] << ' '
    << p[2] << ' '
    << p[3] << ' '
    << p[4] << endl;
}

/*void printMatrix(GLdouble m[5][5])
 *
 *Prints the given matrix to stdout.
 */
void printMatrix(GLdouble m[5][5])
{
  cout.precision(4);
  
  for(int i = 0; i < 5; ++i){
    for(int j = 0; j < 5; ++j)
      cout <<  m[i][j] << " ";    
    cout << endl;
  }  
}