/*
 * libvec.c - a library of vector operations and a random number generator
 *
 * Version:  2.2 (11/17/87)
 * Author:  Eric Haines, 3D/Eye, Inc.
 *
 * Modified: 1 October 1992
 *           Alexander R. Enzmann
 *
 *           Object generation routines split off to keep file size down
 */

#include <stdio.h>
#include <math.h>
#include "libvec.h"

/*
 * Normalize the vector (X,Y,Z) so that X*X + Y*Y + Z*Z = 1.
 *
 * The normalization divisor is returned.  If the divisor is zero, no
 * normalization occurs.
 *
 */
double
lib_normalize_coord3(cvec)
   COORD4 *cvec;
{
   double len;

   len  = DOT_PRODUCT(*cvec, *cvec);
   if (len > 0.0) {
      len = sqrt(len);
      cvec->x /= len;
      cvec->y /= len;
      cvec->z /= len;
      }
    return len;
}


/*
 * Set all matrix elements to zero.
 */
void
lib_zero_matrix(mx)
   MATRIX mx;
{
   long i, j;

   for (i=0;i<4;++i)
      for (j=0;j<4;++j)
         mx[i][j] = 0.0;
}

/*
 * Create identity matrix.
 */
void
lib_create_identity_matrix(mx)
   MATRIX mx;
{
   int i;

   lib_zero_matrix(mx);
   for (i=0;i<4;++i)
      mx[i][i] = 1.0;
}

/*
 * Copy one matrix to another
 */
void
lib_copy_matrix(mres, mx)
   MATRIX mres, mx;
{
   int i, j;

   for (i=0;i<4;i++)
      for (j=0;j<4;j++)
         mres[i][j] = mx[i][j];
}

/*
 * Create a rotation matrix along the given axis by the given angle in radians.
 */
void
lib_create_rotate_matrix(mx, axis, angle)
   MATRIX mx;
   int axis;
   double angle;
{
   double cosine, sine;

   lib_zero_matrix(mx);
   cosine = cos((double)angle);
   sine = sin((double)angle);
   switch (axis) {
      case X_AXIS:
         mx[0][0] = 1.0;
         mx[1][1] = cosine;
         mx[2][2] = cosine;
         mx[1][2] = sine;
         mx[2][1] = -sine;
         break ;
      case Y_AXIS:
         mx[1][1] = 1.0;
         mx[0][0] = cosine;
         mx[2][2] = cosine;
         mx[2][0] = sine;
         mx[0][2] = -sine;
         break;
      case Z_AXIS:
         mx[2][2] = 1.0;
         mx[0][0] = cosine;
         mx[1][1] = cosine;
         mx[0][1] = sine;
         mx[1][0] = -sine;
         break;
      }
   mx[3][3] = 1.0;
}

/*
 * Create a rotation matrix along the given axis by the given angle in radians.
 * The axis is a set of direction cosines.
 */
void
lib_create_axis_rotate_matrix(mx, rvec, angle)
   MATRIX mx;
   COORD4 *rvec;
   double angle;
{
   COORD4  axis;
   double  cosine, one_minus_cosine, sine;

   COPY_COORD(axis, (*rvec));
   cosine = cos((double)angle);
   sine = sin((double)angle);
   one_minus_cosine = 1.0 - cosine;

   mx[0][0] = SQR(axis.x) + (1.0 - SQR(axis.x)) * cosine;
   mx[0][1] = axis.x * axis.y * one_minus_cosine + axis.z * sine;
   mx[0][2] = axis.x * axis.z * one_minus_cosine - axis.y * sine;
   mx[0][3] = 0.0;

   mx[1][0] = axis.x * axis.y * one_minus_cosine - axis.z * sine;
   mx[1][1] = SQR(axis.y) + (1.0 - SQR(axis.y)) * cosine;
   mx[1][2] = axis.y * axis.z * one_minus_cosine + axis.x * sine;
   mx[1][3] = 0.0;

   mx[2][0] = axis.x * axis.z * one_minus_cosine + axis.y * sine;
   mx[2][1] = axis.y * axis.z * one_minus_cosine - axis.x * sine;
   mx[2][2] = SQR(axis.z) + (1.0 - SQR(axis.z)) * cosine;
   mx[2][3] = 0.0;

   mx[3][0] = 0.0;
   mx[3][1] = 0.0;
   mx[3][2] = 0.0;
   mx[3][3] = 1.0;
}

