#include <string.h>
#include <math.h>
#include "dpltypes.h"

/*{{{  banner*/
/* **************************************************

   Copyright DIVISION Limited (c) 1994
   All rights reserved


   File    : matrix.c
   Project : dpl interface
   Author  : PJA
   Date    : 22/06/94

   Function: matrix manipulation for applications

   History : Rev 1.0, 23 / 06 / 1994

**************************** */
/*}}}  */

/*{{{  low-level matrix manipulations*/
#define degtorad(f) ((f)*(float32) 0.017453292)

static int dpl_DegAngles=1;

/*{{{  static void sinCos( float32 *s, float32 *c, float32 degrees )*/
static void sinCos( float32 *s, float32 *c, float32 degrees )
{
  float32 angle;

  if (dpl_DegAngles)
    angle=degtorad(degrees);
  else
    angle=degrees;

  *s=(float32) sin((double) angle);
  *c=(float32) cos((double) angle);
}
/*}}}  */

/*{{{  #define _idmatrix(m)*/

#define _idmatrix(m) \
m[0][0] = 1; m[0][1]=0; m[0][2]=0; m[0][3]=0; \
m[1][0] = 0; m[1][1]=1; m[1][2]=0; m[1][3]=0; \
m[2][0] = 0; m[2][1]=0; m[2][2]=1; m[2][3]=0; \
m[3][0] = 0; m[3][1]=0; m[3][2]=0; m[3][3]=1
/*}}}  */
/*{{{  static void _concatenate ( dpl_MATRIX c, dpl_MATRIX a, dpl_MATRIX b )*/
static void _concatenate ( dpl_MATRIX c, dpl_MATRIX a, dpl_MATRIX b )
{
  dpl_MATRIX dest;

  register int32 n;
  register float32 sx, sy, sz, sw;
  register float32 *s=(float32 *)a, *d=(float32 *)dest;

  for (n=0; n<4; n++ ) {
    sx =s[0]; sy =s[1]; sz =s[2]; sw=s[3];
    d[0] = (sx*b[0][0]) + (sy*b[1][0]) + (sz*b[2][0]) + (sw*b[3][0]);
    d[1] = (sx*b[0][1]) + (sy*b[1][1]) + (sz*b[2][1]) + (sw*b[3][1]);
    d[2] = (sx*b[0][2]) + (sy*b[1][2]) + (sz*b[2][2]) + (sw*b[3][2]);
    d[3] = (sx*b[0][3]) + (sy*b[1][3]) + (sz*b[2][3]) + (sw*b[3][3]);
    s+=4; d+=4;
  }
  memcpy ( c, dest, sizeof(dpl_MATRIX));
}
/*}}}  */

