//
// Autor: Matthias Huether (huether@dfki.uni-sb.de)
//
// Beschreibung: Funktionen fuer Matrizenrechnung
//

//
// M sei eine Matrix, wie sie in den Animationsskripten vorkommt.
//
// M = TR  =>  M^{-1} = (TR)^{-1} = R^{-1}T^{-1},
// wobei T: Translationsmatrix, R: Rotationsmatrix
// M hat folgende Gestalt:
//         ( 1  0  0  x )
//     M = ( 0  a -1  y )
//         ( 0  1  b  z )
//         ( 0  0  0  1 )
// Die Translation steht in der 4. SPALTE der Matrix M, der Rest ist Rotation.

// Die Rotationsmatrix erhaelt man, indem man die Translation aus M 
// ausschneidet, d.h. M[1][4]=M[2][4]=M[3][4]=0:
//         ( 1  0  0  0 )
//     R = ( 0  a -1  0 )
//         ( 0  1  b  0 )
//         ( 0  0  0  1 )

// Die Translationsmatrix erhaelt man, indem man in M die Rotation durch die
// Einheismatrix ersetzt:
//         ( 1  0  0  x )
//     T = ( 0  1  0  y )
//         ( 0  0  1  z )
//         ( 0  0  0  1 )

// Die inverse Rotationsmatrix Ri ergibt sich aus der Rotationsmatrix R durch
// Transponieren von R:
//         ( 1  0  0  0 )
//    Ri = ( 0  a  1  0 )
//         ( 0 -1  b  0 )
//         ( 0  0  0  1 )

// Die inverse Translationsmatrix Ti ist die negative Translation:
//         ( 1  0  0 -x )
//    Ti = ( 0  1  0 -y )
//         ( 0  0  1 -z )
//         ( 0  0  0  1 )


#include <stdio.h>
#include <math.h>
#include <ri.h>

#define min 1e-06   // Zahl<min -> Zahl=0
#define pi 3.14159265359


// Ausgabe einer nxn-Matrix
void PrintMatrix (RtMatrix M,short d=4)
{
  short i,j;
  printf("\n");
  for (i=0;i<d;++i)
    {
      printf("\t( ");
      for (j=0;j<d-1;++j)
	printf("%9.6g\t",fabs(M[i][j])<min?0:M[i][j]);
      printf("%9.6g )\n",fabs(M[i][d-1])<min?0:M[i][d-1]);
    }
  printf("\n");
}

// Ausgabe eines Vektors in Zeilenform
void PrintVector (RtFloat *u, short d=3)
{
  short i;
  printf("\n\t ( ");
  for (i=0;i<d-1;++i)
    printf("%g, ",fabs(u[i])<min?0:u[i]);
  printf("%g )\n",fabs(u[d-1])<min?0:u[d-1]);
}

// Berechnung des Skalarprodukts <.,.> zwischen zwei Vektoren u, v;
// <u,v> = u[1]*v[1] + u[2]*v[2] + ... + u[n]*v[n]
RtFloat ScalarProduct (RtFloat *u, RtFloat *v, short d=3)
{
  RtFloat s=0.0;
  short i;
  for (i=0;i<d;++i)
    s += u[i]*v[i];
  printf("Skalarprodukt = %g\n",s);
  return s;
}

// Berechnung des Betrags eines Vektors
RtFloat Norm (RtFloat *u, short d=3)
{
  RtFloat m;
  m = sqrt(ScalarProduct(u,u,d)); 	
  printf("Betrag = %g\n",m);
  return m;
}

// Berechnung des Winkels zwischen zwei Vektoren u, v.
// Verfahren: cos(u,v) = <u,v>/(|u|*|v|), wobei <.,.> das Skalarprodukt
//   zwischen zwei Vektoren und |.| der Betrag eines Vektors sind.
RtFloat AngleBetweenVectors (RtFloat *u, RtFloat *v, short d=3)
{
  RtFloat p;
  p = acos(ScalarProduct(u,v,d)/(Norm(u,d)*Norm(v,d)));
  printf("Winkel = %g\n",p*180.0/3.1415925359);
  return p;
}

// Liefert die Einheitsmatrix in Id zurueck.
void IdentityMatrix (RtMatrix *Id)
{
  short i,j;
  for (i=0;i<4;++i)
    for (j=0;j<4;++j)
      (*Id)[i][j] = (i==j);
}

