diff --git a/src/main/java/pythagoras/d/AbstractMatrix3.java b/src/main/java/pythagoras/d/AbstractMatrix3.java new file mode 100644 index 0000000..7697479 --- /dev/null +++ b/src/main/java/pythagoras/d/AbstractMatrix3.java @@ -0,0 +1,346 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.d; + +import java.nio.DoubleBuffer; + +import pythagoras.util.Platform; +import pythagoras.util.SingularMatrixException; + +/** + * Provides most of the implementation of {@link IMatrix3}, obtaining only the components from the + * derived class. + */ +public abstract class AbstractMatrix3 implements IMatrix3 +{ + @Override // from IVector3 + public Matrix3 transpose () { + return transpose(new Matrix3()); + } + + @Override // from IVector3 + public Matrix3 transpose (Matrix3 result) { + return result.set( + m00(), m01(), m02(), + m10(), m11(), m12(), + m20(), m21(), m22()); + } + + @Override // from IVector3 + public Matrix3 mult (IMatrix3 other) { + return mult(other, new Matrix3()); + } + + @Override // from IVector3 + public Matrix3 mult (IMatrix3 other, Matrix3 result) { + double m00 = m00(), m01 = m01(), m02 = m02(); + double m10 = m10(), m11 = m11(), m12 = m12(); + double m20 = m20(), m21 = m21(), m22 = m22(); + double om00 = other.m00(), om01 = other.m01(), om02 = other.m02(); + double om10 = other.m10(), om11 = other.m11(), om12 = other.m12(); + double om20 = other.m20(), om21 = other.m21(), om22 = other.m22(); + return result.set( + m00*om00 + m10*om01 + m20*om02, + m00*om10 + m10*om11 + m20*om12, + m00*om20 + m10*om21 + m20*om22, + + m01*om00 + m11*om01 + m21*om02, + m01*om10 + m11*om11 + m21*om12, + m01*om20 + m11*om21 + m21*om22, + + m02*om00 + m12*om01 + m22*om02, + m02*om10 + m12*om11 + m22*om12, + m02*om20 + m12*om21 + m22*om22); + } + + @Override // from IVector3 + public boolean isAffine () { + return (m02() == 0f && m12() == 0f && m22() == 1f); + } + + @Override // from IVector3 + public Matrix3 multAffine (IMatrix3 other) { + return multAffine(other, new Matrix3()); + } + + @Override // from IVector3 + public Matrix3 multAffine (IMatrix3 other, Matrix3 result) { + double m00 = m00(), m01 = m01(), m02 = m02(); + double m10 = m10(), m11 = m11(), m12 = m12(); + double m20 = m20(), m21 = m21(), m22 = m22(); + double om00 = other.m00(), om01 = other.m01(), om02 = other.m02(); + double om10 = other.m10(), om11 = other.m11(), om12 = other.m12(); + double om20 = other.m20(), om21 = other.m21(), om22 = other.m22(); + return result.set( + m00*om00 + m10*om01, + m00*om10 + m10*om11, + m00*om20 + m10*om21 + m20, + + m01*om00 + m11*om01, + m01*om10 + m11*om11, + m01*om20 + m11*om21 + m21, + + 0f, 0f, 1f); + } + + @Override // from IVector3 + public Matrix3 invert () { + return invert(new Matrix3()); + } + + /** + * Inverts this matrix and places the result in the given object. This code is based on the + * examples in the Matrix and + * Quaternion FAQ. + * + * @return a reference to the result matrix, for chaining. + */ + public Matrix3 invert (Matrix3 result) throws SingularMatrixException { + double m00 = m00(), m01 = m01(), m02 = m02(); + double m10 = m10(), m11 = m11(), m12 = m12(); + double m20 = m20(), m21 = m21(), m22 = m22(); + // compute the determinant, storing the subdeterminants for later use + double sd00 = m11*m22 - m21*m12; + double sd10 = m01*m22 - m21*m02; + double sd20 = m01*m12 - m11*m02; + double det = m00*sd00 + m20*sd20 - m10*sd10; + if (Math.abs(det) == 0f) { + // determinant is zero; matrix is not invertible + throw new SingularMatrixException(this.toString()); + } + double rdet = 1f / det; + return result.set( + +sd00 * rdet, + -(m10*m22 - m20*m12) * rdet, + +(m10*m21 - m20*m11) * rdet, + + -sd10 * rdet, + +(m00*m22 - m20*m02) * rdet, + -(m00*m21 - m20*m01) * rdet, + + +sd20 * rdet, + -(m00*m12 - m10*m02) * rdet, + +(m00*m11 - m10*m01) * rdet); + } + + @Override // from IVector3 + public Matrix3 invertAffine () { + return invertAffine(new Matrix3()); + } + + @Override // from IVector3 + public Matrix3 invertAffine (Matrix3 result) throws SingularMatrixException { + double m00 = m00(), m01 = m01(), m02 = m02(); + double m10 = m10(), m11 = m11(), m12 = m12(); + double m20 = m20(), m21 = m21(), m22 = m22(); + // compute the determinant, storing the subdeterminants for later use + double det = m00*m11 - m10*m01; + if (Math.abs(det) == 0f) { + // determinant is zero; matrix is not invertible + throw new SingularMatrixException(this.toString()); + } + double rdet = 1f / det; + return result.set( + +m11 * rdet, + -m10 * rdet, + +(m10*m21 - m20*m11) * rdet, + + -m01 * rdet, + +m00 * rdet, + -(m00*m21 - m20*m01) * rdet, + + 0f, 0f, 1f); + } + + @Override // from IVector3 + public Matrix3 lerp (IMatrix3 other, double t) { + return lerp(other, t, new Matrix3()); + } + + @Override // from IVector3 + public Matrix3 lerp (IMatrix3 other, double t, Matrix3 result) { + double m00 = m00(), m01 = m01(), m02 = m02(); + double m10 = m10(), m11 = m11(), m12 = m12(); + double m20 = m20(), m21 = m21(), m22 = m22(); + double om00 = other.m00(), om01 = other.m01(), om02 = other.m02(); + double om10 = other.m10(), om11 = other.m11(), om12 = other.m12(); + double om20 = other.m20(), om21 = other.m21(), om22 = other.m22(); + return result.set( + m00 + t*(om00 - m00), + m10 + t*(om10 - m10), + m20 + t*(om20 - m20), + + m01 + t*(om01 - m01), + m11 + t*(om11 - m11), + m21 + t*(om21 - m21), + + m02 + t*(om02 - m02), + m12 + t*(om12 - m12), + m22 + t*(om22 - m22)); + } + + @Override // from IVector3 + public Matrix3 lerpAffine (IMatrix3 other, double t) { + return lerpAffine(other, t, new Matrix3()); + } + + @Override // from IVector3 + public Matrix3 lerpAffine (IMatrix3 other, double t, Matrix3 result) { + double m00 = m00(), m01 = m01(), m02 = m02(); + double m10 = m10(), m11 = m11(), m12 = m12(); + double m20 = m20(), m21 = m21(), m22 = m22(); + double om00 = other.m00(), om01 = other.m01(), om02 = other.m02(); + double om10 = other.m10(), om11 = other.m11(), om12 = other.m12(); + double om20 = other.m20(), om21 = other.m21(), om22 = other.m22(); + return result.set( + m00 + t*(om00 - m00), + m10 + t*(om10 - m10), + m20 + t*(om20 - m20), + + m01 + t*(om01 - m01), + m11 + t*(om11 - m11), + m21 + t*(om21 - m21), + + 0f, 0f, 1f); + } + + @Override // from IVector3 + public DoubleBuffer get (DoubleBuffer buf) { + buf.put(m00()).put(m01()).put(m02()); + buf.put(m10()).put(m11()).put(m12()); + buf.put(m20()).put(m21()).put(m22()); + return buf; + } + + @Override // from IVector3 + public Vector3 transformLocal (Vector3 vector) { + return transform(vector, vector); + } + + @Override // from IVector3 + public Vector3 transform (IVector3 vector) { + return transform(vector, new Vector3()); + } + + @Override // from IVector3 + public Vector3 transform (IVector3 vector, Vector3 result) { + double vx = vector.x(), vy = vector.y(), vz = vector.z(); + return result.set( + m00()*vx + m10()*vy + m20()*vz, + m01()*vx + m11()*vy + m21()*vz, + m02()*vx + m12()*vy + m22()*vz); + } + + @Override // from IVector3 + public Vector transformPointLocal (Vector point) { + return transformPoint(point, point); + } + + @Override // from IVector3 + public Vector transformPoint (IVector point) { + return transformPoint(point, new Vector()); + } + + @Override // from IVector3 + public Vector transformPoint (IVector point, Vector result) { + double px = point.x(), py = point.y(); + return result.set(m00()*px + m10()*py + m20(), m01()*px + m11()*py + m21()); + } + + @Override // from IVector3 + public Vector transformVectorLocal (Vector vector) { + return transformVector(vector, vector); + } + + @Override // from IVector3 + public Vector transformVector (IVector vector) { + return transformVector(vector, new Vector()); + } + + @Override // from IVector3 + public Vector transformVector (IVector vector, Vector result) { + double vx = vector.x(), vy = vector.y(); + return result.set(m00()*vx + m10()*vy, m01()*vx + m11()*vy); + } + + @Override // from IVector3 + public double extractRotation () { + // start with the contents of the upper 2x2 portion of the matrix + double n00 = m00(), n10 = m10(); + double n01 = m01(), n11 = m11(); + for (int ii = 0; ii < 10; ii++) { + // store the results of the previous iteration + double o00 = n00, o10 = n10; + double o01 = n01, o11 = n11; + + // compute average of the matrix with its inverse transpose + double det = o00*o11 - o10*o01; + if (Math.abs(det) == 0f) { + // determinant is zero; matrix is not invertible + throw new SingularMatrixException(this.toString()); + } + double hrdet = 0.5f / det; + n00 = +o11 * hrdet + o00*0.5f; + n10 = -o01 * hrdet + o10*0.5f; + + n01 = -o10 * hrdet + o01*0.5f; + n11 = +o00 * hrdet + o11*0.5f; + + // compute the difference; if it's small enough, we're done + double d00 = n00 - o00, d10 = n10 - o10; + double d01 = n01 - o01, d11 = n11 - o11; + if (d00*d00 + d10*d10 + d01*d01 + d11*d11 < MathUtil.EPSILON) { + break; + } + } + // now that we have a nice orthogonal matrix, we can extract the rotation + return Math.atan2(n01, n00); + } + + @Override // from IVector3 + public Vector extractScale () { + return extractScale(new Vector()); + } + + @Override // from IVector3 + public Vector extractScale (Vector result) { + double m00 = m00(), m01 = m01(), m10 = m10(), m11 = m11(); + return result.set( + Math.sqrt(m00*m00 + m01*m01), + Math.sqrt(m10*m10 + m11*m11)); + } + + @Override // from IVector3 + public double approximateUniformScale () { + double cp = m00()*m11() - m01()*m10(); + return (cp < 0f) ? -Math.sqrt(-cp) : Math.sqrt(cp); + } + + @Override + public String toString () { + return "[[" + m00() + ", " + m10() + ", " + m20() + "], " + + "[" + m01() + ", " + m11() + ", " + m21() + "], " + + "[" + m02() + ", " + m12() + ", " + m22() + "]]"; + } + + @Override + public int hashCode () { + return Platform.hashCode(m00()) ^ Platform.hashCode(m10()) ^ Platform.hashCode(m20()) ^ + Platform.hashCode(m01()) ^ Platform.hashCode(m11()) ^ Platform.hashCode(m21()) ^ + Platform.hashCode(m02()) ^ Platform.hashCode(m12()) ^ Platform.hashCode(m22()); + } + + @Override + public boolean equals (Object other) { + if (!(other instanceof AbstractMatrix3)) { + return false; + } + AbstractMatrix3 omat = (AbstractMatrix3)other; + return + m00() == omat.m00() && m10() == omat.m10() && m20() == omat.m20() && + m01() == omat.m01() && m11() == omat.m11() && m21() == omat.m21() && + m02() == omat.m02() && m12() == omat.m12() && m22() == omat.m22(); + } +} diff --git a/src/main/java/pythagoras/d/AbstractVector3.java b/src/main/java/pythagoras/d/AbstractVector3.java new file mode 100644 index 0000000..d3b8d8c --- /dev/null +++ b/src/main/java/pythagoras/d/AbstractVector3.java @@ -0,0 +1,204 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.d; + +import java.nio.DoubleBuffer; + +import pythagoras.util.Platform; + +/** + * Provides most of the implementation of {@link IVector3}, obtaining only x, y and z from the + * derived class. + */ +public abstract class AbstractVector3 implements IVector3 +{ + @Override // from interface IVector3 + public double dot (IVector3 other) { + return x()*other.x() + y()*other.y() + z()*other.z(); + } + + @Override // from interface IVector3 + public Vector3 cross (IVector3 other) { + return cross(other, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 cross (IVector3 other, Vector3 result) { + double x = x(), y = y(), z = z(); + double ox = other.x(), oy = other.y(), oz = other.z(); + return result.set(y*oz - z*oy, z*ox - x*oz, x*oy - y*ox); + } + + @Override // from interface IVector3 + public double triple (IVector3 b, IVector3 c) { + double bx = b.x(), by = b.y(), bz = b.z(); + double cx = c.x(), cy = c.y(), cz = c.z(); + return x()*(by*cz - bz*cy) + y()*(bz*cx - bx*cz) + z()*(bx*cy - by*cx); + } + + @Override // from interface IVector3 + public Vector3 negate () { + return negate(new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 negate (Vector3 result) { + return result.set(-x(), -y(), -z()); + } + + @Override // from interface IVector3 + public Vector3 normalize () { + return normalize(new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 normalize (Vector3 result) { + return mult(1f / length(), result); + } + + @Override // from interface IVector3 + public double angle (IVector3 other) { + return Math.acos(dot(other) / (length() * other.length())); + } + + @Override // from interface IVector3 + public double length () { + return Math.sqrt(lengthSquared()); + } + + @Override // from interface IVector3 + public double lengthSquared () { + double x = x(), y = y(), z = z(); + return (x*x + y*y + z*z); + } + + @Override // from interface IVector3 + public double distance (IVector3 other) { + return Math.sqrt(distanceSquared(other)); + } + + @Override // from interface IVector3 + public double distanceSquared (IVector3 other) { + double dx = x() - other.x(), dy = y() - other.y(), dz = z() - other.z(); + return dx*dx + dy*dy + dz*dz; + } + + @Override // from interface IVector3 + public double manhattanDistance (IVector3 other) { + return Math.abs(x() - other.x()) + Math.abs(y() - other.y()) + Math.abs(z() - other.z()); + } + + @Override // from interface IVector3 + public Vector3 mult (double v) { + return mult(v, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 mult (double v, Vector3 result) { + return result.set(x()*v, y()*v, z()*v); + } + + @Override // from interface IVector3 + public Vector3 mult (IVector3 other) { + return mult(other, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 mult (IVector3 other, Vector3 result) { + return result.set(x()*other.x(), y()*other.y(), z()*other.z()); + } + + @Override // from interface IVector3 + public Vector3 add (IVector3 other) { + return add(other, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 add (IVector3 other, Vector3 result) { + return add(other.x(), other.y(), other.z(), result); + } + + @Override // from interface IVector3 + public Vector3 subtract (IVector3 other) { + return subtract(other, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 subtract (IVector3 other, Vector3 result) { + return add(-other.x(), -other.y(), -other.z(), result); + } + + @Override // from interface IVector3 + public Vector3 add (double x, double y, double z) { + return add(x, y, z, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 add (double x, double y, double z, Vector3 result) { + return result.set(x() + x, y() + y, z() + z); + } + + @Override // from interface IVector3 + public Vector3 addScaled (IVector3 other, double v) { + return addScaled(other, v, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 addScaled (IVector3 other, double v, Vector3 result) { + return result.set(x() + other.x()*v, y() + other.y()*v, z() + other.z()*v); + } + + @Override // from interface IVector3 + public Vector3 lerp (IVector3 other, double t) { + return lerp(other, t, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 lerp (IVector3 other, double t, Vector3 result) { + double x = x(), y = y(), z = z(); + return result.set(x + t*(other.x() - x), y + t*(other.y() - y), z + t*(other.z() - z)); + } + + @Override // from interface IVector3 + public double get (int idx) { + switch (idx) { + case 0: return x(); + case 1: return y(); + case 2: return z(); + } + throw new IndexOutOfBoundsException(String.valueOf(idx)); + } + + @Override // from interface IVector3 + public void get (double[] values) { + values[0] = x(); + values[1] = y(); + values[2] = z(); + } + + @Override // from interface IVector3 + public DoubleBuffer get (DoubleBuffer buf) { + return buf.put(x()).put(y()).put(z()); + } + + @Override + public String toString () { + return "[" + x() + ", " + y() + ", " + z() + "]"; + } + + @Override + public int hashCode () { + return Platform.hashCode(x()) ^ Platform.hashCode(y()) ^ Platform.hashCode(z()); + } + + @Override + public boolean equals (Object other) { + if (!(other instanceof AbstractVector3)) { + return false; + } + AbstractVector3 ovec = (AbstractVector3)other; + return (x() == ovec.x() && y() == ovec.y() && z() == ovec.z()); + } +} diff --git a/src/main/java/pythagoras/d/IMatrix3.java b/src/main/java/pythagoras/d/IMatrix3.java new file mode 100644 index 0000000..403cc34 --- /dev/null +++ b/src/main/java/pythagoras/d/IMatrix3.java @@ -0,0 +1,250 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.d; + +import java.nio.DoubleBuffer; + +import pythagoras.util.SingularMatrixException; + +/** + * Provides read-only access to a {@link Matrix3}. + */ +interface IMatrix3 +{ + /** Returns the (0,0)th component of the matrix. */ + double m00 (); + + /** Returns the (1,0)th component of the matrix. */ + double m10 (); + + /** Returns the (2,0)th component of the matrix. */ + double m20 (); + + /** Returns the (0,1)th component of the matrix. */ + double m01 (); + + /** Returns the (1,1)th component of the matrix. */ + double m11 (); + + /** Returns the (2,1)th component of the matrix. */ + double m21 (); + + /** Returns the (0,2)th component of the matrix. */ + double m02 (); + + /** Returns the (1,2)th component of the matrix. */ + double m12 (); + + /** Returns the (2,2)th component of the matrix. */ + double m22 (); + + /** + * Transposes this matrix. + * + * @return a new matrix containing the result. + */ + Matrix3 transpose (); + + /** + * Transposes this matrix, storing the result in the provided object. + * + * @return the result matrix, for chaining. + */ + Matrix3 transpose (Matrix3 result); + + /** + * Multiplies this matrix by another. + * + * @return a new matrix containing the result. + */ + Matrix3 mult (IMatrix3 other); + + /** + * Multiplies this matrix by another and stores the result in the object provided. + * + * @return a reference to the result matrix, for chaining. + */ + Matrix3 mult (IMatrix3 other, Matrix3 result); + + /** + * Determines whether this matrix represents an affine transformation. + */ + boolean isAffine (); + + /** + * Multiplies this matrix by another, treating the matrices as affine. + * + * @return a new matrix containing the result. + */ + Matrix3 multAffine (IMatrix3 other); + + /** + * Multiplies this matrix by another, treating the matrices as affine, and stores the result + * in the object provided. + * + * @return a reference to the result matrix, for chaining. + */ + Matrix3 multAffine (IMatrix3 other, Matrix3 result); + + /** + * Inverts this matrix. + * + * @return a new matrix containing the result. + */ + Matrix3 invert (); + + /** + * Inverts this matrix and places the result in the given object. This code is based on the + * examples in the Matrix and + * Quaternion FAQ. + * + * @return a reference to the result matrix, for chaining. + */ + Matrix3 invert (Matrix3 result) throws SingularMatrixException; + + /** + * Inverts this matrix as an affine matrix. + * + * @return a new matrix containing the result. + */ + Matrix3 invertAffine (); + + /** + * Inverts this matrix as an affine matrix and places the result in the given object. + * + * @return a reference to the result matrix, for chaining. + */ + Matrix3 invertAffine (Matrix3 result) throws SingularMatrixException; + + /** + * Linearly interpolates between this and the specified other matrix. + * + * @return a new matrix containing the result. + */ + Matrix3 lerp (IMatrix3 other, double t); + + /** + * Linearly interpolates between this and the specified other matrix, placing the result in + * the object provided. + * + * @return a reference to the result object, for chaining. + */ + Matrix3 lerp (IMatrix3 other, double t, Matrix3 result); + + /** + * Linearly interpolates between this and the specified other matrix, treating the matrices as + * affine. + * + * @return a new matrix containing the result. + */ + Matrix3 lerpAffine (IMatrix3 other, double t); + + /** + * Linearly interpolates between this and the specified other matrix (treating the matrices as + * affine), placing the result in the object provided. + * + * @return a reference to the result object, for chaining. + */ + Matrix3 lerpAffine (IMatrix3 other, double t, Matrix3 result); + + /** + * Places the contents of this matrix into the given buffer in the standard OpenGL order. + * + * @return a reference to the buffer, for chaining. + */ + DoubleBuffer get (DoubleBuffer buf); + + /** + * Transforms a vector in-place by the inner 3x3 part of this matrix. + * + * @return a reference to the vector, for chaining. + */ + Vector3 transformLocal (Vector3 vector); + + /** + * Transforms a vector by this matrix. + * + * @return a new vector containing the result. + */ + Vector3 transform (IVector3 vector); + + /** + * Transforms a vector by this matrix and places the result in the object provided. + * + * @return a reference to the result, for chaining. + */ + Vector3 transform (IVector3 vector, Vector3 result); + + /** + * Transforms a point in-place by this matrix. + * + * @return a reference to the point, for chaining. + */ + Vector transformPointLocal (Vector point); + + /** + * Transforms a point by this matrix. + * + * @return a new vector containing the result. + */ + Vector transformPoint (IVector point); + + /** + * Transforms a point by this matrix and places the result in the object provided. + * + * @return a reference to the result, for chaining. + */ + Vector transformPoint (IVector point, Vector result); + + /** + * Transforms a vector in-place by the inner 2x2 part of this matrix. + * + * @return a reference to the vector, for chaining. + */ + Vector transformVectorLocal (Vector vector); + + /** + * Transforms a vector by this inner 2x2 part of this matrix. + * + * @return a new vector containing the result. + */ + Vector transformVector (IVector vector); + + /** + * Transforms a vector by the inner 2x2 part of this matrix and places the result in the object + * provided. + * + * @return a reference to the result, for chaining. + */ + Vector transformVector (IVector vector, Vector result); + + /** + * Extracts the rotation component of the matrix. This uses the iterative polar decomposition + * algorithm described by + * Ken + * Shoemake. + */ + double extractRotation (); + + /** + * Extracts the scale component of the matrix. + * + * @return a new vector containing the result. + */ + Vector extractScale (); + + /** + * Extracts the scale component of the matrix and places it in the provided result vector. + * + * @return a reference to the result vector, for chaining. + */ + Vector extractScale (Vector result); + + /** + * Returns an approximation of the uniform scale for this matrix (the square root of the + * signed area of the parallelogram spanned by the axis vectors). + */ + double approximateUniformScale (); +} diff --git a/src/main/java/pythagoras/d/IVector3.java b/src/main/java/pythagoras/d/IVector3.java new file mode 100644 index 0000000..30cfab5 --- /dev/null +++ b/src/main/java/pythagoras/d/IVector3.java @@ -0,0 +1,221 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.d; + +import java.nio.DoubleBuffer; + +/** + * Provides read-only access to a {@link Vector3}. + */ +public interface IVector3 +{ + /** Returns the x-component of this vector. */ + double x (); + + /** Returns the y-component of this vector. */ + double y (); + + /** Returns the z-component of this vector. */ + double z (); + + /** + * Computes and returns the dot product of this and the specified other vector. + */ + double dot (IVector3 other); + + /** + * Computes the cross product of this and the specified other vector. + * + * @return a new vector containing the result. + */ + Vector3 cross (IVector3 other); + + /** + * Computes the cross product of this and the specified other vector, placing the result + * in the object supplied. + * + * @return a reference to the result, for chaining. + */ + Vector3 cross (IVector3 other, Vector3 result); + + /** + * Computes the triple product of this and the specified other vectors, which is equal to + * this.dot(b.cross(c)). + */ + double triple (IVector3 b, IVector3 c); + + /** + * Negates this vector. + * + * @return a new vector containing the result. + */ + Vector3 negate (); + + /** + * Negates this vector, storing the result in the supplied object. + * + * @return a reference to the result, for chaining. + */ + Vector3 negate (Vector3 result); + + /** + * Normalizes this vector. + * + * @return a new vector containing the result. + */ + Vector3 normalize (); + + /** + * Normalizes this vector, storing the result in the object supplied. + * + * @return a reference to the result, for chaining. + */ + Vector3 normalize (Vector3 result); + + /** + * Returns the angle between this vector and the specified other vector. + */ + double angle (IVector3 other); + + /** + * Returns the length of this vector. + */ + double length (); + + /** + * Returns the squared length of this vector. + */ + double lengthSquared (); + + /** + * Returns the distance from this vector to the specified other vector. + */ + double distance (IVector3 other); + + /** + * Returns the squared distance from this vector to the specified other. + */ + double distanceSquared (IVector3 other); + + /** + * Returns the Manhattan distance between this vector and the specified other. + */ + double manhattanDistance (IVector3 other); + + /** + * Multiplies this vector by a scalar. + * + * @return a new vector containing the result. + */ + Vector3 mult (double v); + + /** + * Multiplies this vector by a scalar and places the result in the supplied object. + * + * @return a reference to the result, for chaining. + */ + Vector3 mult (double v, Vector3 result); + + /** + * Multiplies this vector by another. + * + * @return a new vector containing the result. + */ + Vector3 mult (IVector3 other); + + /** + * Multiplies this vector by another, storing the result in the object provided. + * + * @return a reference to the result vector, for chaining. + */ + Vector3 mult (IVector3 other, Vector3 result); + + /** + * Adds a vector to this one. + * + * @return a new vector containing the result. + */ + Vector3 add (IVector3 other); + /** + * Adds a vector to this one, storing the result in the object provided. + * + * @return a reference to the result, for chaining. + */ + public IVector3 add (IVector3 other, Vector3 result); + + /** + * Subtracts a vector from this one. + * + * @return a new vector containing the result. + */ + Vector3 subtract (IVector3 other); + + /** + * Subtracts a vector from this one and places the result in the supplied object. + * + * @return a reference to the result, for chaining. + */ + Vector3 subtract (IVector3 other, Vector3 result); + + /** + * Adds a vector to this one. + * + * @return a new vector containing the result. + */ + Vector3 add (double x, double y, double z); + + /** + * Adds a vector to this one and stores the result in the object provided. + * + * @return a reference to the result, for chaining. + */ + Vector3 add (double x, double y, double z, Vector3 result); + + /** + * Adds a scaled vector to this one. + * + * @return a new vector containing the result. + */ + Vector3 addScaled (IVector3 other, double v); + + /** + * Adds a scaled vector to this one and stores the result in the supplied vector. + * + * @return a reference to the result, for chaining. + */ + Vector3 addScaled (IVector3 other, double v, Vector3 result); + + /** + * Linearly interpolates between this and the specified other vector by the supplied amount. + * + * @return a new vector containing the result. + */ + Vector3 lerp (IVector3 other, double t); + + /** + * Linearly interpolates between this and the supplied other vector by the supplied amount, + * storing the result in the supplied object. + * + * @return a reference to the result, for chaining. + */ + Vector3 lerp (IVector3 other, double t, Vector3 result); + + /** + * Returns the element at the idx'th position of the vector. + */ + double get (int idx); + + /** + * Populates the supplied array with the contents of this vector. + */ + void get (double[] values); + + /** + * Populates the supplied buffer with the contents of this vector. + * + * @return a reference to the buffer, for chaining. + */ + DoubleBuffer get (DoubleBuffer buf); +} diff --git a/src/main/java/pythagoras/d/Matrix3.java b/src/main/java/pythagoras/d/Matrix3.java new file mode 100644 index 0000000..a7f65fc --- /dev/null +++ b/src/main/java/pythagoras/d/Matrix3.java @@ -0,0 +1,406 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.d; + +import java.nio.DoubleBuffer; + +/** + * A 3x3 column-major matrix. + */ +public class Matrix3 extends AbstractMatrix3 +{ + /** The identity matrix. */ + public static final Matrix3 IDENTITY = new Matrix3(); + + /** The values of the matrix. */ + public double m00, m10, m20; + public double m01, m11, m21; + public double m02, m12, m22; + + /** + * Creates a matrix from its components. + */ + public Matrix3 (double m00, double m10, double m20, + double m01, double m11, double m21, + double m02, double m12, double m22) { + set(m00, m10, m20, + m01, m11, m21, + m02, m12, m22); + } + + /** + * Creates a matrix from an array of values. + */ + public Matrix3 (double[] values) { + set(values); + } + + /** + * Copy constructor. + */ + public Matrix3 (Matrix3 other) { + set(other); + } + + /** + * Creates an identity matrix. + */ + public Matrix3 () { + setToIdentity(); + } + + /** + * Sets this matrix to the identity matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToIdentity () { + return set( + 1f, 0f, 0f, + 0f, 1f, 0f, + 0f, 0f, 1f); + } + + /** + * Sets this to a rotation matrix that rotates one vector onto another. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToRotation (IVector3 from, IVector3 to) { + double angle = from.angle(to); + return (angle < 0.0001f) ? + setToIdentity() : setToRotation(angle, from.cross(to).normalizeLocal()); + } + + /** + * Sets this to a rotation matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToRotation (double angle, IVector3 axis) { + return setToRotation(angle, axis.x(), axis.y(), axis.z()); + } + + /** + * Sets this to a rotation matrix. The formula comes from the OpenGL documentation for the + * glRotatef function. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToRotation (double angle, double x, double y, double z) { + double c = Math.cos(angle), s = Math.sin(angle), omc = 1f - c; + double xs = x*s, ys = y*s, zs = z*s, xy = x*y, xz = x*z, yz = y*z; + return set( + x*x*omc + c, xy*omc - zs, xz*omc + ys, + xy*omc + zs, y*y*omc + c, yz*omc - xs, + xz*omc - ys, yz*omc + xs, z*z*omc + c); + } + + // /** + // * Sets this to a rotation matrix. The formula comes from the + // * Matrix and Quaternion FAQ. + // * + // * @return a reference to this matrix, for chaining. + // */ + // public Matrix3 setToRotation (Quaternion quat) { + // double xx = quat.x*quat.x, yy = quat.y*quat.y, zz = quat.z*quat.z; + // double xy = quat.x*quat.y, xz = quat.x*quat.z, xw = quat.x*quat.w; + // double yz = quat.y*quat.z, yw = quat.y*quat.w, zw = quat.z*quat.w; + // return set( + // 1f - 2f*(yy + zz), 2f*(xy - zw), 2f*(xz + yw), + // 2f*(xy + zw), 1f - 2f*(xx + zz), 2f*(yz - xw), + // 2f*(xz - yw), 2f*(yz + xw), 1f - 2f*(xx + yy)); + // } + + /** + * Sets this to a scale matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToScale (IVector3 scale) { + return setToScale(scale.x(), scale.y(), scale.z()); + } + + /** + * Sets this to a uniform scale matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToScale (double s) { + return setToScale(s, s, s); + } + + /** + * Sets this to a scale matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToScale (double x, double y, double z) { + return set( + x, 0f, 0f, + 0f, y, 0f, + 0f, 0f, z); + } + + /** + * Sets this to a reflection across a plane intersecting the origin with the supplied normal. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToReflection (IVector3 normal) { + return setToReflection(normal.x(), normal.y(), normal.z()); + } + + /** + * Sets this to a reflection across a plane intersecting the origin with the supplied normal. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToReflection (double x, double y, double z) { + double x2 = -2f*x, y2 = -2f*y, z2 = -2f*z; + double xy2 = x2*y, xz2 = x2*z, yz2 = y2*z; + return set( + 1f + x2*x, xy2, xz2, + xy2, 1f + y2*y, yz2, + xz2, yz2, 1f + z2*z); + } + + /** + * Sets this to a matrix that first rotates, then translates. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToTransform (IVector translation, double rotation) { + return setToRotation(rotation).setTranslation(translation); + } + + /** + * Sets this to a matrix that first scales, then rotates, then translates. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToTransform (IVector translation, double rotation, double scale) { + return setToRotation(rotation).set( + m00 * scale, m10 * scale, translation.x(), + m01 * scale, m11 * scale, translation.y(), + 0f, 0f, 1f); + } + + /** + * Sets this to a matrix that first scales, then rotates, then translates. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToTransform (IVector translation, double rotation, IVector scale) { + double sx = scale.x(), sy = scale.y(); + return setToRotation(rotation).set( + m00 * sx, m10 * sy, translation.x(), + m01 * sx, m11 * sy, translation.y(), + 0f, 0f, 1f); + } + + /** + * Sets this to a translation matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToTranslation (IVector translation) { + return setToTranslation(translation.x(), translation.y()); + } + + /** + * Sets this to a translation matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToTranslation (double x, double y) { + return set( + 1f, 0f, x, + 0f, 1f, y, + 0f, 0f, 1f); + } + + /** + * Sets the translation component of this matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setTranslation (IVector translation) { + return setTranslation(translation.x(), translation.y()); + } + + /** + * Sets the translation component of this matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setTranslation (double x, double y) { + m20 = x; + m21 = y; + return this; + } + + /** + * Sets this to a rotation matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToRotation (double angle) { + double sina = Math.sin(angle), cosa = Math.cos(angle); + return set( + cosa, -sina, 0f, + sina, cosa, 0f, + 0f, 0f, 1f); + } + + /** + * Transposes this matrix in-place. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 transposeLocal () { + return transpose(this); + } + + /** + * Multiplies this matrix in-place by another. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 multLocal (IMatrix3 other) { + return mult(other, this); + } + + /** + * Multiplies this matrix in-place by another, treating the matricees as affine. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 multAffineLocal (IMatrix3 other) { + return multAffine(other, this); + } + + /** + * Inverts this matrix in-place. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 invertLocal () { + return invert(this); + } + + /** + * Inverts this matrix in-place as an affine matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 invertAffineLocal () { + return invertAffine(this); + } + + /** + * Linearly interpolates between the this and the specified other matrix, placing the result in + * this matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 lerpLocal (IMatrix3 other, double t) { + return lerp(other, t, this); + } + + /** + * Linearly interpolates between this and the specified other matrix (treating the matrices as + * affine), placing the result in this matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 lerpAffineLocal (IMatrix3 other, double t) { + return lerpAffine(other, t, this); + } + + /** + * Copies the contents of another matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 set (IMatrix3 other) { + return set( + other.m00(), other.m10(), other.m20(), + other.m01(), other.m11(), other.m21(), + other.m02(), other.m12(), other.m22()); + } + + /** + * Copies the elements of an array. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 set (double[] values) { + return set( + values[0], values[1], values[2], + values[3], values[4], values[5], + values[6], values[7], values[8]); + } + + /** + * Sets all of the matrix's components at once. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 set ( + double m00, double m10, double m20, + double m01, double m11, double m21, + double m02, double m12, double m22) { + this.m00 = m00; this.m01 = m01; this.m02 = m02; + this.m10 = m10; this.m11 = m11; this.m12 = m12; + this.m20 = m20; this.m21 = m21; this.m22 = m22; + return this; + } + + @Override // from AbstractMatrix3 + public double m00 () { + return m00; + } + + @Override // from AbstractMatrix3 + public double m10 () { + return m10; + } + + @Override // from AbstractMatrix3 + public double m20 () { + return m20; + } + + @Override // from AbstractMatrix3 + public double m01 () { + return m01; + } + + @Override // from AbstractMatrix3 + public double m11 () { + return m11; + } + + @Override // from AbstractMatrix3 + public double m21 () { + return m21; + } + + @Override // from AbstractMatrix3 + public double m02 () { + return m02; + } + + @Override // from AbstractMatrix3 + public double m12 () { + return m12; + } + + @Override // from AbstractMatrix3 + public double m22 () { + return m22; + } +} diff --git a/src/main/java/pythagoras/d/Vector3.java b/src/main/java/pythagoras/d/Vector3.java new file mode 100644 index 0000000..6b2b407 --- /dev/null +++ b/src/main/java/pythagoras/d/Vector3.java @@ -0,0 +1,206 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.d; + +import java.io.Serializable; + +/** + * A three element vector. + */ +public class Vector3 extends AbstractVector3 implements Serializable +{ + /** A unit vector in the X+ direction. */ + public static final IVector3 UNIT_X = new Vector3(1f, 0f, 0f); + + /** A unit vector in the Y+ direction. */ + public static final IVector3 UNIT_Y = new Vector3(0f, 1f, 0f); + + /** A unit vector in the Z+ direction. */ + public static final IVector3 UNIT_Z = new Vector3(0f, 0f, 1f); + + /** A vector containing unity for all components. */ + public static final IVector3 UNIT_XYZ = new Vector3(1f, 1f, 1f); + + /** A normalized version of UNIT_XYZ. */ + public static final IVector3 NORMAL_XYZ = UNIT_XYZ.normalize(); + + /** The zero vector. */ + public static final IVector3 ZERO = new Vector3(0f, 0f, 0f); + + /** A vector containing the minimum doubleing point value for all components + * (note: the components are -{@link Float#MAX_VALUE}, not {@link Float#MIN_VALUE}). */ + public static final IVector3 MIN_VALUE = + new Vector3(-Float.MAX_VALUE, -Float.MAX_VALUE, -Float.MAX_VALUE); + + /** A vector containing the maximum doubleing point value for all components. */ + public static final IVector3 MAX_VALUE = + new Vector3(Float.MAX_VALUE, Float.MAX_VALUE, Float.MAX_VALUE); + + /** The components of the vector. */ + public double x, y, z; + + /** + * Creates a vector from three components. + */ + public Vector3 (double x, double y, double z) { + set(x, y, z); + } + + /** + * Creates a vector from an array of values. + */ + public Vector3 (double[] values) { + set(values); + } + + /** + * Copy constructor. + */ + public Vector3 (IVector3 other) { + set(other); + } + + /** + * Creates a zero vector. + */ + public Vector3 () { + } + + /** + * Computes the cross product of this and the specified other vector, storing the result + * in this vector. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 crossLocal (IVector3 other) { + return cross(other, this); + } + + /** + * Negates this vector in-place. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 negateLocal () { + return negate(this); + } + + /** + * Normalizes this vector in-place. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 normalizeLocal () { + return normalize(this); + } + + /** + * Multiplies this vector in-place by a scalar. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 multLocal (double v) { + return mult(v, this); + } + + /** + * Multiplies this vector in-place by another. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 multLocal (IVector3 other) { + return mult(other, this); + } + + /** + * Adds a vector in-place to this one. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 addLocal (IVector3 other) { + return add(other, this); + } + + /** + * Subtracts a vector in-place from this one. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 subtractLocal (IVector3 other) { + return subtract(other, this); + } + + /** + * Adds a vector in-place to this one. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 addLocal (double x, double y, double z) { + return add(x, y, z, this); + } + + /** + * Adds a scaled vector in-place to this one. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 addScaledLocal (IVector3 other, double v) { + return addScaled(other, v, this); + } + + /** + * Linearly interpolates between this and the specified other vector in-place by the supplied + * amount. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 lerpLocal (IVector3 other, double t) { + return lerp(other, t, this); + } + /** + * Copies the elements of another vector. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 set (IVector3 other) { + return set(other.x(), other.y(), other.z()); + } + + /** + * Copies the elements of an array. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 set (double[] values) { + return set(values[0], values[1], values[2]); + } + + /** + * Sets all of the elements of the vector. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 set (double x, double y, double z) { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + @Override // from AbstractVector3 + public double x () { + return x; + } + + @Override // from AbstractVector3 + public double y () { + return y; + } + + @Override // from AbstractVector3 + public double z () { + return z; + } +} diff --git a/src/main/java/pythagoras/f/AbstractMatrix3.java b/src/main/java/pythagoras/f/AbstractMatrix3.java new file mode 100644 index 0000000..08e658e --- /dev/null +++ b/src/main/java/pythagoras/f/AbstractMatrix3.java @@ -0,0 +1,346 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.f; + +import java.nio.FloatBuffer; + +import pythagoras.util.Platform; +import pythagoras.util.SingularMatrixException; + +/** + * Provides most of the implementation of {@link IMatrix3}, obtaining only the components from the + * derived class. + */ +public abstract class AbstractMatrix3 implements IMatrix3 +{ + @Override // from IVector3 + public Matrix3 transpose () { + return transpose(new Matrix3()); + } + + @Override // from IVector3 + public Matrix3 transpose (Matrix3 result) { + return result.set( + m00(), m01(), m02(), + m10(), m11(), m12(), + m20(), m21(), m22()); + } + + @Override // from IVector3 + public Matrix3 mult (IMatrix3 other) { + return mult(other, new Matrix3()); + } + + @Override // from IVector3 + public Matrix3 mult (IMatrix3 other, Matrix3 result) { + float m00 = m00(), m01 = m01(), m02 = m02(); + float m10 = m10(), m11 = m11(), m12 = m12(); + float m20 = m20(), m21 = m21(), m22 = m22(); + float om00 = other.m00(), om01 = other.m01(), om02 = other.m02(); + float om10 = other.m10(), om11 = other.m11(), om12 = other.m12(); + float om20 = other.m20(), om21 = other.m21(), om22 = other.m22(); + return result.set( + m00*om00 + m10*om01 + m20*om02, + m00*om10 + m10*om11 + m20*om12, + m00*om20 + m10*om21 + m20*om22, + + m01*om00 + m11*om01 + m21*om02, + m01*om10 + m11*om11 + m21*om12, + m01*om20 + m11*om21 + m21*om22, + + m02*om00 + m12*om01 + m22*om02, + m02*om10 + m12*om11 + m22*om12, + m02*om20 + m12*om21 + m22*om22); + } + + @Override // from IVector3 + public boolean isAffine () { + return (m02() == 0f && m12() == 0f && m22() == 1f); + } + + @Override // from IVector3 + public Matrix3 multAffine (IMatrix3 other) { + return multAffine(other, new Matrix3()); + } + + @Override // from IVector3 + public Matrix3 multAffine (IMatrix3 other, Matrix3 result) { + float m00 = m00(), m01 = m01(), m02 = m02(); + float m10 = m10(), m11 = m11(), m12 = m12(); + float m20 = m20(), m21 = m21(), m22 = m22(); + float om00 = other.m00(), om01 = other.m01(), om02 = other.m02(); + float om10 = other.m10(), om11 = other.m11(), om12 = other.m12(); + float om20 = other.m20(), om21 = other.m21(), om22 = other.m22(); + return result.set( + m00*om00 + m10*om01, + m00*om10 + m10*om11, + m00*om20 + m10*om21 + m20, + + m01*om00 + m11*om01, + m01*om10 + m11*om11, + m01*om20 + m11*om21 + m21, + + 0f, 0f, 1f); + } + + @Override // from IVector3 + public Matrix3 invert () { + return invert(new Matrix3()); + } + + /** + * Inverts this matrix and places the result in the given object. This code is based on the + * examples in the Matrix and + * Quaternion FAQ. + * + * @return a reference to the result matrix, for chaining. + */ + public Matrix3 invert (Matrix3 result) throws SingularMatrixException { + float m00 = m00(), m01 = m01(), m02 = m02(); + float m10 = m10(), m11 = m11(), m12 = m12(); + float m20 = m20(), m21 = m21(), m22 = m22(); + // compute the determinant, storing the subdeterminants for later use + float sd00 = m11*m22 - m21*m12; + float sd10 = m01*m22 - m21*m02; + float sd20 = m01*m12 - m11*m02; + float det = m00*sd00 + m20*sd20 - m10*sd10; + if (Math.abs(det) == 0f) { + // determinant is zero; matrix is not invertible + throw new SingularMatrixException(this.toString()); + } + float rdet = 1f / det; + return result.set( + +sd00 * rdet, + -(m10*m22 - m20*m12) * rdet, + +(m10*m21 - m20*m11) * rdet, + + -sd10 * rdet, + +(m00*m22 - m20*m02) * rdet, + -(m00*m21 - m20*m01) * rdet, + + +sd20 * rdet, + -(m00*m12 - m10*m02) * rdet, + +(m00*m11 - m10*m01) * rdet); + } + + @Override // from IVector3 + public Matrix3 invertAffine () { + return invertAffine(new Matrix3()); + } + + @Override // from IVector3 + public Matrix3 invertAffine (Matrix3 result) throws SingularMatrixException { + float m00 = m00(), m01 = m01(), m02 = m02(); + float m10 = m10(), m11 = m11(), m12 = m12(); + float m20 = m20(), m21 = m21(), m22 = m22(); + // compute the determinant, storing the subdeterminants for later use + float det = m00*m11 - m10*m01; + if (Math.abs(det) == 0f) { + // determinant is zero; matrix is not invertible + throw new SingularMatrixException(this.toString()); + } + float rdet = 1f / det; + return result.set( + +m11 * rdet, + -m10 * rdet, + +(m10*m21 - m20*m11) * rdet, + + -m01 * rdet, + +m00 * rdet, + -(m00*m21 - m20*m01) * rdet, + + 0f, 0f, 1f); + } + + @Override // from IVector3 + public Matrix3 lerp (IMatrix3 other, float t) { + return lerp(other, t, new Matrix3()); + } + + @Override // from IVector3 + public Matrix3 lerp (IMatrix3 other, float t, Matrix3 result) { + float m00 = m00(), m01 = m01(), m02 = m02(); + float m10 = m10(), m11 = m11(), m12 = m12(); + float m20 = m20(), m21 = m21(), m22 = m22(); + float om00 = other.m00(), om01 = other.m01(), om02 = other.m02(); + float om10 = other.m10(), om11 = other.m11(), om12 = other.m12(); + float om20 = other.m20(), om21 = other.m21(), om22 = other.m22(); + return result.set( + m00 + t*(om00 - m00), + m10 + t*(om10 - m10), + m20 + t*(om20 - m20), + + m01 + t*(om01 - m01), + m11 + t*(om11 - m11), + m21 + t*(om21 - m21), + + m02 + t*(om02 - m02), + m12 + t*(om12 - m12), + m22 + t*(om22 - m22)); + } + + @Override // from IVector3 + public Matrix3 lerpAffine (IMatrix3 other, float t) { + return lerpAffine(other, t, new Matrix3()); + } + + @Override // from IVector3 + public Matrix3 lerpAffine (IMatrix3 other, float t, Matrix3 result) { + float m00 = m00(), m01 = m01(), m02 = m02(); + float m10 = m10(), m11 = m11(), m12 = m12(); + float m20 = m20(), m21 = m21(), m22 = m22(); + float om00 = other.m00(), om01 = other.m01(), om02 = other.m02(); + float om10 = other.m10(), om11 = other.m11(), om12 = other.m12(); + float om20 = other.m20(), om21 = other.m21(), om22 = other.m22(); + return result.set( + m00 + t*(om00 - m00), + m10 + t*(om10 - m10), + m20 + t*(om20 - m20), + + m01 + t*(om01 - m01), + m11 + t*(om11 - m11), + m21 + t*(om21 - m21), + + 0f, 0f, 1f); + } + + @Override // from IVector3 + public FloatBuffer get (FloatBuffer buf) { + buf.put(m00()).put(m01()).put(m02()); + buf.put(m10()).put(m11()).put(m12()); + buf.put(m20()).put(m21()).put(m22()); + return buf; + } + + @Override // from IVector3 + public Vector3 transformLocal (Vector3 vector) { + return transform(vector, vector); + } + + @Override // from IVector3 + public Vector3 transform (IVector3 vector) { + return transform(vector, new Vector3()); + } + + @Override // from IVector3 + public Vector3 transform (IVector3 vector, Vector3 result) { + float vx = vector.x(), vy = vector.y(), vz = vector.z(); + return result.set( + m00()*vx + m10()*vy + m20()*vz, + m01()*vx + m11()*vy + m21()*vz, + m02()*vx + m12()*vy + m22()*vz); + } + + @Override // from IVector3 + public Vector transformPointLocal (Vector point) { + return transformPoint(point, point); + } + + @Override // from IVector3 + public Vector transformPoint (IVector point) { + return transformPoint(point, new Vector()); + } + + @Override // from IVector3 + public Vector transformPoint (IVector point, Vector result) { + float px = point.x(), py = point.y(); + return result.set(m00()*px + m10()*py + m20(), m01()*px + m11()*py + m21()); + } + + @Override // from IVector3 + public Vector transformVectorLocal (Vector vector) { + return transformVector(vector, vector); + } + + @Override // from IVector3 + public Vector transformVector (IVector vector) { + return transformVector(vector, new Vector()); + } + + @Override // from IVector3 + public Vector transformVector (IVector vector, Vector result) { + float vx = vector.x(), vy = vector.y(); + return result.set(m00()*vx + m10()*vy, m01()*vx + m11()*vy); + } + + @Override // from IVector3 + public float extractRotation () { + // start with the contents of the upper 2x2 portion of the matrix + float n00 = m00(), n10 = m10(); + float n01 = m01(), n11 = m11(); + for (int ii = 0; ii < 10; ii++) { + // store the results of the previous iteration + float o00 = n00, o10 = n10; + float o01 = n01, o11 = n11; + + // compute average of the matrix with its inverse transpose + float det = o00*o11 - o10*o01; + if (Math.abs(det) == 0f) { + // determinant is zero; matrix is not invertible + throw new SingularMatrixException(this.toString()); + } + float hrdet = 0.5f / det; + n00 = +o11 * hrdet + o00*0.5f; + n10 = -o01 * hrdet + o10*0.5f; + + n01 = -o10 * hrdet + o01*0.5f; + n11 = +o00 * hrdet + o11*0.5f; + + // compute the difference; if it's small enough, we're done + float d00 = n00 - o00, d10 = n10 - o10; + float d01 = n01 - o01, d11 = n11 - o11; + if (d00*d00 + d10*d10 + d01*d01 + d11*d11 < MathUtil.EPSILON) { + break; + } + } + // now that we have a nice orthogonal matrix, we can extract the rotation + return FloatMath.atan2(n01, n00); + } + + @Override // from IVector3 + public Vector extractScale () { + return extractScale(new Vector()); + } + + @Override // from IVector3 + public Vector extractScale (Vector result) { + float m00 = m00(), m01 = m01(), m10 = m10(), m11 = m11(); + return result.set( + FloatMath.sqrt(m00*m00 + m01*m01), + FloatMath.sqrt(m10*m10 + m11*m11)); + } + + @Override // from IVector3 + public float approximateUniformScale () { + float cp = m00()*m11() - m01()*m10(); + return (cp < 0f) ? -FloatMath.sqrt(-cp) : FloatMath.sqrt(cp); + } + + @Override + public String toString () { + return "[[" + m00() + ", " + m10() + ", " + m20() + "], " + + "[" + m01() + ", " + m11() + ", " + m21() + "], " + + "[" + m02() + ", " + m12() + ", " + m22() + "]]"; + } + + @Override + public int hashCode () { + return Platform.hashCode(m00()) ^ Platform.hashCode(m10()) ^ Platform.hashCode(m20()) ^ + Platform.hashCode(m01()) ^ Platform.hashCode(m11()) ^ Platform.hashCode(m21()) ^ + Platform.hashCode(m02()) ^ Platform.hashCode(m12()) ^ Platform.hashCode(m22()); + } + + @Override + public boolean equals (Object other) { + if (!(other instanceof AbstractMatrix3)) { + return false; + } + AbstractMatrix3 omat = (AbstractMatrix3)other; + return + m00() == omat.m00() && m10() == omat.m10() && m20() == omat.m20() && + m01() == omat.m01() && m11() == omat.m11() && m21() == omat.m21() && + m02() == omat.m02() && m12() == omat.m12() && m22() == omat.m22(); + } +} diff --git a/src/main/java/pythagoras/f/AbstractVector3.java b/src/main/java/pythagoras/f/AbstractVector3.java new file mode 100644 index 0000000..895601e --- /dev/null +++ b/src/main/java/pythagoras/f/AbstractVector3.java @@ -0,0 +1,204 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.f; + +import java.nio.FloatBuffer; + +import pythagoras.util.Platform; + +/** + * Provides most of the implementation of {@link IVector3}, obtaining only x, y and z from the + * derived class. + */ +public abstract class AbstractVector3 implements IVector3 +{ + @Override // from interface IVector3 + public float dot (IVector3 other) { + return x()*other.x() + y()*other.y() + z()*other.z(); + } + + @Override // from interface IVector3 + public Vector3 cross (IVector3 other) { + return cross(other, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 cross (IVector3 other, Vector3 result) { + float x = x(), y = y(), z = z(); + float ox = other.x(), oy = other.y(), oz = other.z(); + return result.set(y*oz - z*oy, z*ox - x*oz, x*oy - y*ox); + } + + @Override // from interface IVector3 + public float triple (IVector3 b, IVector3 c) { + float bx = b.x(), by = b.y(), bz = b.z(); + float cx = c.x(), cy = c.y(), cz = c.z(); + return x()*(by*cz - bz*cy) + y()*(bz*cx - bx*cz) + z()*(bx*cy - by*cx); + } + + @Override // from interface IVector3 + public Vector3 negate () { + return negate(new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 negate (Vector3 result) { + return result.set(-x(), -y(), -z()); + } + + @Override // from interface IVector3 + public Vector3 normalize () { + return normalize(new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 normalize (Vector3 result) { + return mult(1f / length(), result); + } + + @Override // from interface IVector3 + public float angle (IVector3 other) { + return FloatMath.acos(dot(other) / (length() * other.length())); + } + + @Override // from interface IVector3 + public float length () { + return FloatMath.sqrt(lengthSquared()); + } + + @Override // from interface IVector3 + public float lengthSquared () { + float x = x(), y = y(), z = z(); + return (x*x + y*y + z*z); + } + + @Override // from interface IVector3 + public float distance (IVector3 other) { + return FloatMath.sqrt(distanceSquared(other)); + } + + @Override // from interface IVector3 + public float distanceSquared (IVector3 other) { + float dx = x() - other.x(), dy = y() - other.y(), dz = z() - other.z(); + return dx*dx + dy*dy + dz*dz; + } + + @Override // from interface IVector3 + public float manhattanDistance (IVector3 other) { + return Math.abs(x() - other.x()) + Math.abs(y() - other.y()) + Math.abs(z() - other.z()); + } + + @Override // from interface IVector3 + public Vector3 mult (float v) { + return mult(v, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 mult (float v, Vector3 result) { + return result.set(x()*v, y()*v, z()*v); + } + + @Override // from interface IVector3 + public Vector3 mult (IVector3 other) { + return mult(other, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 mult (IVector3 other, Vector3 result) { + return result.set(x()*other.x(), y()*other.y(), z()*other.z()); + } + + @Override // from interface IVector3 + public Vector3 add (IVector3 other) { + return add(other, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 add (IVector3 other, Vector3 result) { + return add(other.x(), other.y(), other.z(), result); + } + + @Override // from interface IVector3 + public Vector3 subtract (IVector3 other) { + return subtract(other, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 subtract (IVector3 other, Vector3 result) { + return add(-other.x(), -other.y(), -other.z(), result); + } + + @Override // from interface IVector3 + public Vector3 add (float x, float y, float z) { + return add(x, y, z, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 add (float x, float y, float z, Vector3 result) { + return result.set(x() + x, y() + y, z() + z); + } + + @Override // from interface IVector3 + public Vector3 addScaled (IVector3 other, float v) { + return addScaled(other, v, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 addScaled (IVector3 other, float v, Vector3 result) { + return result.set(x() + other.x()*v, y() + other.y()*v, z() + other.z()*v); + } + + @Override // from interface IVector3 + public Vector3 lerp (IVector3 other, float t) { + return lerp(other, t, new Vector3()); + } + + @Override // from interface IVector3 + public Vector3 lerp (IVector3 other, float t, Vector3 result) { + float x = x(), y = y(), z = z(); + return result.set(x + t*(other.x() - x), y + t*(other.y() - y), z + t*(other.z() - z)); + } + + @Override // from interface IVector3 + public float get (int idx) { + switch (idx) { + case 0: return x(); + case 1: return y(); + case 2: return z(); + } + throw new IndexOutOfBoundsException(String.valueOf(idx)); + } + + @Override // from interface IVector3 + public void get (float[] values) { + values[0] = x(); + values[1] = y(); + values[2] = z(); + } + + @Override // from interface IVector3 + public FloatBuffer get (FloatBuffer buf) { + return buf.put(x()).put(y()).put(z()); + } + + @Override + public String toString () { + return "[" + x() + ", " + y() + ", " + z() + "]"; + } + + @Override + public int hashCode () { + return Platform.hashCode(x()) ^ Platform.hashCode(y()) ^ Platform.hashCode(z()); + } + + @Override + public boolean equals (Object other) { + if (!(other instanceof AbstractVector3)) { + return false; + } + AbstractVector3 ovec = (AbstractVector3)other; + return (x() == ovec.x() && y() == ovec.y() && z() == ovec.z()); + } +} diff --git a/src/main/java/pythagoras/f/IMatrix3.java b/src/main/java/pythagoras/f/IMatrix3.java new file mode 100644 index 0000000..2ce605d --- /dev/null +++ b/src/main/java/pythagoras/f/IMatrix3.java @@ -0,0 +1,250 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.f; + +import java.nio.FloatBuffer; + +import pythagoras.util.SingularMatrixException; + +/** + * Provides read-only access to a {@link Matrix3}. + */ +interface IMatrix3 +{ + /** Returns the (0,0)th component of the matrix. */ + float m00 (); + + /** Returns the (1,0)th component of the matrix. */ + float m10 (); + + /** Returns the (2,0)th component of the matrix. */ + float m20 (); + + /** Returns the (0,1)th component of the matrix. */ + float m01 (); + + /** Returns the (1,1)th component of the matrix. */ + float m11 (); + + /** Returns the (2,1)th component of the matrix. */ + float m21 (); + + /** Returns the (0,2)th component of the matrix. */ + float m02 (); + + /** Returns the (1,2)th component of the matrix. */ + float m12 (); + + /** Returns the (2,2)th component of the matrix. */ + float m22 (); + + /** + * Transposes this matrix. + * + * @return a new matrix containing the result. + */ + Matrix3 transpose (); + + /** + * Transposes this matrix, storing the result in the provided object. + * + * @return the result matrix, for chaining. + */ + Matrix3 transpose (Matrix3 result); + + /** + * Multiplies this matrix by another. + * + * @return a new matrix containing the result. + */ + Matrix3 mult (IMatrix3 other); + + /** + * Multiplies this matrix by another and stores the result in the object provided. + * + * @return a reference to the result matrix, for chaining. + */ + Matrix3 mult (IMatrix3 other, Matrix3 result); + + /** + * Determines whether this matrix represents an affine transformation. + */ + boolean isAffine (); + + /** + * Multiplies this matrix by another, treating the matrices as affine. + * + * @return a new matrix containing the result. + */ + Matrix3 multAffine (IMatrix3 other); + + /** + * Multiplies this matrix by another, treating the matrices as affine, and stores the result + * in the object provided. + * + * @return a reference to the result matrix, for chaining. + */ + Matrix3 multAffine (IMatrix3 other, Matrix3 result); + + /** + * Inverts this matrix. + * + * @return a new matrix containing the result. + */ + Matrix3 invert (); + + /** + * Inverts this matrix and places the result in the given object. This code is based on the + * examples in the Matrix and + * Quaternion FAQ. + * + * @return a reference to the result matrix, for chaining. + */ + Matrix3 invert (Matrix3 result) throws SingularMatrixException; + + /** + * Inverts this matrix as an affine matrix. + * + * @return a new matrix containing the result. + */ + Matrix3 invertAffine (); + + /** + * Inverts this matrix as an affine matrix and places the result in the given object. + * + * @return a reference to the result matrix, for chaining. + */ + Matrix3 invertAffine (Matrix3 result) throws SingularMatrixException; + + /** + * Linearly interpolates between this and the specified other matrix. + * + * @return a new matrix containing the result. + */ + Matrix3 lerp (IMatrix3 other, float t); + + /** + * Linearly interpolates between this and the specified other matrix, placing the result in + * the object provided. + * + * @return a reference to the result object, for chaining. + */ + Matrix3 lerp (IMatrix3 other, float t, Matrix3 result); + + /** + * Linearly interpolates between this and the specified other matrix, treating the matrices as + * affine. + * + * @return a new matrix containing the result. + */ + Matrix3 lerpAffine (IMatrix3 other, float t); + + /** + * Linearly interpolates between this and the specified other matrix (treating the matrices as + * affine), placing the result in the object provided. + * + * @return a reference to the result object, for chaining. + */ + Matrix3 lerpAffine (IMatrix3 other, float t, Matrix3 result); + + /** + * Places the contents of this matrix into the given buffer in the standard OpenGL order. + * + * @return a reference to the buffer, for chaining. + */ + FloatBuffer get (FloatBuffer buf); + + /** + * Transforms a vector in-place by the inner 3x3 part of this matrix. + * + * @return a reference to the vector, for chaining. + */ + Vector3 transformLocal (Vector3 vector); + + /** + * Transforms a vector by this matrix. + * + * @return a new vector containing the result. + */ + Vector3 transform (IVector3 vector); + + /** + * Transforms a vector by this matrix and places the result in the object provided. + * + * @return a reference to the result, for chaining. + */ + Vector3 transform (IVector3 vector, Vector3 result); + + /** + * Transforms a point in-place by this matrix. + * + * @return a reference to the point, for chaining. + */ + Vector transformPointLocal (Vector point); + + /** + * Transforms a point by this matrix. + * + * @return a new vector containing the result. + */ + Vector transformPoint (IVector point); + + /** + * Transforms a point by this matrix and places the result in the object provided. + * + * @return a reference to the result, for chaining. + */ + Vector transformPoint (IVector point, Vector result); + + /** + * Transforms a vector in-place by the inner 2x2 part of this matrix. + * + * @return a reference to the vector, for chaining. + */ + Vector transformVectorLocal (Vector vector); + + /** + * Transforms a vector by this inner 2x2 part of this matrix. + * + * @return a new vector containing the result. + */ + Vector transformVector (IVector vector); + + /** + * Transforms a vector by the inner 2x2 part of this matrix and places the result in the object + * provided. + * + * @return a reference to the result, for chaining. + */ + Vector transformVector (IVector vector, Vector result); + + /** + * Extracts the rotation component of the matrix. This uses the iterative polar decomposition + * algorithm described by + * Ken + * Shoemake. + */ + float extractRotation (); + + /** + * Extracts the scale component of the matrix. + * + * @return a new vector containing the result. + */ + Vector extractScale (); + + /** + * Extracts the scale component of the matrix and places it in the provided result vector. + * + * @return a reference to the result vector, for chaining. + */ + Vector extractScale (Vector result); + + /** + * Returns an approximation of the uniform scale for this matrix (the square root of the + * signed area of the parallelogram spanned by the axis vectors). + */ + float approximateUniformScale (); +} diff --git a/src/main/java/pythagoras/f/IVector3.java b/src/main/java/pythagoras/f/IVector3.java new file mode 100644 index 0000000..7fa2270 --- /dev/null +++ b/src/main/java/pythagoras/f/IVector3.java @@ -0,0 +1,221 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.f; + +import java.nio.FloatBuffer; + +/** + * Provides read-only access to a {@link Vector3}. + */ +public interface IVector3 +{ + /** Returns the x-component of this vector. */ + float x (); + + /** Returns the y-component of this vector. */ + float y (); + + /** Returns the z-component of this vector. */ + float z (); + + /** + * Computes and returns the dot product of this and the specified other vector. + */ + float dot (IVector3 other); + + /** + * Computes the cross product of this and the specified other vector. + * + * @return a new vector containing the result. + */ + Vector3 cross (IVector3 other); + + /** + * Computes the cross product of this and the specified other vector, placing the result + * in the object supplied. + * + * @return a reference to the result, for chaining. + */ + Vector3 cross (IVector3 other, Vector3 result); + + /** + * Computes the triple product of this and the specified other vectors, which is equal to + * this.dot(b.cross(c)). + */ + float triple (IVector3 b, IVector3 c); + + /** + * Negates this vector. + * + * @return a new vector containing the result. + */ + Vector3 negate (); + + /** + * Negates this vector, storing the result in the supplied object. + * + * @return a reference to the result, for chaining. + */ + Vector3 negate (Vector3 result); + + /** + * Normalizes this vector. + * + * @return a new vector containing the result. + */ + Vector3 normalize (); + + /** + * Normalizes this vector, storing the result in the object supplied. + * + * @return a reference to the result, for chaining. + */ + Vector3 normalize (Vector3 result); + + /** + * Returns the angle between this vector and the specified other vector. + */ + float angle (IVector3 other); + + /** + * Returns the length of this vector. + */ + float length (); + + /** + * Returns the squared length of this vector. + */ + float lengthSquared (); + + /** + * Returns the distance from this vector to the specified other vector. + */ + float distance (IVector3 other); + + /** + * Returns the squared distance from this vector to the specified other. + */ + float distanceSquared (IVector3 other); + + /** + * Returns the Manhattan distance between this vector and the specified other. + */ + float manhattanDistance (IVector3 other); + + /** + * Multiplies this vector by a scalar. + * + * @return a new vector containing the result. + */ + Vector3 mult (float v); + + /** + * Multiplies this vector by a scalar and places the result in the supplied object. + * + * @return a reference to the result, for chaining. + */ + Vector3 mult (float v, Vector3 result); + + /** + * Multiplies this vector by another. + * + * @return a new vector containing the result. + */ + Vector3 mult (IVector3 other); + + /** + * Multiplies this vector by another, storing the result in the object provided. + * + * @return a reference to the result vector, for chaining. + */ + Vector3 mult (IVector3 other, Vector3 result); + + /** + * Adds a vector to this one. + * + * @return a new vector containing the result. + */ + Vector3 add (IVector3 other); + /** + * Adds a vector to this one, storing the result in the object provided. + * + * @return a reference to the result, for chaining. + */ + public IVector3 add (IVector3 other, Vector3 result); + + /** + * Subtracts a vector from this one. + * + * @return a new vector containing the result. + */ + Vector3 subtract (IVector3 other); + + /** + * Subtracts a vector from this one and places the result in the supplied object. + * + * @return a reference to the result, for chaining. + */ + Vector3 subtract (IVector3 other, Vector3 result); + + /** + * Adds a vector to this one. + * + * @return a new vector containing the result. + */ + Vector3 add (float x, float y, float z); + + /** + * Adds a vector to this one and stores the result in the object provided. + * + * @return a reference to the result, for chaining. + */ + Vector3 add (float x, float y, float z, Vector3 result); + + /** + * Adds a scaled vector to this one. + * + * @return a new vector containing the result. + */ + Vector3 addScaled (IVector3 other, float v); + + /** + * Adds a scaled vector to this one and stores the result in the supplied vector. + * + * @return a reference to the result, for chaining. + */ + Vector3 addScaled (IVector3 other, float v, Vector3 result); + + /** + * Linearly interpolates between this and the specified other vector by the supplied amount. + * + * @return a new vector containing the result. + */ + Vector3 lerp (IVector3 other, float t); + + /** + * Linearly interpolates between this and the supplied other vector by the supplied amount, + * storing the result in the supplied object. + * + * @return a reference to the result, for chaining. + */ + Vector3 lerp (IVector3 other, float t, Vector3 result); + + /** + * Returns the element at the idx'th position of the vector. + */ + float get (int idx); + + /** + * Populates the supplied array with the contents of this vector. + */ + void get (float[] values); + + /** + * Populates the supplied buffer with the contents of this vector. + * + * @return a reference to the buffer, for chaining. + */ + FloatBuffer get (FloatBuffer buf); +} diff --git a/src/main/java/pythagoras/f/Matrix3.java b/src/main/java/pythagoras/f/Matrix3.java new file mode 100644 index 0000000..ae0cfef --- /dev/null +++ b/src/main/java/pythagoras/f/Matrix3.java @@ -0,0 +1,406 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.f; + +import java.nio.FloatBuffer; + +/** + * A 3x3 column-major matrix. + */ +public class Matrix3 extends AbstractMatrix3 +{ + /** The identity matrix. */ + public static final Matrix3 IDENTITY = new Matrix3(); + + /** The values of the matrix. */ + public float m00, m10, m20; + public float m01, m11, m21; + public float m02, m12, m22; + + /** + * Creates a matrix from its components. + */ + public Matrix3 (float m00, float m10, float m20, + float m01, float m11, float m21, + float m02, float m12, float m22) { + set(m00, m10, m20, + m01, m11, m21, + m02, m12, m22); + } + + /** + * Creates a matrix from an array of values. + */ + public Matrix3 (float[] values) { + set(values); + } + + /** + * Copy constructor. + */ + public Matrix3 (Matrix3 other) { + set(other); + } + + /** + * Creates an identity matrix. + */ + public Matrix3 () { + setToIdentity(); + } + + /** + * Sets this matrix to the identity matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToIdentity () { + return set( + 1f, 0f, 0f, + 0f, 1f, 0f, + 0f, 0f, 1f); + } + + /** + * Sets this to a rotation matrix that rotates one vector onto another. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToRotation (IVector3 from, IVector3 to) { + float angle = from.angle(to); + return (angle < 0.0001f) ? + setToIdentity() : setToRotation(angle, from.cross(to).normalizeLocal()); + } + + /** + * Sets this to a rotation matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToRotation (float angle, IVector3 axis) { + return setToRotation(angle, axis.x(), axis.y(), axis.z()); + } + + /** + * Sets this to a rotation matrix. The formula comes from the OpenGL documentation for the + * glRotatef function. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToRotation (float angle, float x, float y, float z) { + float c = FloatMath.cos(angle), s = FloatMath.sin(angle), omc = 1f - c; + float xs = x*s, ys = y*s, zs = z*s, xy = x*y, xz = x*z, yz = y*z; + return set( + x*x*omc + c, xy*omc - zs, xz*omc + ys, + xy*omc + zs, y*y*omc + c, yz*omc - xs, + xz*omc - ys, yz*omc + xs, z*z*omc + c); + } + + // /** + // * Sets this to a rotation matrix. The formula comes from the + // * Matrix and Quaternion FAQ. + // * + // * @return a reference to this matrix, for chaining. + // */ + // public Matrix3 setToRotation (Quaternion quat) { + // float xx = quat.x*quat.x, yy = quat.y*quat.y, zz = quat.z*quat.z; + // float xy = quat.x*quat.y, xz = quat.x*quat.z, xw = quat.x*quat.w; + // float yz = quat.y*quat.z, yw = quat.y*quat.w, zw = quat.z*quat.w; + // return set( + // 1f - 2f*(yy + zz), 2f*(xy - zw), 2f*(xz + yw), + // 2f*(xy + zw), 1f - 2f*(xx + zz), 2f*(yz - xw), + // 2f*(xz - yw), 2f*(yz + xw), 1f - 2f*(xx + yy)); + // } + + /** + * Sets this to a scale matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToScale (IVector3 scale) { + return setToScale(scale.x(), scale.y(), scale.z()); + } + + /** + * Sets this to a uniform scale matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToScale (float s) { + return setToScale(s, s, s); + } + + /** + * Sets this to a scale matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToScale (float x, float y, float z) { + return set( + x, 0f, 0f, + 0f, y, 0f, + 0f, 0f, z); + } + + /** + * Sets this to a reflection across a plane intersecting the origin with the supplied normal. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToReflection (IVector3 normal) { + return setToReflection(normal.x(), normal.y(), normal.z()); + } + + /** + * Sets this to a reflection across a plane intersecting the origin with the supplied normal. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToReflection (float x, float y, float z) { + float x2 = -2f*x, y2 = -2f*y, z2 = -2f*z; + float xy2 = x2*y, xz2 = x2*z, yz2 = y2*z; + return set( + 1f + x2*x, xy2, xz2, + xy2, 1f + y2*y, yz2, + xz2, yz2, 1f + z2*z); + } + + /** + * Sets this to a matrix that first rotates, then translates. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToTransform (IVector translation, float rotation) { + return setToRotation(rotation).setTranslation(translation); + } + + /** + * Sets this to a matrix that first scales, then rotates, then translates. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToTransform (IVector translation, float rotation, float scale) { + return setToRotation(rotation).set( + m00 * scale, m10 * scale, translation.x(), + m01 * scale, m11 * scale, translation.y(), + 0f, 0f, 1f); + } + + /** + * Sets this to a matrix that first scales, then rotates, then translates. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToTransform (IVector translation, float rotation, IVector scale) { + float sx = scale.x(), sy = scale.y(); + return setToRotation(rotation).set( + m00 * sx, m10 * sy, translation.x(), + m01 * sx, m11 * sy, translation.y(), + 0f, 0f, 1f); + } + + /** + * Sets this to a translation matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToTranslation (IVector translation) { + return setToTranslation(translation.x(), translation.y()); + } + + /** + * Sets this to a translation matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToTranslation (float x, float y) { + return set( + 1f, 0f, x, + 0f, 1f, y, + 0f, 0f, 1f); + } + + /** + * Sets the translation component of this matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setTranslation (IVector translation) { + return setTranslation(translation.x(), translation.y()); + } + + /** + * Sets the translation component of this matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setTranslation (float x, float y) { + m20 = x; + m21 = y; + return this; + } + + /** + * Sets this to a rotation matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 setToRotation (float angle) { + float sina = FloatMath.sin(angle), cosa = FloatMath.cos(angle); + return set( + cosa, -sina, 0f, + sina, cosa, 0f, + 0f, 0f, 1f); + } + + /** + * Transposes this matrix in-place. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 transposeLocal () { + return transpose(this); + } + + /** + * Multiplies this matrix in-place by another. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 multLocal (IMatrix3 other) { + return mult(other, this); + } + + /** + * Multiplies this matrix in-place by another, treating the matricees as affine. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 multAffineLocal (IMatrix3 other) { + return multAffine(other, this); + } + + /** + * Inverts this matrix in-place. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 invertLocal () { + return invert(this); + } + + /** + * Inverts this matrix in-place as an affine matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 invertAffineLocal () { + return invertAffine(this); + } + + /** + * Linearly interpolates between the this and the specified other matrix, placing the result in + * this matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 lerpLocal (IMatrix3 other, float t) { + return lerp(other, t, this); + } + + /** + * Linearly interpolates between this and the specified other matrix (treating the matrices as + * affine), placing the result in this matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 lerpAffineLocal (IMatrix3 other, float t) { + return lerpAffine(other, t, this); + } + + /** + * Copies the contents of another matrix. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 set (IMatrix3 other) { + return set( + other.m00(), other.m10(), other.m20(), + other.m01(), other.m11(), other.m21(), + other.m02(), other.m12(), other.m22()); + } + + /** + * Copies the elements of an array. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 set (float[] values) { + return set( + values[0], values[1], values[2], + values[3], values[4], values[5], + values[6], values[7], values[8]); + } + + /** + * Sets all of the matrix's components at once. + * + * @return a reference to this matrix, for chaining. + */ + public Matrix3 set ( + float m00, float m10, float m20, + float m01, float m11, float m21, + float m02, float m12, float m22) { + this.m00 = m00; this.m01 = m01; this.m02 = m02; + this.m10 = m10; this.m11 = m11; this.m12 = m12; + this.m20 = m20; this.m21 = m21; this.m22 = m22; + return this; + } + + @Override // from AbstractMatrix3 + public float m00 () { + return m00; + } + + @Override // from AbstractMatrix3 + public float m10 () { + return m10; + } + + @Override // from AbstractMatrix3 + public float m20 () { + return m20; + } + + @Override // from AbstractMatrix3 + public float m01 () { + return m01; + } + + @Override // from AbstractMatrix3 + public float m11 () { + return m11; + } + + @Override // from AbstractMatrix3 + public float m21 () { + return m21; + } + + @Override // from AbstractMatrix3 + public float m02 () { + return m02; + } + + @Override // from AbstractMatrix3 + public float m12 () { + return m12; + } + + @Override // from AbstractMatrix3 + public float m22 () { + return m22; + } +} diff --git a/src/main/java/pythagoras/f/Vector3.java b/src/main/java/pythagoras/f/Vector3.java new file mode 100644 index 0000000..acb198a --- /dev/null +++ b/src/main/java/pythagoras/f/Vector3.java @@ -0,0 +1,206 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.f; + +import java.io.Serializable; + +/** + * A three element vector. + */ +public class Vector3 extends AbstractVector3 implements Serializable +{ + /** A unit vector in the X+ direction. */ + public static final IVector3 UNIT_X = new Vector3(1f, 0f, 0f); + + /** A unit vector in the Y+ direction. */ + public static final IVector3 UNIT_Y = new Vector3(0f, 1f, 0f); + + /** A unit vector in the Z+ direction. */ + public static final IVector3 UNIT_Z = new Vector3(0f, 0f, 1f); + + /** A vector containing unity for all components. */ + public static final IVector3 UNIT_XYZ = new Vector3(1f, 1f, 1f); + + /** A normalized version of UNIT_XYZ. */ + public static final IVector3 NORMAL_XYZ = UNIT_XYZ.normalize(); + + /** The zero vector. */ + public static final IVector3 ZERO = new Vector3(0f, 0f, 0f); + + /** A vector containing the minimum floating point value for all components + * (note: the components are -{@link Float#MAX_VALUE}, not {@link Float#MIN_VALUE}). */ + public static final IVector3 MIN_VALUE = + new Vector3(-Float.MAX_VALUE, -Float.MAX_VALUE, -Float.MAX_VALUE); + + /** A vector containing the maximum floating point value for all components. */ + public static final IVector3 MAX_VALUE = + new Vector3(Float.MAX_VALUE, Float.MAX_VALUE, Float.MAX_VALUE); + + /** The components of the vector. */ + public float x, y, z; + + /** + * Creates a vector from three components. + */ + public Vector3 (float x, float y, float z) { + set(x, y, z); + } + + /** + * Creates a vector from an array of values. + */ + public Vector3 (float[] values) { + set(values); + } + + /** + * Copy constructor. + */ + public Vector3 (IVector3 other) { + set(other); + } + + /** + * Creates a zero vector. + */ + public Vector3 () { + } + + /** + * Computes the cross product of this and the specified other vector, storing the result + * in this vector. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 crossLocal (IVector3 other) { + return cross(other, this); + } + + /** + * Negates this vector in-place. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 negateLocal () { + return negate(this); + } + + /** + * Normalizes this vector in-place. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 normalizeLocal () { + return normalize(this); + } + + /** + * Multiplies this vector in-place by a scalar. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 multLocal (float v) { + return mult(v, this); + } + + /** + * Multiplies this vector in-place by another. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 multLocal (IVector3 other) { + return mult(other, this); + } + + /** + * Adds a vector in-place to this one. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 addLocal (IVector3 other) { + return add(other, this); + } + + /** + * Subtracts a vector in-place from this one. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 subtractLocal (IVector3 other) { + return subtract(other, this); + } + + /** + * Adds a vector in-place to this one. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 addLocal (float x, float y, float z) { + return add(x, y, z, this); + } + + /** + * Adds a scaled vector in-place to this one. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 addScaledLocal (IVector3 other, float v) { + return addScaled(other, v, this); + } + + /** + * Linearly interpolates between this and the specified other vector in-place by the supplied + * amount. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 lerpLocal (IVector3 other, float t) { + return lerp(other, t, this); + } + /** + * Copies the elements of another vector. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 set (IVector3 other) { + return set(other.x(), other.y(), other.z()); + } + + /** + * Copies the elements of an array. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 set (float[] values) { + return set(values[0], values[1], values[2]); + } + + /** + * Sets all of the elements of the vector. + * + * @return a reference to this vector, for chaining. + */ + public Vector3 set (float x, float y, float z) { + this.x = x; + this.y = y; + this.z = z; + return this; + } + + @Override // from AbstractVector3 + public float x () { + return x; + } + + @Override // from AbstractVector3 + public float y () { + return y; + } + + @Override // from AbstractVector3 + public float z () { + return z; + } +} diff --git a/src/main/java/pythagoras/util/NoninvertibleTransformException.java b/src/main/java/pythagoras/util/NoninvertibleTransformException.java index d7e6a19..f9d6321 100644 --- a/src/main/java/pythagoras/util/NoninvertibleTransformException.java +++ b/src/main/java/pythagoras/util/NoninvertibleTransformException.java @@ -8,7 +8,7 @@ package pythagoras.util; * An exception thrown by {@code Transform} when a request for an inverse transform cannot be * satisfied. */ -public class NoninvertibleTransformException extends java.lang.RuntimeException +public class NoninvertibleTransformException extends RuntimeException { public NoninvertibleTransformException (String s) { super(s); diff --git a/src/main/java/pythagoras/util/SingularMatrixException.java b/src/main/java/pythagoras/util/SingularMatrixException.java new file mode 100644 index 0000000..0687b34 --- /dev/null +++ b/src/main/java/pythagoras/util/SingularMatrixException.java @@ -0,0 +1,24 @@ +// +// Pythagoras - a collection of geometry classes +// http://github.com/samskivert/pythagoras + +package pythagoras.util; + +/** + * Thrown when inversion is attempted on a singular (non-invertible) matrix. + */ +public class SingularMatrixException extends RuntimeException +{ + /** + * Creates a new exception. + */ + public SingularMatrixException () { + } + + /** + * Creates a new exception with the provided message. + */ + public SingularMatrixException (String message) { + super(message); + } +}