/*{{{  static void _rotX (dpl_MATRIX m, float32 angle )*/
static void _rotX (dpl_MATRIX m, float32 angle )
{
  /*{{{  picture of matrices*/
  /*
  
  [  a  b  cc d  ] [ 1  0  0  0 ]
  [  e  f  g  h  ] [ 0  c  s  0 ]
  [  i  j  k  l  ] [ 0  -s c  0 ]
  [  m  n  o  p  ] [ 0  0  0  1 ]
  
  forward
  
  [ a, b.c - cc.s, b.s + cc.c, d ]  etc.
  
  
  backward
  
  [ 1  0  0  0 ] [  a  b  cc d  ]
  [ 0  c  s  0 ] [  e  f  g  h  ]
  [ 0  -s c  0 ] [  i  j  k  l  ]
  [ 0  0  0  1 ] [  m  n  o  p  ]
  
  [  a  b  c  d ]
  [ c.e + s.i,  c.f + s.j, c.g + s.k, c.h + s.l ]
  s=-s;
  [ s.e + c.i,  s.f + c.j, s.g + c.k, s.h + c.l ]
  
  [  m  n  o  p ]
  
  */
  /*}}}  */
  float32 c, s;
  float32 b, cc;

  sinCos ( &s, &c, angle );

  /*{{{  row 0*/
  b = m[0][1];
  cc= m[0][2];
  
  m[0][1]=b*c - cc*s;
  m[0][2]=b*s + cc*c;
  /*}}}  */
  /*{{{  row 1*/
  b = m[1][1];
  cc= m[1][2];
  
  m[1][1]=b*c - cc*s;
  m[1][2]=b*s + cc*c;
  /*}}}  */
  /*{{{  row 2*/
  b = m[2][1];
  cc= m[2][2];
  
  m[2][1]=b*c - cc*s;
  m[2][2]=b*s + cc*c;
  /*}}}  */
  /*{{{  row 3*/
  b = m[3][1];
  cc= m[3][2];
  
  m[3][1]=b*c - cc*s;
  m[3][2]=b*s + cc*c;
  /*}}}  */
}
/*}}}  */
/*{{{  static void _rotY (dpl_MATRIX m, float32 angle )*/
static void _rotY (dpl_MATRIX m, float32 angle )
{
  /*{{{  old version assumes many horses*/
  /*
  float32 c, s;
  dpl_MATRIX t;
  
  _idmatrix (t);
  sinCos ( &s, &c, angle );
  
  t[0][0] = c; t[0][2] = -s;
  t[2][0] = s; t[2][2] =  c;
  
  if (post) _concatenate (m, m, t);
  else      _concatenate (m, t, m);
  */
  /*}}}  */
  /*{{{  picture of matrices*/
  /*
  
  [  a  b cc  d  ] [ c  0 -s  0 ]
  [  e  f  g  h  ] [ 0  1  0  0 ]
  [  i  j  k  l  ] [ s  0  c  0 ]
  [  m  n  o  p  ] [ 0  0  0  1 ]
  
  [ a.c+cc.s, b, -s.a + cc.c, d ]  etc.
  
  backward
  
  [ c  0 -s  0 ] [  a  b cc  d  ]
  [ 0  1  0  0 ] [  e  f  g  h  ]
  [ s  0  c  0 ] [  i  j  k  l  ]
  [ 0  0  0  1 ] [  m  n  o  p  ]
  
  row0  [ c.a - s.i, c.b - s.j, c.cc - s.k, c.d - s.l ]
  
  row 2 [ s.a + c.i, s.b + c.j, s.cc + c.k, s.d + c.l ]
  
  */
  /*}}}  */
  float32 c, s;
  float32 a, cc;

  sinCos ( &s, &c, angle );
  /*{{{  row 0*/
  a= m[0][0];
  cc=m[0][2];
  
  m[0][0]=a*c + cc*s;
  m[0][2]=cc*c - a*s;
  /*}}}  */
  /*{{{  row 1*/
  a= m[1][0];
  cc=m[1][2];
  
  m[1][0]=a*c + cc*s;
  m[1][2]=cc*c - a*s;
  /*}}}  */
  /*{{{  row 2*/
  a= m[2][0];
  cc=m[2][2];
  
  m[2][0]=a*c + cc*s;
  m[2][2]=cc*c - a*s;
  /*}}}  */
  /*{{{  row 3*/
  a= m[3][0];
  cc=m[3][2];
  
  m[3][0]=a*c + cc*s;
  m[3][2]=cc*c - a*s;
  /*}}}  */
}
/*}}}  */
/*{{{  static void _rotZ (dpl_MATRIX m, float32 angle )*/
static void _rotZ (dpl_MATRIX m, float32 angle )
{
  /*{{{  picture of matrices*/
  /*
  
	forward
  
  [  a  b  c  d  ] [ c  s  0  0 ]
  [  e  f  g  h  ] [-s  c  0  0 ]
  [  i  j  k  l  ] [ 0  0  1  0 ]
  [  m  n  o  p  ] [ 0  0  0  1 ]
  
  
  [ ac-bs  as+bc  c  d ]
  [ ec-fs  es+fc  g  h ]
  [ ic-js  is+jc  k  l ]
  [ mc-ns  ms+nc  o  p ]
  
	backward
  
  
  [ c  s  0  0 ]  [  a  b  c  d  ]
  [-s  c  0  0 ]  [  e  f  g  h  ]
  [ 0  0  1  0 ]  [  i  j  k  l  ]
  [ 0  0  0  1 ]  [  m  n  o  p  ]
  
  [ ca+se, cb+sf, cc+sg, cd+sh ]
  [ ce-sa, cf-sb, cg-sc, ch-sd ]
  [ i      j      k      l     ]
  [ m      n      o      p     ]
  
  */
  /*}}}  */
  float32 c, s, a, b;
  sinCos ( &s, &c, angle );
  /*{{{  row 0*/
  a = m[0][0];
  b = m[0][1];
  
  m[0][0]=a*c - b*s;
  m[0][1]=a*s + b*c;
  /*}}}  */
  /*{{{  row 1*/
  a = m[1][0];
  b = m[1][1];
  
  m[1][0]=a*c - b*s;
  m[1][1]=a*s + b*c;
  /*}}}  */
  /*{{{  row 2*/
  a = m[2][0];
  b = m[2][1];
  
  m[2][0]=a*c - b*s;
  m[2][1]=a*s + b*c;
  /*}}}  */
  /*{{{  row 3*/
  a = m[3][0];
  b = m[3][1];
  
  m[3][0]=a*c - b*s;
  m[3][1]=a*s + b*c;
  /*}}}  */
}
/*}}}  */
/*}}}  */
/*{{{  dpl interface matrix calls*/
/*{{{  void dpl_SetAngleMode( int degrees )*/
void dpl_SetAngleMode( int degrees )
{
  dpl_DegAngles=degrees;
}

/*}}}  */

