/* vectors.c
 * Giram - A GPLed Modelling Program.
 * Copyright (C) 1999 David Odin <David@dindinx.org>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

#include <math.h>
#include "vectors.h"

/*****************************************************************************
*  MidPoint
******************************************************************************/
void MidPoint(Vector Result, Vector a, Vector b)
{
  Result[0] = (a[0]+b[0]) / 2.0;
  Result[1] = (a[1]+b[1]) / 2.0;
  Result[2] = (a[2]+b[2]) / 2.0;
}

/*****************************************************************************
*  VCross
******************************************************************************/
void VCross(Vector a, Vector b, Vector c)
{
  a[0]=b[1]*c[2]-b[2]*c[1];
  a[1]=b[2]*c[0]-b[0]*c[2];
  a[2]=b[0]*c[1]-b[1]*c[0];
}

/*****************************************************************************
*  MIdentity
******************************************************************************/
void MIdentity(Matrix Mat)
{
  int i,j;

  for (i=0 ; i<4 ; i++)
    for (j=0 ; j<4 ; j++)
      if (i==j)
        Mat[i][j] = 1.0;
      else
        Mat[i][j] = 0.0;
}

/*****************************************************************************
*  MTranspose
******************************************************************************/
void MTranspose(Matrix Result, Matrix Src)
{
  int i;
  Matrix TmpMat;

  for (i=0 ; i<4 ; i++)
  {
    TmpMat[i][0] = Src[0][i];
    TmpMat[i][1] = Src[1][i];
    TmpMat[i][2] = Src[2][i];
    TmpMat[i][3] = Src[3][i];
  }
  for (i=0 ; i<4 ; i++)
  {
    Result[i][0] = TmpMat[i][0];
    Result[i][1] = TmpMat[i][1];
    Result[i][2] = TmpMat[i][2];
    Result[i][3] = TmpMat[i][3];
  }
}

/*****************************************************************************
*  MTimes
******************************************************************************/
void MTimes(Matrix Result, Matrix M1, Matrix M2)
{
  int i, j, k;
  Matrix tmp;

  for (i=0 ; i<4 ; i++)
    for (j=0 ; j<4 ; j++) 
    {
      tmp[j][i] = 0.0;
      for (k=0 ; k<4 ; k++)
        tmp[j][i] += M1[k][i] * M2[j][k];
    }
    for (i=0 ; i<4 ; i++)
      for (j=0 ; j<4 ; j++)
        Result[i][j] = tmp[i][j];
}

/*****************************************************************************
*  MInverse
******************************************************************************/
void MInverse(Matrix Result, Matrix M)
{
  double d00, d01, d02, d03;
  double d10, d11, d12, d13;
  double d20, d21, d22, d23;
  double d30, d31, d32, d33;
  double m00, m01, m02, m03;
  double m10, m11, m12, m13;
  double m20, m21, m22, m23;
  double m30, m31, m32, m33;
  double D;

  m00 = M[0][0];  m01 = M[0][1];  m02 = M[0][2];  m03 = M[0][3];
  m10 = M[1][0];  m11 = M[1][1];  m12 = M[1][2];  m13 = M[1][3];
  m20 = M[2][0];  m21 = M[2][1];  m22 = M[2][2];  m23 = M[2][3];
  m30 = M[3][0];  m31 = M[3][1];  m32 = M[3][2];  m33 = M[3][3];

  d00 = m11*m22*m33 + m12*m23*m31 + m13*m21*m32
       -m31*m22*m13 - m32*m23*m11 - m33*m21*m12;
  d01 = m10*m22*m33 + m12*m23*m30 + m13*m20*m32
       -m30*m22*m13 - m32*m23*m10 - m33*m20*m12;
  d02 = m10*m21*m33 + m11*m23*m30 + m13*m20*m31
       -m30*m21*m13 - m31*m23*m10 - m33*m20*m11;
  d03 = m10*m21*m32 + m11*m22*m30 + m12*m20*m31
       -m30*m21*m12 - m31*m22*m10 - m32*m20*m11;

  d10 = m01*m22*m33 + m02*m23*m31 + m03*m21*m32
       -m31*m22*m03 - m32*m23*m01 - m33*m21*m02;
  d11 = m00*m22*m33 + m02*m23*m30 + m03*m20*m32
       -m30*m22*m03 - m32*m23*m00 - m33*m20*m02;
  d12 = m00*m21*m33 + m01*m23*m30 + m03*m20*m31
       -m30*m21*m03 - m31*m23*m00 - m33*m20*m01;
  d13 = m00*m21*m32 + m01*m22*m30 + m02*m20*m31
       -m30*m21*m02 - m31*m22*m00 - m32*m20*m01;

  d20 = m01*m12*m33 + m02*m13*m31 + m03*m11*m32
       -m31*m12*m03 - m32*m13*m01 - m33*m11*m02;
  d21 = m00*m12*m33 + m02*m13*m30 + m03*m10*m32
       -m30*m12*m03 - m32*m13*m00 - m33*m10*m02;
  d22 = m00*m11*m33 + m01*m13*m30 + m03*m10*m31
       -m30*m11*m03 - m31*m13*m00 - m33*m10*m01;
  d23 = m00*m11*m32 + m01*m12*m30 + m02*m10*m31
       -m30*m11*m02 - m31*m12*m00 - m32*m10*m01;

  d30 = m01*m12*m23 + m02*m13*m21 + m03*m11*m22
       -m21*m12*m03 - m22*m13*m01 - m23*m11*m02;
  d31 = m00*m12*m23 + m02*m13*m20 + m03*m10*m22
       -m20*m12*m03 - m22*m13*m00 - m23*m10*m02;
  d32 = m00*m11*m23 + m01*m13*m20 + m03*m10*m21
       -m20*m11*m03 - m21*m13*m00 - m23*m10*m01;
  d33 = m00*m11*m22 + m01*m12*m20 + m02*m10*m21
       -m20*m11*m02 - m21*m12*m00 - m22*m10*m01;

  D = m00*d00 - m01*d01 + m02*d02 - m03*d03;

  Result[0][0] =  d00/D; Result[0][1] = -d10/D; Result[0][2] =  d20/D; Result[0][3] = -d30/D;
  Result[1][0] = -d01/D; Result[1][1] =  d11/D; Result[1][2] = -d21/D; Result[1][3] =  d31/D;
  Result[2][0] =  d02/D; Result[2][1] = -d12/D; Result[2][2] =  d22/D; Result[2][3] = -d32/D;
  Result[3][0] = -d03/D; Result[3][1] =  d13/D; Result[3][2] = -d23/D; Result[3][3] =  d33/D;
}

