/**
 *  This file is part of miniCDx benchmark of oSCJ.
 *
 *   miniCDx is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU Lesser General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   miniCDx 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 Lesser General Public License for more details.
 *
 *   You should have received a copy of the GNU Lesser General Public License
 *   along with miniCDx.  If not, see <http://www.gnu.org/licenses/>.
 *
 *
 *   Copyright 2009, 2010
 *   @authors  Daniel Tang, Ales Plsek
 *
 *   See: http://sss.cs.purdue.edu/projects/oscj/
 */
package cdx;

/**
 * The <code>VectorMath</code> class implements the mathematical functions
 * for manipulating vectors of two and three dimensions. Its operators are
 * patterned after three address code machines, specifying two operands and
 * a destination operand. This machine is optimised for performance, thus the
 * methods are static, contain no calls, create no objects, and do not perform
 * error checking or synchronisation.
 *
 * @author Ben L. Titzer
 */
final class VectorMath {
  /**************************/
  /* 3D Vector Computations */
  /**************************/

  /**
   * The <code>add</code> method takes two vectors and adds them, placing the
   * result in a third vector.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @param dest the destination Vector3d to store the result
   */
  public static void add(Vector3d a, Vector3d b, Vector3d dest) {
    dest.x = a.x + b.x;
    dest.y = a.y + b.y;
    dest.z = a.z + b.z;
  }

  /**
   * The <code>subtract</code> method takes two vectors and subtracts them,
   * placing the result in a third vector.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @param dest the destination Vector3d to store the result
   */
  public static void subtract(Vector3d a, Vector3d b, Vector3d dest) {
    dest.x = a.x - b.x;
    dest.y = a.y - b.y;
    dest.z = a.z - b.z;
  }

  /**
   * The <code>scale</code> method takes a <code>Vector3d</code> and a scalar
   * float value multiplies each component of the Vector, storing the result
   * in the third parameter.
   *
   * @param a the value of the first vector
   * @param scale the value to scale the vector by
   * @param dest the destination Vector3d to store the result
   */
  public static void scale(Vector3d a, float scale, Vector3d dest) {
    dest.x = a.x * scale;
    dest.y = a.y * scale;
    dest.z = a.z * scale;
  }

  /**
   * The <code>normalize</code> method takes a <code>Vector3d</code> and if it
   * is non-zero, will normalize it so that its magnitude will be 1.
   *
   * @param a the value of the vector to normalize
   * @param dest the destination Vector3d to store the result
   * @throws ZeroVectorException if the vector is zero
   */
  public static void normalize(Vector3d a, Vector3d dest) {
    float mag = magnitude(a);
    if (mag == 0) { throw new ZeroVectorException("undefined"); }
    scale(a, 1 / mag, dest);
  }

  /**
   * The <code>magnitude</code> method takes a <code>Vector3d</code> and
   * computes its magnitude according the Euclidean norm.
   *
   * @param a the value of the vector of which to compute the magnitude
   * @returns the magnitude of the vector
   */
  public static float magnitude(Vector3d a) {
    return (float) Math.sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
  }

  /**
   * The <code>distance</code> method takes two vectors and computes their
   * (Euclidean) distance.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @returns the distance between the two vectors
   */
  public static float distance(Vector3d a, Vector3d b) {
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    float dz = a.z - b.z;
    return (float) Math.sqrt(dx * dx + dy * dy + dz * dz);
  }

  /**
   * The <code>sqDistance</code> method takes two vectors and computes the
   * square of their (Euclidean) distance. This is just an optimization for
   * the <code>distance</code> method that avoids an expensive floating point
   * square root computation.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @returns the square of the distance between the two vectors
   */
  public static float sqDistance(Vector3d a, Vector3d b) {
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    float dz = a.z - b.z;
    return dx * dx + dy * dy + dz * dz;
  }

  /**
   * The <code>dotProduct</code> method computes the dot product between two
   * vectors using the standard inner product formula.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @returns the value of their dot product
   */
  public static float dotProduct(Vector3d a, Vector3d b) {
    return a.x * b.x + a.y * b.y + a.z * b.z;
  }

  /**
   * The <code>rotate</code> method takes a <code>Vector3d</code> and a scalar
   * float value and will rotate the vector in the xy plane.
   *
   * @param a the value of the first vector
   * @param radians the value to rotate the vector by
   * @param dest the destination Vector3d to store the result
   */
  public static void rotate(Vector3d a, float radians, Vector3d dest) {
    float cos = (float) Math.cos(radians);
    float sin = (float) Math.sin(radians);
    float x = a.x, y = a.y;
    dest.x = x * cos - y * sin;
    dest.y = x * sin + y * cos;
    dest.z = a.z;
  }