/* Create translation matrix */
void
lib_create_translate_matrix(mx, vec)
   MATRIX mx;
   COORD4 *vec;
{
    int i;
    lib_create_identity_matrix(mx);
    mx[3][0] = vec->x;
    mx[3][1] = vec->y;
    mx[3][2] = vec->z;
}

/* Create scaling matrix */
void
lib_create_scale_matrix(mx, vec)
   MATRIX mx;
   COORD4 *vec;
{
    int i;
    lib_zero_matrix(mx);
    mx[0][0] = vec->x;
    mx[1][1] = vec->y;
    mx[2][2] = vec->z;
}

/* Given a point and a direction, find the transform that brings a point
   in a canonical coordinate system into a coordinate system defined by
   that position and direction.  Both the forward and inverse transform
   matrices are calculated. */
void
lib_create_canonical_matrix(trans, itrans, origin, up)
   MATRIX trans, itrans;
   COORD4 *origin;
   COORD4 *up;
{
   MATRIX trans1, trans2;
   COORD4 tmpv;
   double ang;

   /* Translate "origin" to <0, 0, 0> */
   SET_COORD(tmpv, -origin->x, -origin->y, -origin->z);
   lib_create_translate_matrix(trans1, &tmpv);

   /* Determine the axis to rotate about */
   if (fabs(up->z) == 1.0)
      SET_COORD4(tmpv, 1.0, 0.0, 0.0, 1.0)
   else
      SET_COORD4(tmpv, -up->y, up->x, 0.0, 1.0)
   lib_normalize_coord3(&tmpv);
   ang = acos(up->z);

   /* Calculate the forward matrix */
   lib_create_axis_rotate_matrix(trans2, &tmpv, -ang);
   lib_matrix_multiply(trans, trans1, trans2);

   /* Calculate the inverse transform */
   lib_create_axis_rotate_matrix(trans1, &tmpv, ang);
   lib_create_translate_matrix(trans2, origin);
   lib_matrix_multiply(itrans, trans1, trans2);
}

/* Find the transformation that takes the current eye position and
   orientation and puts it at {0, 0, 0} with the up vector aligned
   with {0, 1, 0} */
