/*
 *  matrix.c -- matrix efuns.
 *              2-93 : Dwayne Fontenot : original coding.
 */

#include "config.h"
#include <math.h>
#include "efuns.h"
#include "efuns_matrix.h"

#ifdef MATRIX

static Matrix identity = {1., 0., 0., 0.,
			  0., 1., 0., 0.,
			  0., 0., 1., 0.,
			  0., 0., 0., 1.};

void f_id_matrix(num_arg, instruction)
     int num_arg, instruction;
{
  struct vector *matrix;
  int i;

  matrix = allocate_array(16);
  for(i=0;i<16;i++){
    matrix->item[i].type = T_REAL;
    matrix->item[i].u.real = identity[i];
  }
  push_vector(matrix);
  matrix->ref--;
}

void f_translate(num_arg, instruction)
     int num_arg, instruction;
{
  struct vector *matrix;
  double x, y, z;
  Matrix current_matrix;
  Matrix trans_matrix;
  Matrix final_matrix;
  int i;

  if ((sp-1)->type != T_REAL) {
      bad_arg(3, instruction);
  }
  if (sp->type != T_REAL) {
      bad_arg(4, instruction);
  }
  /*
   * get arguments from stack.
   */
  matrix = (sp-3)->u.vec;
  x =      (sp-2)->u.real;
  y =      (sp-1)->u.real;
  z =          sp->u.real;
  pop_n_elems(3);
  /*
   * convert vec matrix to float matrix.
   */
  for(i=0;i<16;i++){
    current_matrix[i] = matrix->item[i].u.real;
  }
  /*
   * create translation matrix.
   */
  translate_matrix(x, y, z, trans_matrix);
  /*
   * compute transformed matrix.
   */
  mult_matrix(current_matrix, trans_matrix, final_matrix);
  /*
   * convert float matrix to vec matrix.
   */
  for(i=0;i<16;i++){
    matrix->item[i].u.real = final_matrix[i];
  }
}

void f_scale(num_arg, instruction)
     int num_arg, instruction;
{
  struct vector *matrix;
  double x, y, z;
  Matrix current_matrix;
  Matrix scaling_matrix;
  Matrix final_matrix;
  int i;

  if ((sp-1)->type != T_REAL) {
      bad_arg(3, instruction);
  }
  if (sp->type != T_REAL) {
      bad_arg(4, instruction);
  }
  /*
   * get arguments from stack.
   */
  matrix = (sp-3)->u.vec;
  x =      (sp-2)->u.real;
  y =      (sp-1)->u.real;
  z =          sp->u.real;
  pop_n_elems(3);
  /*
   * convert vec matrix to float matrix.
   */
  for(i=0;i<16;i++){
    current_matrix[i] = matrix->item[i].u.real;
  }
  /*
   * create scaling matrix.
   */
  scale_matrix(x, y, z, scaling_matrix);
  /*
   * compute transformed matrix.
   */
  mult_matrix(current_matrix, scaling_matrix, final_matrix);
  /*
   * convert float matrix to vec matrix.
   */
  for(i=0;i<16;i++){
    matrix->item[i].u.real = final_matrix[i];
  }
}

void f_rotate_x(num_arg, instruction)
     int num_arg, instruction;
{
  struct vector *matrix;
  double angle;
  Matrix current_matrix;
  Matrix rot_matrix;
  Matrix final_matrix;
  int i;

  /*
   * get arguments from stack.
   */
  matrix = (sp-1)->u.vec;
  angle =      sp->u.real;
  pop_n_elems(1);
  /*
   * convert vec matrix to float matrix.
   */
  for(i=0;i<16;i++){
    current_matrix[i] = matrix->item[i].u.real;
  }
  /*
   * create x rotation matrix.
   */
  rotate_x_matrix(angle, rot_matrix);
  /*
   * compute transformed matrix.
   */
  mult_matrix(current_matrix, rot_matrix, final_matrix);
  /*
   * convert float matrix to vec matrix.
   */
  for(i=0;i<16;i++){
    matrix->item[i].u.real = final_matrix[i];
  }
}

void f_rotate_y(num_arg, instruction)
     int num_arg, instruction;
{
  struct vector *matrix;
  double angle;
  Matrix current_matrix;
  Matrix rot_matrix;
  Matrix final_matrix;
  int i;

  /*
   * get arguments from stack.
   */
  matrix = (sp-1)->u.vec;
  angle =      sp->u.real;
  pop_n_elems(1);
  /*
   * convert vec matrix to float matrix.
   */
  for(i=0;i<16;i++){
    current_matrix[i] = matrix->item[i].u.real;
  }
  /*
   * create y rotation matrix.
   */
  rotate_y_matrix(angle, rot_matrix);
  /*
   * compute transformed matrix.
   */
  mult_matrix(current_matrix, rot_matrix, final_matrix);
  /*
   * convert float matrix to vec matrix.
   */
  for(i=0;i<16;i++){
    matrix->item[i].u.real = final_matrix[i];
  }
}