/*{{{  void dpl_IdMatrix ( dpl_MATRIX m )*/
void dpl_IdMatrix ( dpl_MATRIX m )
{
  _idmatrix (m);
}
/*}}}  */
/*{{{  void dpl_Translate( dpl_MATRIX m, float32 dx, float32 dy, float32 dz )*/
void dpl_Translate( dpl_MATRIX m, float32 dx, float32 dy, float32 dz )
{
  dpl_MATRIX t;

  _idmatrix (t);

  t[dpl_W][dpl_X] = dx;
  t[dpl_W][dpl_Y] = dy;
  t[dpl_W][dpl_Z] = dz;

  _concatenate (m, m, t);
}
/*}}}  */
/*{{{  void dpl_Rotate   ( dpl_MATRIX m, float32 angle, int32 axis )*/
void dpl_Rotate   ( dpl_MATRIX m, float32 angle, int32 axis )
{
  if      (axis==dpl_X) _rotX ( m, angle );
  else if (axis==dpl_Y) _rotY ( m, angle );
  else                  _rotZ ( m, angle );
}
/*}}}  */
/*{{{  void dpl_Scale    ( dpl_MATRIX m, float32 x, float32 y, float32 z )*/
void dpl_Scale    ( dpl_MATRIX m, float32 x, float32 y, float32 z )
{
  dpl_MATRIX t;

  _idmatrix (t);

  t[dpl_X][dpl_X] = x;
  t[dpl_Y][dpl_Y] = y;
  t[dpl_Z][dpl_Z] = z;

  _concatenate (m, m, t);
}
/*}}}  */
/*{{{  void dpl_Concat   ( dpl_MATRIX m, dpl_MATRIX a, dpl_MATRIX b )*/
void dpl_Concat   ( dpl_MATRIX m, dpl_MATRIX a, dpl_MATRIX b )
{
  _concatenate ( m, a, b );
}
/*}}}  */

/*{{{  void dpl_Invert ( dpl_MATRIX inverse, dpl_MATRIX mat )*/
void dpl_Invert ( dpl_MATRIX inverse, dpl_MATRIX mat )
{
  /*
    This inverts graphics matrices only - these must be of the
    form    a b c 0
	    d e f 0
	    g h i 0
	    j k l 1

    We invert the upper 3x3 using cofactors, and apply the inverse
    translation in the bottom row.
  */

  register float32 a1, b1, c1,
		 a2, b2, c2,
		 a3, b3, c3,
		 i0, i1, i2, t,
		 tx, ty, tz;

  /*{{{  init inverse*/
  
  inverse [0][3]=0;
  inverse [1][3]=0;
  inverse [2][3]=0;
  inverse [3][3]=1;
  /*}}}  */

  /*{{{  pull a1, b1, c1 etc out of matrix*/
  a1=mat[0][0];
  b1=mat[0][1];
  c1=mat[0][2];
  
  a2=mat[1][0];
  b2=mat[1][1];
  c2=mat[1][2];
  
  a3=mat[2][0];
  b3=mat[2][1];
  c3=mat[2][2];
  
  tx=mat[3][0];
  ty=mat[3][1];
  tz=mat[3][2];
  /*}}}  */

  /*{{{  compute 9 cofactors, place transposed into matrix*/
  i0 =     ((b2*c3) - (b3*c2));
  i1 =    -((a2*c3) - (a3*c2));
  i2 =     ((a2*b3) - (a3*b2));
  
  t=1.0f / ((a1*i0) + (b1*i1) + (c1 * i2));
  
  i0*=t;
  i1*=t;
  i2*=t;
  
  inverse[0][0] = i0;
  inverse[1][0] = i1;
  inverse[2][0] = i2;
  inverse[3][0] = -((tx*i0) + (ty*i1) + (tz*i2));
  
  i0 =    -t*((b1*c3) - (b3*c1));
  i1 =     t*((a1*c3) - (a3*c1));
  i2 =    -t*((a1*b3) - (a3*b1));
  
  inverse[0][1] = i0;
  inverse[1][1] = i1;
  inverse[2][1] = i2;
  inverse[3][1] = -((tx*i0) + (ty*i1) + (tz*i2));
  
  i0 =    t*((b1*c2) - (b2*c1));
  i1 =   -t*((a1*c2) - (a2*c1));
  i2 =    t*((a1*b2) - (a2*b1));
  
  inverse[0][2] = i0;
  inverse[1][2] = i1;
  inverse[2][2] = i2;
  inverse[3][2] = -((tx*i0) + (ty*i1) + (tz*i2));
  
  /*}}}  */

}
/*}}}  */
/*{{{  void dpl_XformPoint ( dpl_POINT q, dpl_POINT p, dpl_MATRIX m )*/
void dpl_XformPoint ( dpl_POINT q, dpl_POINT p, dpl_MATRIX m )
{
  dpl_POINT r;
  int i;

  for (i=0; i<4; i++ ) {
    r[i]=(p[0]*m[0][i])+
	 (p[1]*m[1][i])+
	 (p[2]*m[2][i])+
	       m[3][i];
  }
  memcpy ( q, r, sizeof(dpl_POINT));
}
/*}}}  */
/*}}}  */