void
lib_create_view_matrix(trans, from, at, up, xres, yres, angle, aspect)
   MATRIX trans;
   COORD4 *from, *at, *up;
   int xres, yres;
   double angle, aspect;
{
   double x, y, z, d, theta;
   double xscale, yscale, view_dist;
   COORD4 Va, Ve, Vv, Rt, right, up_dir;
   MATRIX Tv, Tt, Tx;

   /* Change the view angle into radians */
   angle = PI * angle / 180.0;

   /* Translate point of interest to the origin */
   SET_COORD(Va, -at->x, -at->y, -at->z);
   lib_create_translate_matrix(Tv, &Va);

   /* Move the eye by the same amount */
   ADD3_COORD(Ve, *from, Va);

   /* Make sure this is a valid sort of setup */
   if (Ve.x == 0.0 && Ve.y == 0.0 && Ve.z == 0.0) {
      fprintf(stderr, "Degenerate perspective transformation\n");
      exit(1);
      }

   /* Get the up vector to be perpendicular to the view vector */
   SET_COORD(Rt, -Ve.x, -Ve.y, -Ve.z);
   lib_normalize_coord3(&Rt);
   COPY_COORD(up_dir, *up);
   CROSS(right, up_dir, Rt);
   lib_normalize_coord3(&right);
   CROSS(up_dir, Rt, right);
   lib_normalize_coord3(&up_dir);

   /* Determine the angle of rotation about the x-axis that takes
     the eye into the x-z plane. */
   y = Ve.y;
   if (fabs(y) > EPSILON) {
      z = Ve.z;
      d = sqrt(y * y + z * z);
      if (fabs(d) > EPSILON) {
         if (y > 0)
            theta =  acos(z/d) - PI;
         else
            theta = -acos(z/d) - PI;
         lib_create_rotate_matrix(Tt, X_AXIS, theta);
         lib_matrix_multiply(Tx, Tv, Tt);
         lib_copy_matrix(Tv, Tx);
         }
      }

   /* Determine the angle of rotation about the y-axis that takes
      the eye onto the minus z-axis */
   from->w = 1;
   lib_transform_coord(&Ve, from, Tv);
   x = Ve.x;
   if (fabs(x) > EPSILON) {
      z = Ve.z;
      d = sqrt(x * x + z * z);
      if (fabs(d) > EPSILON) {
         if (x > 0)
            theta = PI - acos(z / d);
         else
            theta = PI + acos(z / d);
         lib_create_rotate_matrix(Tt, Y_AXIS, theta);
         lib_matrix_multiply(Tx, Tv, Tt);
         lib_copy_matrix(Tv, Tx);
         }
      }
   else if (Ve.z > 0) {
      lib_create_rotate_matrix(Tt, Y_AXIS, PI);
      lib_matrix_multiply(Tx, Tv, Tt);
      lib_copy_matrix(Tv, Tx);
      }

   /* Determine the angle of rotation about the z-axis that takes
      the up vector into alignment with the y-axis */
   up_dir.w = 0;
   lib_transform_coord(&Ve, &up_dir, Tv);
   y = Ve.y;
   if (fabs(y) < 1.0) {
      x = Ve.x;
      d = sqrt(x * x + y * y);
      if (fabs(d) > EPSILON) {
         if (x > 0)
            theta =  acos(y / d);
         else
            theta = -acos(y / d);
         lib_create_rotate_matrix(Tt, Z_AXIS, theta);
         lib_matrix_multiply(Tx, Tv, Tt);
         lib_copy_matrix(Tv, Tx);
         }
      }

   /* Finally, translate the eye position to the point {0, 0, 0} */
   from->w = 1;
   SUB3_COORD(Ve, *at, *from);
   view_dist = sqrt(DOT_PRODUCT(Ve, Ve));
   lib_transform_coord(&Ve, from, Tv);
   SET_COORD(Va, 0, 0, -Ve.z);
   lib_create_translate_matrix(Tt, &Va);
   lib_matrix_multiply(Tx, Tv, Tt);
   lib_copy_matrix(Tv, Tx);
   /* Determine how much to scale things by */
   yscale = (double)yres / (2.0 * view_dist * tan(angle/2.0));
   xscale = yscale * (double)xres / ((double)yres * aspect);
   SET_COORD(Va, xscale, yscale, 1);
   lib_create_scale_matrix(Tt, &Va);
   lib_matrix_multiply(Tx, Tv, Tt);
   lib_copy_matrix(Tv, Tx);
   
   /* Now add in the perspective transformation */
   lib_create_identity_matrix(Tt);
   Tt[2][3] = 1.0 / view_dist;
   lib_matrix_multiply(Tx, Tv, Tt);

   /* We now have the final transformation matrix */
   lib_copy_matrix(trans, Tx);
}

/*
 * Multiply a 4 element vector by a matrix.
 */
void
lib_transform_coord(vres, vec, mx)
   COORD4 *vres, *vec;
   MATRIX mx;
{
   vres->x = vec->x*mx[0][0]+vec->y*mx[1][0]+vec->z*mx[2][0]+vec->w*mx[3][0];
   vres->y = vec->x*mx[0][1]+vec->y*mx[1][1]+vec->z*mx[2][1]+vec->w*mx[3][1];
   vres->z = vec->x*mx[0][2]+vec->y*mx[1][2]+vec->z*mx[2][2]+vec->w*mx[3][2];
   vres->w = vec->x*mx[0][3]+vec->y*mx[1][3]+vec->z*mx[2][3]+vec->w*mx[3][3];
}

/*
 * Multiply two 4x4 matrices.
 */
void
lib_matrix_multiply(mxres, mx1, mx2)
   MATRIX mxres, mx1, mx2;
{
   int i, j;

   for (i=0;i<4;i++)
      for (j=0;j<4;j++)
         mxres[i][j] = mx1[i][0]*mx2[0][j] + mx1[i][1]*mx2[1][j] +
                       mx1[i][2]*mx2[2][j] + mx1[i][3]*mx2[3][j];
}

