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


/*void translate5d(GLdouble m[6][6], GLdouble d1, GLdouble d2, GLdouble d3, GLdouble d5)
 *
 *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 translate5d(GLdouble m[6][6], GLdouble d1, GLdouble d2, GLdouble d3, GLdouble d4, GLdouble d5)
{
  m[0][5] += d1;
  m[1][5] += d2;
  m[2][5] += d3;
  m[3][5] += d4;
  m[4][5] += d5;
}

/*void translate5d(GLdouble m[6][6], GLdouble p[6])
 *
 *Similar to the other translate5d 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 translate5d(GLdouble m[6][6], GLdouble p[6])
{
  m[0][5] += p[0];
  m[1][5] += p[1];
  m[2][5] += p[2];
  m[3][5] += p[3];
  m[4][5] += p[4];
}

/*void rotate5d(GLdouble m[6][6], 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 rotate5d(GLdouble m[6][6], GLdouble theta, GLint axis)
{
  GLdouble r[6][6] = {
	  {1.0, 0.0, 0.0, 0.0, 0.0, 0.0},
	  {0.0, 1.0, 0.0, 0.0, 0.0, 0.0},
	  {0.0, 0.0, 1.0, 0.0, 0.0, 0.0},
	  {0.0, 0.0, 0.0, 1.0, 0.0, 0.0},
	  {0.0, 0.0, 0.0, 0.0, 1.0, 0.0},
	  {0.0, 0.0, 0.0, 0.0, 0.0, 1.0},
  };

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


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

  GLdouble result[6][6] = {
	  {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, 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 < 6; ++i){
		for(j = 0; j < 6; ++j){			            
			for(k = 0; k < 6; ++k){
			  result[i][j] += m1[i][k] * m2[k][j];
              
			}
		}
	}
    
  for(i = 0; i < 6; ++i)
	for(j = 0; j < 6; ++j)
      m3[i][j] = result[i][j];
}

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

/*void perspectiveDivision(GLdouble p[6], GLint d)
 *
 *Performs perspective division in a certain dimension.  Created
 *for use in projecting from multiple dimensions.
 */
void perspectiveDivision(GLdouble p[6], GLint d)
{
  GLint i;

  for(i=0; i<=d; ++i)
    p[i] = p[i] / p[d];
}



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

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

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

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

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