/* XRACER (C) 1999-2000 Richard W.M. Jones <rich@annexia.org> and other AUTHORS
 *
 * 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.
 *
 * $Id: math.c,v 1.4 2000/02/20 13:07:45 rich Exp $
 */

#include "config.h"

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "xracer-math.h"

GLfloat xrI[16] = {
  1, 0, 0, 0,
  0, 1, 0, 0,
  0, 0, 1, 0,
  0, 0, 0, 1
};

void
xrIdentityMatrix (GLfloat *m)
{
  memcpy (m, xrI, sizeof (GLfloat) * 16);
}

/* This code is taken from Mesa 3.0. I have exchanged
 * degrees for radians.
 */
void
xrMakeRotationMatrix (GLfloat angle,
		      GLfloat x, GLfloat y, GLfloat z,
		      GLfloat *m)
{
  /* This function contributed by Erich Boleyn (erich@uruk.org) */
  GLfloat mag, s, c;
  GLfloat xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;

  s = sin(angle);
  c = cos(angle);

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

  if (mag == 0.0) {
    /* generate an identity matrix and return */
    xrIdentityMatrix (m);
    return;
  }

  x /= mag;
  y /= mag;
  z /= mag;

#define M(row,col)  m[col*4+row]

  /*
   *     Arbitrary axis rotation matrix.
   *
   *  This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied
   *  like so:  Rz * Ry * T * Ry' * Rz'.  T is the final rotation
   *  (which is about the X-axis), and the two composite transforms
   *  Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary
   *  from the arbitrary axis to the X-axis then back.  They are
   *  all elementary rotations.
   *
   *  Rz' is a rotation about the Z-axis, to bring the axis vector
   *  into the x-z plane.  Then Ry' is applied, rotating about the
   *  Y-axis to bring the axis vector parallel with the X-axis.  The
   *  rotation about the X-axis is then performed.  Ry and Rz are
   *  simply the respective inverse transforms to bring the arbitrary
   *  axis back to it's original orientation.  The first transforms
   *  Rz' and Ry' are considered inverses, since the data from the
   *  arbitrary axis gives you info on how to get to it, not how
   *  to get away from it, and an inverse must be applied.
   *
   *  The basic calculation used is to recognize that the arbitrary
   *  axis vector (x, y, z), since it is of unit length, actually
   *  represents the sines and cosines of the angles to rotate the
   *  X-axis to the same orientation, with theta being the angle about
   *  Z and phi the angle about Y (in the order described above)
   *  as follows:
   *
   *  cos ( theta ) = x / sqrt ( 1 - z^2 )
   *  sin ( theta ) = y / sqrt ( 1 - z^2 )
   *
   *  cos ( phi ) = sqrt ( 1 - z^2 )
   *  sin ( phi ) = z
   *
   *  Note that cos ( phi ) can further be inserted to the above
   *  formulas:
   *
   *  cos ( theta ) = x / cos ( phi )
   *  sin ( theta ) = y / sin ( phi )
   *
   *  ...etc.  Because of those relations and the standard trigonometric
   *  relations, it is pssible to reduce the transforms down to what
   *  is used below.  It may be that any primary axis chosen will give the
   *  same results (modulo a sign convention) using thie method.
   *
   *  Particularly nice is to notice that all divisions that might
   *  have caused trouble when parallel to certain planes or
   *  axis go away with care paid to reducing the expressions.
   *  After checking, it does perform correctly under all cases, since
   *  in all the cases of division where the denominator would have
   *  been zero, the numerator would have been zero as well, giving
   *  the expected result.
   */

  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;

  M(0,0) = (one_c * xx) + c;
  M(0,1) = (one_c * xy) - zs;
  M(0,2) = (one_c * zx) + ys;
  M(0,3) = 0.0F;

  M(1,0) = (one_c * xy) + zs;
  M(1,1) = (one_c * yy) + c;
  M(1,2) = (one_c * yz) - xs;
  M(1,3) = 0.0F;

  M(2,0) = (one_c * zx) - ys;
  M(2,1) = (one_c * yz) + xs;
  M(2,2) = (one_c * zz) + c;
  M(2,3) = 0.0F;

  M(3,0) = 0.0F;
  M(3,1) = 0.0F;
  M(3,2) = 0.0F;
  M(3,3) = 1.0F;

#undef M
}