  /**
   * The <code>theta</code> method takes a <code>Vector3d</code> and calculates
   * the angle between the X-axis and the vector, ignoring the z component of
   * the vector.
   *
   * @param a the vector of which to calculate the theta angle
   * @returns the radian value in the range [0, 2*pi] that represents the angle
   * between the x axis and this vector (in the xy plane)
   * @throws ZeroVectorException if the vector passed equals the zero vector,
   * for which the theta value is undefined
   */
  public static float theta(Vector3d a) {
    float x = a.x, y = a.y;
    if (x == 0) { // tangent undefined for x = 0
      if (y == 0) throw new ZeroVectorException("undefined");
      if (y < 0) return (float) (1.5 * Math.PI);
      return (float) (0.5 * Math.PI);
    }
    float t = (float) Math.atan(y / x); // calculate theta

    if (x < 0) return (float) Math.PI - t; // adjust quadrant

    if (t < 0) t += 2 * Math.PI; // range adjustment [0, 2*pi]

    return t;
  }

  /**
   * The <code>phi</code> method takes a <code>Vector3d</code> and calculates
   * the elevation between the XY-plane and the vector.
   *
   * @param a the vector of which to calculate the phi angle
   * @returns the radian value in the range [-pi/2, pi/2] that represents the
   * angle between the x axis and this vector (in the xy plane)
   * @throws ZeroVectorException if the vector passed equals the zero vector,
   * for which the phi value is undefined
   */
  public static float phi(Vector3d a) {
    float x = a.x, y = a.y, z = a.z;
    if (x == 0 && y == 0) { // tangent undefined for h = 0
      if (z == 0) throw new ZeroVectorException("undefined");
      if (z < 0) return (float) (-0.5 * Math.PI);
      return (float) (0.5 * Math.PI);
    }
    float h = (float) Math.sqrt(x * x + y * y);
    float t = (float) Math.atan(y / h); // calculate phi

    return t;
  }

  /**************************/
  /* 2D Vector Computations */
  /**************************/

  /**
   * The <code>add</code> method takes two vectors and adds them, placing the
   * result in a third vector.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @param dest the destination Vector2d to store the result
   */
  public static void add(Vector2d a, Vector2d b, Vector2d dest) {
    dest.x = a.x + b.x;
    dest.y = a.y + b.y;
  }

  /**
   * The <code>subtract</code> method takes two vectors and subtracts them,
   * placing the result in a third vector.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @param dest the destination Vector2d to store the result
   */
  public static void subtract(Vector2d a, Vector2d b, Vector2d dest) {
    dest.x = a.x - b.x;
    dest.y = a.y - b.y;
  }

  /**
   * The <code>scale</code> method takes a <code>Vector2d</code> and a scalar
   * float value multiplies each component of the Vector, storing the result
   * in the third parameter.
   *
   * @param a the value of the first vector
   * @param scale the value to scale the vector by
   * @param dest the destination Vector2d to store the result
   */
  public static void scale(Vector2d a, float scale, Vector2d dest) {
    dest.x = a.x * scale;
    dest.y = a.y * scale;
  }

  /**
   * The <code>normalize</code> method takes a <code>Vector2d</code> and if it
   * is non-zero, will normalize it so that its magnitude will be 1.
   *
   * @param a the value of the vector to normalize
   * @param dest the destination Vector2d to store the result
   * @throws ZeroVectorException if the vector is zero
   */
  public static void normalize(Vector2d a, Vector2d dest) {
    float mag = magnitude(a);
    if (mag == 0) { throw new ZeroVectorException("undefined"); }
    scale(a, 1 / mag, dest);
  }

  /**
   * The <code>magnitude</code> method takes a <code>Vector2d</code> and
   * computes its magnitude according the Euclidean norm.
   *
   * @param a the value of the vector of which to compute the magnitude
   * @returns the magnitude of the vector
   */
  public static float magnitude(Vector2d a) {
    return (float) Math.sqrt(a.x * a.x + a.y * a.y);
  }