// Multiplizieren zweier 4x4-Matrizen M, N mit dem Schulverfahren.
// Das Ergebnis der Multiplikation von M und N steht in der Matrix MN.
void MultiplyMatrices (RtMatrix M, RtMatrix N, RtMatrix *MN)
{
  short i,j,k;
  for (i=0;i<4;++i)
    for (j=0;j<4;++j)
      {
	(*MN)[i][j] = 0;
	for (k=0;k<4;++k)
	  (*MN)[i][j] += M[i][k]*N[k][j];
      }
}

// Multiplikation einer 4x4-Matrix M und eines Spaltenvektors u
void MultiplyMatrixVector (RtMatrix M, RtFloat *u, RtFloat *Mu)
{
  short i,j;
  RtFloat vec[4];

  for (i=0;i<3;++i)
    vec[i] = u[i];
  vec[3] = 1;   

  for (i=0;i<4;++i)
    {
      Mu[i] = 0;
      for (j=0;j<4;++j)
	Mu[i] += M[i][j]*vec[j];
    }
}

// Multiplikation eines Zeilenvektors u und einer 4x4-Matrix M
void MultiplyVectorMatrix (RtFloat *u, RtMatrix M, RtFloat *uM)
{
  short i,j;
  RtFloat vec[4];

  for (i=0;i<3;++i)
    vec[i] = u[i];
  vec[3] = 1;

  for (i=0;i<4;++i)
    {
      uM[i] = 0;
      for (j=0;j<4;++j)
	uM[i] += u[j]*M[j][i];
    }
}
  
// Transponieren einer 4x4-Matrix M; Mt: transponierte Matrix M
void TransposeMatrix (RtMatrix M, RtMatrix *Mt)
{
  short i,j;
  //printf("transpose: M = "); PrintMatrix(M);
  for (i=0;i<4;++i)
    for (j=0;j<4;++j)
      (*Mt)[i][j] = M[j][i];
  //printf("transpose: Mt = "); PrintMatrix(*Mt);
}

// erzeugt neue Translationsmatrix
// Argumente: Zeiger auf 3 Floatwerte der Translation,
//            Zeiger auf RtMatrix fuer die Rueckgabe
// Ergebnis: Translationsmatrix T
void NewTranslationMatrix (RtFloat *tval, RtMatrix *T)
{
  short i;
  IdentityMatrix(T);
  for (i=0;i<3;++i)
    (*T)[i][3] = tval[i];
}

// Berechnung der Rotationsmatrix R aus M (Verfahren s. oben)
void RotationMatrix (RtMatrix M, RtMatrix *R, short d=4)
{
  short i,j;
  for (i=0;i<4;++i)
    {
      for (j=0;j<3;++j)
	(*R)[i][j] = M[i][j];
      (*R)[i][3] = 0;
    }
  (*R)[3][3] = M[3][3];
}

// Berechnung der Translationsmatrix T aus M (Verfahren s. oben)
void TranslationMatrix (RtMatrix M, RtMatrix *T)
{
  short i;
  RtFloat tval[3];   // Werte fuer die Translation
  for (i=0;i<3;++i)
    tval[i] = M[i][3];
  NewTranslationMatrix (tval,T);
}

// Berechnung der inversen Rotationsmatrix Ri aus der Rotationsmatrix R
//   (Verfahren s. oben)
void InverseRotationMatrix (RtMatrix R, RtMatrix *Ri)
{
  //printf("R = "); PrintMatrix(R);
  TransposeMatrix (R,Ri);
}

// Berechnung der inversen Translationsmatrix Ti aus der Translationsmatrix T
//   (Verfahren s. oben)
void InverseTranslationMatrix (RtMatrix T, RtMatrix *Ti)
{
  short i;
  //printf("T = "); PrintMatrix(T);
  IdentityMatrix (Ti);
  for (i=0;i<3;++i)
    (*Ti)[i][3] = -T[i][3];
}

// Berechnung der inversen Matrix Mi zu M (Verfahren s. oben)
void InverseMatrix (RtMatrix M, RtMatrix *Mi)
{
  RtMatrix R,Ri,T,Ti;
  RotationMatrix (M,&R);
  //printf ("# R = "); PrintMatrix(Ri);
  InverseRotationMatrix (R,&Ri);
  //printf("Ri = "); PrintMatrix(Ri);
  TranslationMatrix (M,&T);
  InverseTranslationMatrix (T,&Ti);
  //printf("Ti = "); PrintMatrix(Ti);
  MultiplyMatrices (Ri,Ti,Mi);
}