void f_rotate_z(num_arg, instruction)
     int num_arg, instruction;
{
  struct vector *matrix;
  double angle;
  Matrix current_matrix;
  Matrix rot_matrix;
  Matrix final_matrix;
  int i;

  /*
   * get arguments from stack.
   */
  matrix = (sp-1)->u.vec;
  angle =      sp->u.real;
  pop_n_elems(1);
  /*
   * convert vec matrix to float matrix.
   */
  for(i=0;i<16;i++){
    current_matrix[i] = matrix->item[i].u.real;
  }
  /*
   * create z rotation matrix.
   */
  rotate_z_matrix(angle, rot_matrix);
  /*
   * compute transformed matrix.
   */
  mult_matrix(current_matrix, rot_matrix, final_matrix);
  /*
   * convert float matrix to vec matrix.
   */
  for(i=0;i<16;i++){
    matrix->item[i].u.real = final_matrix[i];
  }
}

void f_lookat_rotate(num_arg, instruction)
     int num_arg, instruction;
{
  struct vector *matrix;
  double x, y, z;
  Matrix current_matrix;
  Matrix lookat_matrix;
  int i;

  if ((sp-1)->type != T_REAL) {
      bad_arg(3, instruction);
  }
  if (sp->type != T_REAL) {
      bad_arg(4, instruction);
  }
  /*
   * get arguments from stack.
   */
  matrix = (sp-3)->u.vec;
  x =      (sp-2)->u.real;
  y =      (sp-1)->u.real;
  z =          sp->u.real;
  pop_n_elems(3);
  /*
   * convert vec matrix to float matrix.
   */
  for(i=0;i<16;i++){
    current_matrix[i] = matrix->item[i].u.real;
  }
  /*
   * create new viewing transformation matrix.
   */
  lookat_rotate(current_matrix, x, y, z, lookat_matrix);
  /*
   * convert float matrix to vec matrix.
   */
  for(i=0;i<16;i++){
    matrix->item[i].u.real = lookat_matrix[i];
  }
}

void f_lookat_rotate2(num_arg, instruction)
     int num_arg, instruction;
{
  struct vector *matrix;
  double ex, ey, ez, lx, ly, lz;
  Matrix current_matrix;
  Matrix lookat_matrix;
  int i, j;

  for (j = 4; j >= 0; j--) {
     if ((sp-j)->type != T_REAL) {
         bad_arg(7 - j, instruction);
     }
  }
  /*
   * get arguments from stack.
   */
  matrix = (sp-6)->u.vec;
  ex =     (sp-5)->u.real;
  ey =     (sp-4)->u.real;
  ez =     (sp-3)->u.real;
  lx =     (sp-2)->u.real;
  ly =     (sp-1)->u.real;
  lz =         sp->u.real;
  pop_n_elems(6);
  /*
   * convert vec matrix to float matrix.
   */
  for(i=0;i<16;i++){
    current_matrix[i] = matrix->item[i].u.real;
  }
  /*
   * create new viewing transformation matrix.
   */
  lookat_rotate2(ex, ey, ez, lx, ly, lz, lookat_matrix);
  /*
   * convert float matrix to vec matrix.
   */
  for(i=0;i<16;i++){
    matrix->item[i].u.real = lookat_matrix[i];
  }
}

void print_matrix(m, label)
     Matrix m;
     char *label;
{
  int i;
  int j;

  fprintf(stderr,"%s:\n", label);
  for(i=0;i<4;i++){
    for(j=0;j<4;j++){
      fprintf(stderr,"%f\t", m[i * 4 + j]);
    }
    fprintf(stderr,"\n");
  }
}

void print_vector(v, label)
     Vector *v;
     char *label;
{
  fprintf(stderr,"%s:\t%f\t%f\t%f\n", label, v->x, v->y, v->z);
}


Vector *normalize_vector(v)
     Vector *v;
{
  double xx,yy,zz,mm,m;

  xx = v->x * v->x ; yy = v->y * v->y ; zz = v->z * v->z;
  mm = xx + yy + zz;
  m = sqrt(mm);
  if (m) {
     v->x /= m ; v->y /= m ; v->z /= m;
  }
  return(v);
}

Vector *cross_product(v, va, vb)
     Vector *v;
     Vector *va;
     Vector *vb;
{
  v->x = (va->y*vb->z) - (va->z*vb->y);
  v->y = (va->z*vb->x) - (va->x*vb->z);
  v->z = (va->x*vb->y) - (va->y*vb->x);
  return(v);
}

