package scd_micro;
/**
* 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 optimized for
* performance, thus the methods are static, contain no calls, create no objects, and do not perform
* error checking or synchronization.
*
* @author Ben L. Titzer
*/
final class VectorMath {
/***********************************************************************************************
* 3 D V e c t o r C o m p u t a t i o n s
**********************************************************************************************/
/**
* 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("normalize: |a| = 0");
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("theta: |y.0| = 0");
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;
}
/***********************************************************************************************
* 2 D V e c t o r C o m p u t a t i o n s
**********************************************************************************************/
/**
* 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;
}
}
/***********************************************************************************************
* 2 D / 3 D M i x e d C o m p u t a t i o n s
**********************************************************************************************/
/**
* 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;
}
}