// erzeugt Transformationsmatrix anhand einer Rotationsachse und einem 
//   Rotationswinkel
// Argumente: zwei Punkte, die die Rotationsachse bestimmen (startpoint, 
//   endpoint)
//            Rotationswinkel im Bogenmass (angle)
//            Zeiger auf RtMatrix fuer Rueckgabewert
// Ergebnis: Transformationsmatrix M
// Verfahren aus: Steven Harrington; Computer Graphics, A Programming 
//   Approach; Second Edition, 1987; McGraw-Hill Book Company; Seite 256ff

void BuildTransformationMatrix ( RtFloat *startpoint, RtFloat *endpoint, RtFloat angle, RtMatrix *M)
{
  // T: Translationsmatrix, Rx,Ry,Rz : Rotation um die x-,y-,z-Achse,
  // H: Hilfsmatrix
  // a,b,c: bestimmen Richtung der Rotationsachse
  RtMatrix T,Ti,Rx,Rxi,Ry,Ryi,Rz,Rzi,H;
  RtFloat a,b,c,v,l;

  NewTranslationMatrix (startpoint,&T);
  InverseTranslationMatrix (T,&Ti);
  
  //angle = -angle;
  a = endpoint[0]-startpoint[0];
  b = endpoint[1]-startpoint[1];
  c = endpoint[2]-startpoint[2];
  v = sqrt(b*b+c*c);
  l = sqrt(a*a+b*b+c*c);

  if (c<0)
    {
      a = -a;
      b = -b;
      c = -c;
      angle = -angle;
    }

  IdentityMatrix(&Rx);
  Rx[1][1] = Rx[2][2] = (v!=0 ? c/v : 1);  // cos
  Rx[1][2] = (v!=0 ? b/v : 0);             // sin
  Rx[2][1] = -Rx[1][2];
  TransposeMatrix (Rx,&Rxi);
  
  IdentityMatrix(&Ry);
  Ry[0][0] = Ry[2][2] = (l!=0 ? v/l : 1);  // cos
  Ry[0][2] = (l!=0 ? a/l : 0);             // sin
  Ry[2][0] = -Ry[0][2];
  TransposeMatrix(Ry,&Ryi);

  IdentityMatrix(&Rz);
  Rz[0][0] = Rz[1][1] = cos(angle);
  Rz[0][1] = -sin(angle);
  Rz[1][0] = -Rz[0][1];

  MultiplyMatrices (T,Rx,&H);
  MultiplyMatrices (H,Ry,M);
  MultiplyMatrices (*M,Rz,&H);
  MultiplyMatrices (H,Ryi,M);
  MultiplyMatrices (*M,Rxi,&H);
  MultiplyMatrices (H,Ti,M);
}

void AimCamera(RtFloat *startpoint, RtFloat *endpoint, RtFloat roll, RtMatrix *M)
{
  // T: Translationsmatrix, Rx,Ry,Rz : Rotation um die x-,y-,z-Achse,
  // H: Hilfsmatrix
  // a,b,c: bestimmen Richtung der Rotationsachse
  RtMatrix T,Ti,Tti,R,Rx,Ry,Rz,Rzi,Rr,H;
  RtFloat a,b,c,v,l,u,alpha,beta,angle;
  
  NewTranslationMatrix (startpoint,&T);
  InverseTranslationMatrix (T,&Ti);
  TransposeMatrix(Ti,&Tti);

  angle = roll*pi/180.0;
  a = endpoint[0]-startpoint[0];
  b = endpoint[1]-startpoint[1];
  c = endpoint[2]-startpoint[2];
  v = sqrt(b*b+c*c);
  l = sqrt(a*a+b*b+c*c);
  u = sqrt(a*a+c*c);

  IdentityMatrix(&Rx);
  if (b!=0)
  {
  Rx[1][1] = Rx[2][2] = (l!=0 ? u/l : 1);  // cos v
  Rx[1][2] = (l!=0 ? b/l : 0);             // sin a
  Rx[2][1] = -Rx[1][2];
  }
  IdentityMatrix(&Ry);
  Ry[0][0] = Ry[2][2] = (u!=0 ? c/u : 1);  // cos
  Ry[0][2] = (u!=0 ? a/u : 0);             // sin
  Ry[2][0] = -Ry[0][2];

  IdentityMatrix(&Rz);
  Rz[0][0] = Rz[1][1] = cos(angle);
  Rz[0][1] = -sin(angle);
  Rz[1][0] = -Rz[0][1];
  TransposeMatrix(Rz,&Rzi);

  
  MultiplyMatrices (Ry,Rx,&R);

//  BuildTransformationMatrix(startpoint,endpoint,angle,&H);
//  RotationMatrix (H, &Rr,4);
  
  MultiplyMatrices(R,Rzi,&H);

  MultiplyMatrices (Tti,H,M);

}