/* Performs a 3D clip of a line segment from start to end against the
   box defined by bounds.  The actual values of start and end are modified. */
int
lib_clip_to_box(start, end,  bounds)
   COORD4 *start, *end;
   double bounds[2][3];
{
   int i;
   double d, t, dir, pos;
   double tmin, tmax;
   COORD4 D;

   SUB3_COORD(D, *end, *start);
   d = lib_normalize_coord3(&D);
   tmin = 0.0;
   tmax = d;
   for (i=0;i<3;i++) {
      pos = (i == 0 ? start->x : (i == 1 ? start->y : start->z));
      dir = (i == 0 ? D.x : (i == 1 ? D.y : D.z));
      if (dir < -EPSILON) {
         t = (bounds[0][i] - pos) / dir;
         if (t < tmin)
            return 0;
         if (t <= tmax)
            tmax = t;
         t = (bounds[1][i] - pos) / dir;
         if (t >= tmin) {
            if (t > tmax)
               return 0;
            tmin = t;
            }
         }
      else if (dir > EPSILON) {
         t = (bounds[1][i] - pos) / dir;
         if (t < tmin)
            return 0;
         if (t <= tmax)
            tmax = t;
         t = (bounds[0][i] - pos) / dir;
         if (t >= tmin) {
            if (t > tmax)
               return 0;
            tmin = t;
            }
         }
      else if (pos < bounds[0][i] || pos > bounds[1][i]) {
         return 0;
         }
      }
   if (tmax < d) {
      end->x = start->x + tmax * D.x;
      end->y = start->y + tmax * D.y;
      end->z = start->z + tmax * D.z;
      }
   if (tmin > 0.0) {
      start->x = start->x + tmin * D.x;
      start->y = start->y + tmin * D.y;
      start->z = start->z + tmin * D.z;
      }
   return 1;
}

/*
 * Rotate a vector pointing towards the major-axis faces (i.e. the major-axis
 * component of the vector is defined as the largest value) 90 degrees to
 * another cube face.  Mod_face is a face number.
 *
 * If the routine is called six times, with mod_face=0..5, the vector will be
 * rotated to each face of a cube.  Rotations are:
 *     mod_face = 0 mod 3, +Z axis rotate
 *     mod_face = 1 mod 3, +X axis rotate
 *     mod_face = 2 mod 3, -Y axis rotate
 */
void
lib_rotate_cube_face(vec, major_axis, mod_face)
   COORD4 *vec;
   int major_axis, mod_face;
{
   double swap;

   mod_face = (mod_face+major_axis) % 3 ;
   if (mod_face == 0) {
      swap   = vec->x;
      vec->x = -vec->y;
      vec->y = swap;
      }
   else if (mod_face == 1) {
      swap   = vec->y;
      vec->y = -vec->z;
      vec->z = swap;
      }
   else {
      swap   = vec->x;
      vec->x = -vec->z;
      vec->z = swap;
      }
}

/*
 * Portable gaussian random number generator (from "Numerical Recipes", GASDEV)
 * Returns a uniform random deviate between 0.0 and 1.0.  'iseed' must be
 * less than M1 to avoid repetition, and less than (2**31-C1)/A1 [= 300718]
 * to avoid overflow.
 */
#define M1  134456
#define IA1   8121
#define IC1  28411
#define RM1 1.0/M1

double
lib_gauss_rand(iseed)
   long iseed;
{
   double fac, v1, v2, r;
   long   ix1, ix2;

   ix2 = iseed;
   do {
      ix1 = (IC1+ix2*IA1) % M1;
      ix2 = (IC1+ix1*IA1) % M1;
      v1 = ix1 * 2.0 * RM1 - 1.0;
      v2 = ix2 * 2.0 * RM1 - 1.0;
      r = v1*v1 + v2*v2;
      } while ( r >= 1.0 );

   fac = sqrt((double)(-2.0 * log((double)r) / r));
   return v1 * fac;
}