/*****************************************************************************
*  MEvaluatePoint
******************************************************************************/
void MEvaluatePoint(Vector Result, TransformStruct *Trans, Vector Source)
{
  int i;
  Vector Tmp;

  for (i=0 ; i<3 ; i++)
  {
    Tmp[i] = Source[0] * Trans->Direct[0][i] +
             Source[1] * Trans->Direct[1][i] +
             Source[2] * Trans->Direct[2][i] + 
                         Trans->Direct[3][i];
  }
  Result[0] = Tmp[0];
  Result[1] = Tmp[1];
  Result[2] = Tmp[2];
}

/*****************************************************************************
*  MInverseEvaluatePoint
******************************************************************************/
void MInverseEvaluatePoint(Vector Result, TransformStruct *Trans, Vector Source)
{
  int i;
  Vector Tmp;

  for (i=0 ; i<3 ; i++)
  {
    Tmp[i] = Source[0] * Trans->Inverse[0][i] +
             Source[1] * Trans->Inverse[1][i] +
             Source[2] * Trans->Inverse[2][i] + 
                         Trans->Inverse[3][i];
  }
  Result[0] = Tmp[0];
  Result[1] = Tmp[1];
  Result[2] = Tmp[2];
}

/*****************************************************************************
*  MEvaluateVector
******************************************************************************/
void MEvaluateVector(Vector Result, TransformStruct *Trans, Vector Source)
{
  int i;
  Vector Tmp;

  for (i=0 ; i<3 ; i++)
  {
    Tmp[i] = Source[0] * Trans->Direct[0][i] +
             Source[1] * Trans->Direct[1][i] +
             Source[2] * Trans->Direct[2][i];
  }
  Result[0] = Tmp[0];
  Result[1] = Tmp[1];
  Result[2] = Tmp[2];
}

/*****************************************************************************
*  MInverseEvaluateVector
******************************************************************************/
void MInverseEvaluateVector(Vector Result, TransformStruct *Trans, Vector Source)
{
  int i;
  Vector Tmp;

  for (i=0 ; i<3 ; i++)
  {
    Tmp[i] = Source[0] * Trans->Inverse[0][i] +
             Source[1] * Trans->Inverse[1][i] +
             Source[2] * Trans->Inverse[2][i];
  }
  Result[0] = Tmp[0];
  Result[1] = Tmp[1];
  Result[2] = Tmp[2];
}