Vector *points_to_vector(v, pa, pb)
     Vector *v;
     Vector *pa;
     Vector *pb;
{
  v->x = pa->x - pb->x ; v->y = pa->y - pb->y ; v->z = pa->z - pb->z;
  return(v);
}

void lookat_rotate(T, x, y, z, M)
     Matrix T;
     double x, y, z;
     Matrix M;
{
  static Vector N, V, U;
  static Vector ep, lp;

  lp.x = x; lp.y = y; lp.z = z;
  ep.x = T[12]; ep.y = T[13]; ep.z = T[14];
  points_to_vector(&N, &lp, &ep);
  normalize_vector(&N);

  U.x = T[0]; U.y = T[4]; U.z = T[8];
  cross_product(&V, &N, &U);
  normalize_vector(&V);

  cross_product(&U, &V, &N);
  normalize_vector(&U);

  M[0] = U.x; M[1] = V.x; M[2] = N.x; M[3] = 0.;
  M[4] = U.y; M[5] = V.y; M[6] = N.y; M[7] = 0.;
  M[8] = U.z; M[9] = V.z; M[10]= N.z; M[11]= 0.;
#if 0
  M[12]= ep.x; M[13]= ep.y; M[14]= ep.z; M[15]= 1.;
#endif
#if 0
  M[12] = - U.x * ep.x - U.y * ep.y - U.z * ep.z;
  M[13] = - V.x * ep.x - V.y * ep.y - V.z * ep.z;
  M[14] = - N.x * ep.x - N.y * ep.y - N.z * ep.z;
#endif
  M[12] = ((U.x * ep.x) + (U.y * ep.y) + (U.z * ep.z));
  M[13] = ((V.x * ep.x) + (V.y * ep.y) + (V.z * ep.z));
  M[14] = ((N.x * ep.x) + (N.y * ep.y) + (N.z * ep.z));
  M[15] = 1.;

#ifdef DEBUG
  print_vector(&lp, "look point");
  print_vector(&ep, "eye point");
  print_vector(&N, "normal vector");
  print_vector(&V, "V = N x U");
  print_vector(&U, "U = V x N");
  print_matrix(&M, "final matrix");
#endif /* DEBUG */
}

void lookat_rotate2(ex, ey, ez, lx, ly, lz, M)
     double ex, ey, ez;
     double lx, ly, lz;
     Matrix M;
{
  static Vector N, V, U;
  static Vector ep, lp;

  ep.x = ex; ep.y = ey; ep.z = ez;
  lp.x = lx; lp.y = ly; lp.z = lz;
  points_to_vector(&N, &lp, &ep);
  normalize_vector(&N);

  U.x = 0.; U.y = 1.; U.z = 0.;
  cross_product(&V, &N, &U);
  normalize_vector(&V);

  cross_product(&U, &V, &N);
  normalize_vector(&U);

  M[0] = U.x; M[1] = V.x; M[2] = N.x; M[3] = 0.;
  M[4] = U.y; M[5] = V.y; M[6] = N.y; M[7] = 0.;
  M[8] = U.z; M[9] = V.z; M[10]= N.z; M[11]= 0.;
#if 0
  M[12]= ep.x; M[13]= ep.y; M[14]= ep.z; M[15]= 1.;
#endif
#if 0
  M[12] = - U.x * ep.x - U.y * ep.y - U.z * ep.z;
  M[13] = - V.x * ep.x - V.y * ep.y - V.z * ep.z;
  M[14] = - N.x * ep.x - N.y * ep.y - N.z * ep.z;
#endif
  M[12] = ((U.x * ep.x) + (U.y * ep.y) + (U.z * ep.z));
  M[13] = ((V.x * ep.x) + (V.y * ep.y) + (V.z * ep.z));
  M[14] = ((N.x * ep.x) + (N.y * ep.y) + (N.z * ep.z));
  M[15] = 1.;

#ifdef DEBUG
  print_vector(&lp, "look point");
  print_vector(&ep, "eye point");
  print_vector(&N, "normal vector");
  print_vector(&V, "V = N x U");
  print_vector(&U, "U = V x N");
  print_matrix(&M, "final matrix");
#endif /* DEBUG */
}

void translate_matrix(x, y, z, m)
     double x, y, z;
     Matrix m;
{
  m[0] = 1.   ; m[1] = 0.   ; m[2] = 0.   ; m[3] = 0.;
  m[4] = 0.   ; m[5] = 1.   ; m[6] = 0.   ; m[7] = 0.;
  m[8] = 0.   ; m[9] = 0.   ; m[10]= 1.   ; m[11]= 0.;
  m[12]= x    ; m[13]= y    ; m[14]= z    ; m[15]= 1.;
}

