#include "GraphicsSupport.h"
#include "gravity_support.h"
#include <stdio.h>
#include <math.h>




void CreateIdentityMatrix (GRAPHICS_MATRIX Mresult)
{
  int i,j;

  for (i=0; i<NDIMS+1; i++) {
    for (j=0; j<NDIMS+1; j++) {
      Mresult[i][j] = i==j ? 1.0 : 0.0;
    }/*endfor*/
  }/*endfor*/
}




void CreateTranslationMatrix (int d, double dist, GRAPHICS_MATRIX Mresult)
{
  CreateIdentityMatrix(Mresult);
  Mresult[ NDIMS ][ d ] = dist;
}




void CreateRotationMatrix (int d1, int d2, double angle,
                            GRAPHICS_MATRIX Mresult)
{
  if (d1==d2) {
    fprintf(stderr, "CreateRotationMatrix: both dimensions are %d!\n", d1);
    exit(1);
  }/*endif*/

  CreateIdentityMatrix(Mresult);
  Mresult[d1][d1] =   cos(angle); Mresult[d1][d2] = sin(angle);
  Mresult[d2][d1] = - sin(angle); Mresult[d2][d2] = cos(angle);
}




void MultiplyMatrices (GRAPHICS_MATRIX M1, GRAPHICS_MATRIX M2,
                        GRAPHICS_MATRIX Mresult)
{
  GRAPHICS_MATRIX temp_result;	/* in case of argument-duplication! */
  int i,j,k;

  for (i=0; i<NDIMS+1; i++) {
    for (j=0; j<NDIMS+1; j++) {
      temp_result[i][j] = 0.0;
      for (k=0; k<NDIMS+1; k++) {
        temp_result[i][j] += M1[i][k] * M2[k][j];
      }/*endfor*/
    }/*endfor*/
  }/*endfor*/

  for (i=0; i<NDIMS+1; i++) {
    for (j=0; j<NDIMS+1; j++) {
      Mresult[i][j] = temp_result[i][j];
    }/*endfor*/
  }/*endfor*/
}




void MultiplyGraphicsVectorByMatrix
      (GRAPHICS_VECTOR V, GRAPHICS_MATRIX M, GRAPHICS_VECTOR Vresult)
{
  GRAPHICS_VECTOR temp_result;	/* in case of argument-duplication! */
  int i,j;

  for (i=0; i<NDIMS+1; i++) {
    temp_result[i] = 0.0;
    for (j=0; j<NDIMS+1; j++) {
      temp_result[i] += V[j] * M[j][i];
    }/*endfor*/
  }/*endfor*/

  for (i=0; i<NDIMS+1; i++) {
    Vresult[i] = temp_result[i];
  }/*endfor*/
}




void InvertOrthonormal (GRAPHICS_MATRIX M, GRAPHICS_MATRIX Mresult)
{
  GRAPHICS_MATRIX temp_result;	/* in case of argument-duplication! */
  int i,j;

  for (i=0; i<NDIMS; i++) {
    for (j=0; j<NDIMS; j++) {
      temp_result[i][j] = M[j][i];
    }/*endfor*/
  }/*endfor*/

  for (i=0; i<NDIMS; i++) {
    temp_result[i][NDIMS] = 0.0;
  }/*endfor*/

  for (i=0; i<NDIMS; i++) {
    temp_result[NDIMS][i] = 0.0;
    for (j=0; j<NDIMS; j++) {
      temp_result[NDIMS][i] += - M[NDIMS][j] * M[i][j];
       /* because M[i][j] is Mtranspose[j][i] */
    }/*endfor*/
  }/*endfor*/

  temp_result[NDIMS][NDIMS] = 1.0;

  for (i=0; i<NDIMS+1; i++) {
    for (j=0; j<NDIMS+1; j++) {
      Mresult[i][j] = temp_result[i][j];
    }/*endfor*/
  }/*endfor*/
}




void AddGraphicsVectors (GRAPHICS_VECTOR V1, GRAPHICS_VECTOR V2,
                          GRAPHICS_VECTOR Vresult)
{
  int i;

  for (i=0; i<NDIMS+1; i++) {
    Vresult[i] = V1[i] + V2[i];
  }/*endfor*/
}




void beep (void)
{
  putchar(7); fflush(stdout);
}