/* This code is taken from Mesa 3.0.
 */
void
xrMatrixMultiply (const GLfloat *a, const GLfloat *b,
		  GLfloat *product)
{
   /* This matmul was contributed by Thomas Malik */
   GLint i;

#define A(row,col)  a[(col<<2)+row]
#define B(row,col)  b[(col<<2)+row]
#define P(row,col)  product[(col<<2)+row]

   /* i-te Zeile */
   for (i = 0; i < 4; i++) {
     GLfloat ai0=A(i,0),  ai1=A(i,1),  ai2=A(i,2),  ai3=A(i,3);
     P(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
     P(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
     P(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
     P(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
   }

#undef A
#undef B
#undef P
}

/* Multiply matrix by vector. */
void
xrMatrixVectorMultiply (const GLfloat *m, const GLfloat *v,
			GLfloat *result)
{
  result[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2] + m[3]*v[3];
  result[1] = m[4]*v[0] + m[5]*v[1] + m[6]*v[2] + m[7]*v[3];
  result[2] = m[8]*v[0] + m[9]*v[1] + m[10]*v[2] + m[11]*v[3];
  result[3] = m[12]*v[0] + m[13]*v[1] + m[14]*v[2] + m[15]*v[3];
}

/* Compute the magnitude of a vector. */
GLfloat
xrMagnitude (const GLfloat *v)
{
  return sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
}

/* Compute the magnitude of a 2D vector. */
GLfloat
xrMagnitude2d (const GLfloat *v)
{
  return sqrt (v[0]*v[0] + v[1]*v[1]);
}

/* Normalize a vector. */
void
xrNormalize (const GLfloat *v, GLfloat *r)
{
  GLfloat w = xrMagnitude (v);
  r[0] = v[0] / w;
  r[1] = v[1] / w;
  r[2] = v[2] / w;
}

/* Normalize a 2D vector. */
void
xrNormalize2d (const GLfloat *v, GLfloat *r)
{
  GLfloat w = xrMagnitude2d (v);
  r[0] = v[0] / w;
  r[1] = v[1] / w;
}

void
xrUnitNormalToSide (GLfloat *side, GLfloat *normal)
{
  GLfloat n[3] = { side[0], side[1], side[2] };
  xrNormalize (n, normal);
}

/* Compute the dot product of two vectors. */
GLfloat
xrDotProduct (const GLfloat *v1, const GLfloat *v2)
{
  return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}

/* Compute the magnitude of vector v1 in direction vector v2.
 * If v1 == v2, this returns 1. If v1 = -v2, this returns -1.
 * If v1 is perpendicular to v2 this returns 0.
 */
GLfloat
xrMagnitudeInDirection (const GLfloat *v1, const GLfloat *v2)
{
  static GLfloat v1n[3], v2n[3];

  /* Normalize both vectors. */
  xrNormalize (v1, v1n);
  xrNormalize (v2, v2n);

  /* Return their dot product. */
  return xrDotProduct (v1n, v2n);
}

/* Compute angle between two vectors. */
GLfloat
xrAngleBetween (const GLfloat *v1, const GLfloat *v2)
{
  return acos (xrMagnitudeInDirection (v1, v2));
}

/* Calculate midpoint. */
void
xrMidpoint (const GLfloat *p1, const GLfloat *p2, GLfloat *mp)
{
  mp[0] = (p1[0] + p2[0]) / 2;
  mp[1] = (p1[1] + p2[1]) / 2;
  mp[2] = (p1[2] + p2[2]) / 2;
}

/* Calculate midpoint (in 2D). */
void
xrMidpoint2d (const GLfloat *p1, const GLfloat *p2, GLfloat *mp)
{
  mp[0] = (p1[0] + p2[0]) / 2;
  mp[1] = (p1[1] + p2[1]) / 2;
}

/* Cross product of vectors v and w.
 * The cross product is a vector:
 *
 *   v x w = |v| |w| sin t n^
 *
 * where t is the angle between a and
 * b, and n^ is a normal vector perpendicular
 * to a and b such that a,b,n^ form a
 * right-handed set.
 */
void
xrCrossProduct (const GLfloat *v, const GLfloat *w, GLfloat *r)
{
  r[0] = v[1]*w[2] - v[2]*w[1];
  r[1] = v[2]*w[0] - v[0]*w[2];
  r[2] = v[0]*w[1] - v[1]*w[0];
}

/* Distance between two points. */
GLfloat
xrDistance (const GLfloat *p, const GLfloat *q)
{
  double x = p[0] - q[0];
  double y = p[1] - q[1];
  double z = p[2] - q[2];
  return sqrt (x*x + y*y + z*z);
}

/* Distance from a point to a plane. */
GLfloat
xrDistancePointToPlane (const GLfloat *plane, const GLfloat *point)
{
  GLfloat a = plane[0];
  GLfloat b = plane[1];
  GLfloat c = plane[2];
  GLfloat d = plane[3];
  GLfloat x = point[0];
  GLfloat y = point[1];
  GLfloat z = point[2];
  GLfloat t = (a*x + b*y + c*z + d) / - (a*a + b*b + c*c);
  GLfloat t2 = t*t;
  GLfloat dist = sqrt (t2*a*a + t2*b*b + t2*c*c);
  /* Don't lose the sign of t. */
  if (t < 0) return dist; else return -dist;
}

/* Return true if xrDistancePointToPlane > 0. */
int
xrPointInsidePlane (const GLfloat *plane, const GLfloat *point)
{
  GLfloat a = plane[0];
  GLfloat b = plane[1];
  GLfloat c = plane[2];
  GLfloat d = plane[3];
  GLfloat x = point[0];
  GLfloat y = point[1];
  GLfloat z = point[2];
  GLfloat t = (a*x + b*y + c*z + d) / - (a*a + b*b + c*c);

  return t < 0;

#if 0
  GLfloat t2 = t*t;
  GLfloat dist = sqrt (t2*a*a + t2*b*b + t2*c*c);
  /* Don't lose the sign of t. */
  if (t < 0) return dist; else return -dist;
#endif
}

GLfloat
xrDistancePointToLine (const GLfloat *point,
		       const GLfloat *line_point, const GLfloat *line_vector)
{
  GLfloat u[3], p[3], prod[3];
  GLfloat dist;

  /* Normalize the vector u along the line. */
  xrNormalize (line_vector, u);

  /* The distance is given by: | (p-a) x u |
   * where p is the point, a is a point on the line, and x is cross product.
   */
  p[0] = point[0] - line_point[0];
  p[1] = point[1] - line_point[1];
  p[2] = point[2] - line_point[2];
  xrCrossProduct (p, u, prod);
  dist = xrMagnitude (prod);

  return dist;
}

/* Compute the four coefficients of a plane which
 * uniquely specify that plane, given three points
 * (not colinear) on the plane. Most of the variables
 * in this function are redundant (optimized out?),
 * but they get it into the same form as in
 * Lansdown, p. 178.
 */
void
xrPlaneCoefficients (const GLfloat *p, const GLfloat *q, const GLfloat *r,
		     GLfloat *co)
{
  GLfloat x2 = p[0];
  GLfloat y2 = p[1];
  GLfloat z2 = p[2];
  GLfloat x1 = q[0];
  GLfloat y1 = q[1];
  GLfloat z1 = q[2];
  GLfloat x3 = r[0];
  GLfloat y3 = r[1];
  GLfloat z3 = r[2];
  GLfloat xa = x1 + x2;
  GLfloat xb = x2 + x3;
  GLfloat xc = x3 + x1;
  GLfloat ya = y1 + y2;
  GLfloat yb = y2 + y3;
  GLfloat yc = y3 + y1;
  GLfloat za = z1 + z2;
  GLfloat zb = z2 + z3;
  GLfloat zc = z3 + z1;

  co[0] = (y1-y2) * za + (y2-y3) * zb + (y3-y1) * zc;
  co[1] = (z1-z2) * xa + (z2-z3) * xb + (z3-z1) * xc;
  co[2] = (x1-x2) * ya + (x2-x3) * yb + (x3-x1) * yc;
  co[3] = - (co[0]*x1 + co[1]*y1 + co[2]*z1);
}
