/*******************************************************************************
*                                                                              *
*                                   Viewmol                                    *
*                                                                              *
*                               M A T R I X . C                                *
*                                                                              *
*                 Copyright (c) Joerg-R. Hill, October 2003                    *
*                                                                              *
********************************************************************************
*
* $Id: matrix.c,v 1.6 2004/08/29 14:54:32 jrh Exp $
* $Log: matrix.c,v $
* Revision 1.6  2004/08/29 14:54:32  jrh
* Release 2.4.1
*
* Revision 1.5  2003/11/07 11:06:42  jrh
* Release 2.4
*
* Revision 1.4  2000/12/10 15:11:04  jrh
* Release 2.3
*
* Revision 1.3  1999/05/24 01:26:19  jrh
* Release 2.2.1
*
* Revision 1.2  1999/02/07 21:52:01  jrh
* Release 2.2
*
* Revision 1.1  1998/01/26 00:35:07  jrh
* Initial revision
*
*/
#include<math.h>
#include<stdio.h>
#include<GL/gl.h>
#include<GL/glu.h>
#include<X11/StringDefs.h>
#include "viewmol.h"

extern void quaternionToMatrix(GLenum, float *);

void getMatrix(double matrix[4][4]);
void multMatrix(double matrix1[4][4], double matrix2[4][4], double matrix3[4][4]);
void transformCoordinates(int, float input[4], float output[4]);
void makeMatrix(double, float, float, float, float mat[3][3]);

extern struct WINDOW windows[];
extern float *rotObject;

void getMatrix(double matrix[4][4])
{
  double mvmat[4][4], prmat[4][4];

  glGetDoublev(GL_MODELVIEW_MATRIX, &mvmat[0][0]);
  glGetDoublev(GL_PROJECTION_MATRIX, &prmat[0][0]);
  multMatrix(mvmat, prmat, matrix);
}

void multMatrix(double matrix1[4][4], double matrix2[4][4], double matrix3[4][4])
{
  register int i, j, k;

  for (i=0; i<4; i++)
  {
    for (j=0; j<4; j++)
    {
      matrix3[i][j]=0.0;
      for (k=0; k<4; k++)
        matrix3[i][j]+=matrix1[i][k]*matrix2[k][j];
    }
  }
}

void transposeMatrix(double matrix[4][4])
{
  double help;
  register int i, j;

  for (i=0; i<4; i++)
  {
    for (j=i+1; j<4; j++)
    {
      help=matrix[i][j];
      matrix[i][j]=matrix[j][i];
      matrix[j][i]=help;
    }
  }
}

void transformCoordinates(int which, float input[4], float output[4])
{
  float matrix[4][4];

  glPushMatrix();
  glLoadIdentity();
  quaternionToMatrix(GL_MODELVIEW, &rotObject[4*which]);
  glGetFloatv(GL_MODELVIEW_MATRIX, &matrix[0][0]);
  glPopMatrix();

  output[0]=matrix[0][0]*input[0]+matrix[1][0]*input[1]+matrix[2][0]*input[2]+matrix[3][0]*input[3];
  output[1]=matrix[0][1]*input[0]+matrix[1][1]*input[1]+matrix[2][1]*input[2]+matrix[3][1]*input[3];
  output[2]=matrix[0][2]*input[0]+matrix[1][2]*input[1]+matrix[2][2]*input[2]+matrix[3][2]*input[3];
  output[3]=matrix[0][3]*input[0]+matrix[1][3]*input[1]+matrix[2][3]*input[2]+matrix[3][3]*input[3];
}

void getScreenCoordinates(double xw, double yw, double zw, double *xs,
                          double *ys, double *zs)
{
  GLdouble mvMatrix[16], prMatrix[16];
  GLint viewport[4];

  glGetDoublev(GL_MODELVIEW_MATRIX, mvMatrix);
  glGetDoublev(GL_PROJECTION_MATRIX, prMatrix);
  glGetIntegerv(GL_VIEWPORT, viewport);
  gluProject(xw, yw, zw, mvMatrix, prMatrix, viewport, xs, ys, zs);
  *ys=(double)viewport[3]-(*ys);
}

void getWorldCoordinates(double xs, double ys, double zs, double *xw,
                         double *yw, double *zw)
{
  GLdouble mvMatrix[16], prMatrix[16];
  GLint viewport[4];

  glGetDoublev(GL_MODELVIEW_MATRIX, mvMatrix);
  glGetDoublev(GL_PROJECTION_MATRIX, prMatrix);
  glGetIntegerv(GL_VIEWPORT, viewport);
  gluUnProject(xs, ys, zs, mvMatrix, prMatrix, viewport, xw, yw, zw);
}

void rotateAxis(float axis[3], float about[3], double angle)
{
  float mat[3][3], s1, s2;

  makeMatrix((-angle), about[0], about[1], about[2], mat);
  s1=axis[0];
  s2=axis[1];
  axis[0]=mat[0][0]*s1+mat[1][0]*s2+mat[2][0]*axis[2];
  axis[1]=mat[0][1]*s1+mat[1][1]*s2+mat[2][1]*axis[2];
  axis[2]=mat[0][2]*s1+mat[1][2]*s2+mat[2][2]*axis[2];
}

void makeMatrix(double angle, float x, float y, float z, float mat[3][3])
{
  double torad=atan(1.0)/45.0;
  float mag, s, c;
  float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;

  angle*=torad;
  s=(float)sin(angle);
  c=(float)cos(angle);

  mag=1.0/sqrt(x*x+y*y+z*z);
  x*=mag;
  y*=mag;
  z*=mag;

  xx=x*x;
  yy=y*y;
  zz=z*z;
  xy=x*y;
  yz=y*z;
  zx=z*x;
  xs=x*s;
  ys=y*s;
  zs=z*s;
  one_c=1.0F-c;

  mat[0][0]=(one_c*xx)+c;
  mat[0][1]=(one_c*xy)-zs;
  mat[0][2]=(one_c*zx)+ys;

  mat[1][0]=(one_c*xy)+zs;
  mat[1][1]=(one_c*yy)+c;
  mat[1][2]=(one_c*yz)-xs;

  mat[2][0]=(one_c*zx)-ys;
  mat[2][1]=(one_c*yz)+xs;
  mat[2][2]=(one_c*zz)+c;
}

void printMatrix(int which)
{
  double matrix[4][4];
  int i;

  glGetDoublev(which, &matrix[0][0]);
  for (i=0; i<4; i++)
    printf("%f  %f  %f  %f\n", matrix[i][0], matrix[i][1], matrix[i][2], matrix[i][3]);
}