  /**
   * The <code>distance</code> method takes two vectors and computes their
   * (Euclidean) distance.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @returns the distance between the two vectors
   */
  public static float distance(Vector2d a, Vector2d b) {
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    return (float) Math.sqrt(dx * dx + dy * dy);
  }

  /**
   * The <code>sqDistance</code> method takes two vectors and computes the
   * square of their (Euclidean) distance. This is just an optimization for
   * the <code>distance</code> method that avoids an expensive floating point
   * square root computation.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @returns the square of the distance between the two vectors
   */
  public static float sqDistance(Vector2d a, Vector2d b) {
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    return dx * dx + dy * dy;
  }

  /**
   * The <code>dotProduct</code> method computes the dot product between two
   * vectors using the standard inner product formula.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @returns the value of their dot product
   */
  public static float dotProduct(Vector2d a, Vector2d b) {
    return a.x * b.x + a.y * b.y;
  }

  /**
   * The <code>quadrant</code> method is a utility function for two dimensional
   * vectors that takes a vector as a parameter and will return an integer
   * describing what quadrant of the xy plane the vector lies in.
   *
   * @param a the vector to compute the quadrant of
   * @returns the integer VectorConstants.XX_QUADRANT value corresponding to
   * which quadrant the vector lies in
   */
  public static int quadrant(Vector2d a) {
    float x = a.x, y = a.y;
    float xy = x * y;
    if (xy == 0) {
      return VectorConstants.NO_QUADRANT; // lies on axis
    }
    if (xy > 0) {
      if (x > 0) {
        return VectorConstants.NE_QUADRANT;
      }
      else {
        return VectorConstants.SW_QUADRANT;
      }
    }
    else {
      if (x < 0) {
        return VectorConstants.NW_QUADRANT;
      }
      else {
        return VectorConstants.SE_QUADRANT;
      }
    }
  }

  /******************************/
  /* 2D / 3D Mixed Computations */
  /******************************/

  /**
   * The <code>convert</code> methods have been overridden to allow 2d vectors
   * to be converted to 3d vectors and vice versa.
   *
   * @param src the value of the source vector
   * @param dest the value of the destination vector
   */
  public static void convert(Vector3d src, Vector2d dest) {
    dest.x = src.x;
    dest.y = src.y;
  }

  /**
   * The <code>convert</code> methods have been overridden to allow 2d vectors
   * to be converted to 3d vectors and vice versa.
   *
   * @param src the value of the source vector
   * @param dest the value of the destination vector
   */
  public static void convert(Vector2d src, Vector3d dest) {
    dest.x = src.x;
    dest.y = src.y;
    dest.z = 0;
  }

  /**
   * The <code>distance</code> method takes two vectors and computes their
   * (Euclidean) distance. It has been overloaded to allow the computation
   * of the distance between a 3d vector and a 2d vector.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @returns the distance between the two vectors
   */
  public static float distance(Vector3d a, Vector2d b) {
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    return (float) Math.sqrt(dx * dx + dy * dy);
  }

  /**
   * The <code>distance</code> method takes two vectors and computes their
   * (Euclidean) distance. It has been overloaded to allow the computation
   * of the distance between a 3d vector and a 2d vector.
   *
   * @param a
   *            the value of the first vector
   * @param b
   *            the value of the second vector
   * @returns the distance between the two vectors
   */
  public static float distance(Vector2d a, Vector3d b) {
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    return (float) Math.sqrt(dx * dx + dy * dy);
  }

  /**
   * The <code>sqDistance</code> method takes two vectors and computes the
   * square of their (Euclidean) distance. This is just an optimization for
   * the <code>distance</code> method that avoids an expensive floating point
   * square root computation. It has been overloaded to allow the computation
   * of the distance between a 3d vector and a 2d vector.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @returns the square of the distance between the two vectors
   */
  public static float sqDistance(Vector3d a, Vector2d b) {
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    return dx * dx + dy * dy;
  }

  /**
   * The <code>sqDistance</code> method takes two vectors and computes the
   * square of their (Euclidean) distance. This is just an optimization for
   * the <code>distance</code> method that avoids an expensive floating point
   * square root computation. It has been overloaded to allow the computation
   * of the distance between a 3d vector and a 2d vector.
   *
   * @param a the value of the first vector
   * @param b the value of the second vector
   * @returns the square of the distance between the two vectors
   */
  public static float sqDistance(Vector2d a, Vector3d b) {
    float dx = a.x - b.x;
    float dy = a.y - b.y;
    return dx * dx + dy * dy;
  }
}