/*****************************************************************************
*  ComputeTranslateTrans
******************************************************************************/
void ComputeTranslateTrans(TransformStruct *Trans, Vector Vect)
{
  Trans->Inverse[0][0] = Trans->Direct[0][0] = 1.0;
  Trans->Inverse[0][1] = Trans->Direct[0][1] = 0.0;
  Trans->Inverse[0][2] = Trans->Direct[0][2] = 0.0;
  Trans->Inverse[0][3] = Trans->Direct[0][3] = 0.0;
  Trans->Inverse[1][0] = Trans->Direct[1][0] = 0.0;
  Trans->Inverse[1][1] = Trans->Direct[1][1] = 1.0;
  Trans->Inverse[1][2] = Trans->Direct[1][2] = 0.0;
  Trans->Inverse[1][3] = Trans->Direct[1][3] = 0.0;
  Trans->Inverse[2][0] = Trans->Direct[2][0] = 0.0;
  Trans->Inverse[2][1] = Trans->Direct[2][1] = 0.0;
  Trans->Inverse[2][2] = Trans->Direct[2][2] = 1.0;
  Trans->Inverse[2][3] = Trans->Direct[2][3] = 0.0;
  Trans->Inverse[3][0] = -Vect[0]; Trans->Direct[3][0] = Vect[0];
  Trans->Inverse[3][1] = -Vect[1]; Trans->Direct[3][1] = Vect[1];
  Trans->Inverse[3][2] = -Vect[2]; Trans->Direct[3][2] = Vect[2];
  Trans->Inverse[3][3] = Trans->Direct[3][3] = 1.0;
}

/*****************************************************************************
*  ComputeRotateTrans
******************************************************************************/
void ComputeRotateTrans(TransformStruct *Trans, Vector Vect)
{ 
  Matrix TmpMat;
  double cosx, cosy, cosz, sinx, siny, sinz;

  MIdentity(Trans->Direct);
  cosx = cos(Vect[0] * M_PI / 180.0);
  sinx = sin(Vect[0] * M_PI / 180.0);
  cosy = cos(Vect[1] * M_PI / 180.0);
  siny = sin(Vect[1] * M_PI / 180.0);
  cosz = cos(Vect[2] * M_PI / 180.0);
  sinz = sin(Vect[2] * M_PI / 180.0);

  Trans->Direct[0][0] = cosz;
  Trans->Direct[1][1] = cosz;
  Trans->Direct[0][1] = sinz;
  Trans->Direct[1][0] = -sinz;
  MTranspose(Trans->Inverse, Trans->Direct);
  MIdentity(TmpMat);
  TmpMat[0][0] = cosy;
  TmpMat[2][2] = cosy;
  TmpMat[0][2] = -siny;
  TmpMat[2][0] = siny;
  MTimes(Trans->Direct, Trans->Direct, TmpMat);
  MTranspose(TmpMat, TmpMat);
  MTimes(Trans->Inverse, TmpMat, Trans->Inverse);
  MIdentity(TmpMat);
  TmpMat[1][1] = cosx;
  TmpMat[2][2] = cosx;
  TmpMat[1][2] = sinx;
  TmpMat[2][1] = -sinx;
  MTimes(Trans->Direct, Trans->Direct, TmpMat);
  MTranspose(TmpMat, TmpMat);
  MTimes(Trans->Inverse, TmpMat, Trans->Inverse);
}

/*****************************************************************************
*  ComputeScaleTrans
******************************************************************************/
void ComputeScaleTrans(TransformStruct *Trans, Vector Vect)
{
  Trans->Inverse[0][0] = 1.0 / Vect[0]; Trans->Direct[0][0] = Vect[0];
  Trans->Inverse[0][1] = Trans->Direct[0][1] = 0.0;
  Trans->Inverse[0][2] = Trans->Direct[0][2] = 0.0;
  Trans->Inverse[0][3] = Trans->Direct[0][3] = 0.0;
  Trans->Inverse[1][0] = Trans->Direct[1][0] = 0.0;
  Trans->Inverse[1][1] = 1.0 / Vect[1]; Trans->Direct[1][1] = Vect[1];
  Trans->Inverse[1][2] = Trans->Direct[1][2] = 0.0;
  Trans->Inverse[1][3] = Trans->Direct[1][3] = 0.0;
  Trans->Inverse[2][0] = Trans->Direct[2][0] = 0.0;
  Trans->Inverse[2][1] = Trans->Direct[2][1] = 0.0;
  Trans->Inverse[2][2] = 1.0 / Vect[2]; Trans->Direct[2][2] = Vect[2];
  Trans->Inverse[2][3] = Trans->Direct[2][3] = 0.0;
  Trans->Inverse[3][0] = Trans->Direct[3][0] = 0.0;
  Trans->Inverse[3][1] = Trans->Direct[3][1] = 0.0;
  Trans->Inverse[3][2] = Trans->Direct[3][2] = 0.0;
  Trans->Inverse[3][3] = Trans->Direct[3][3] = 1.0;
}

/*****************************************************************************
*  ComposeTrans
******************************************************************************/
void ComposeTrans(TransformStruct *Trans, TransformStruct *NewTrans)
{
  MTimes(Trans->Direct, NewTrans->Direct, Trans->Direct);
  MTimes(Trans->Inverse, Trans->Inverse, NewTrans->Inverse);
}