void scale_matrix(x, y, z, m)
     double x, y, z;
     Matrix m;
{
  m[0] = x    ; m[1] = 0.   ; m[2] = 0.   ; m[3] = 0.;
  m[4] = 0.   ; m[5] = y    ; m[6] = 0.   ; m[7] = 0.;
  m[8] = 0.   ; m[9] = 0.   ; m[10]= z    ; m[11]= 0.;
  m[12]= 0.   ; m[13]= 0.   ; m[14]= 0.   ; m[15]= 1.;
}

void rotate_x_matrix(a, m)
     double a;
     Matrix m;
{
  double a_rad;
  double c, s;

  a_rad = (double)(a * RADIANS_PER_DEGREE);
  c = cos(a_rad);
  s = sin(a_rad);
  m[0] = 1.   ; m[1] = 0.   ; m[2] = 0.   ; m[3] = 0.;
  m[4] = 0.   ; m[5] = c    ; m[6] = s    ; m[7] = 0.;
  m[8] = 0.   ; m[9] = -s   ; m[10]= c    ; m[11]= 0.;
  m[12]= 0.   ; m[13]= 0.   ; m[14]= 0.   ; m[15]= 1.;
}

void rotate_y_matrix(a, m)
     double a;
     Matrix m;
{
  double a_rad;
  double c, s;

  a_rad = (double)(a * RADIANS_PER_DEGREE);
  c = cos(a_rad);
  s = sin(a_rad);
  m[0] = c    ; m[1] = 0.   ; m[2] = -s   ; m[3] = 0.;
  m[4] = 0.   ; m[5] = 1.   ; m[6] = 0.   ; m[7] = 0.;
  m[8] = s    ; m[9] = 0.   ; m[10]= c    ; m[11]= 0.;
  m[12]= 0.   ; m[13]= 0.   ; m[14]= 0.   ; m[15]= 1.;
}

void rotate_z_matrix(a, m)
     double a;
     Matrix m;
{
  double a_rad;
  double c, s;

  a_rad = (double)(a * RADIANS_PER_DEGREE);
  c = cos(a_rad);
  s = sin(a_rad);
  m[0] = c    ; m[1] = s    ; m[2] = 0.   ; m[3] = 0.;
  m[4] = -s   ; m[5] = c    ; m[6] = 0.   ; m[7] = 0.;
  m[8] = 0.   ; m[9] = 0.   ; m[10]= 1.   ; m[11]= 0.;
  m[12]= 0.   ; m[13]= 0.   ; m[14]= 0.   ; m[15]= 1.;
}

void mult_matrix(ma, mb, m)
     Matrix ma, mb, m;
{
  m[0] = ma[0]*mb[0]+ma[1]*mb[4]+ma[2]*mb[8]+ma[3]*mb[12];

  m[1] = ma[0]*mb[1]+ma[1]*mb[5]+ma[2]*mb[9]+ma[3]*mb[13];

  m[2] = ma[0]*mb[2]+ma[1]*mb[6]+ma[2]*mb[10]+ma[3]*mb[14];

  m[3] = ma[0]*mb[3]+ma[1]*mb[7]+ma[2]*mb[11]+ma[3]*mb[15];

  m[4] = ma[4]*mb[0]+ma[5]*mb[4]+ma[6]*mb[8]+ma[7]*mb[12];

  m[5] = ma[4]*mb[1]+ma[5]*mb[5]+ma[6]*mb[9]+ma[7]*mb[13];

  m[6] = ma[4]*mb[2]+ma[5]*mb[6]+ma[6]*mb[10]+ma[7]*mb[14];

  m[7] = ma[4]*mb[3]+ma[5]*mb[7]+ma[6]*mb[11]+ma[7]*mb[15];

  m[8] = ma[8]*mb[0]+ma[9]*mb[4]+ma[10]*mb[8]+ma[11]*mb[12];

  m[9] = ma[8]*mb[1]+ma[9]*mb[5]+ma[10]*mb[9]+ma[11]*mb[13];

  m[10]= ma[8]*mb[2]+ma[9]*mb[6]+ma[10]*mb[10]+ma[11]*mb[14];

  m[11]= ma[8]*mb[3]+ma[9]*mb[7]+ma[10]*mb[11]+ma[11]*mb[15];

  m[12]= ma[12]*mb[0]+ma[13]*mb[4]+ma[14]*mb[8]+ma[15]*mb[12];

  m[13]= ma[12]*mb[1]+ma[13]*mb[5]+ma[14]*mb[9]+ma[15]*mb[13];

  m[14]= ma[12]*mb[2]+ma[13]*mb[6]+ma[14]*mb[10]+ma[15]*mb[14];

  m[15]= ma[12]*mb[3]+ma[13]*mb[7]+ma[14]*mb[11]+ma[15]*mb[15];
}

#endif /* MATRIX */