toxiclibs 0.2-java → 0.5.0-java

data/src/toxi/geom/GVector.java

@@ -0,0 +1,833 @@
+/*
+ *   __               .__       .__  ._____.           
+ * _/  |_  _______  __|__| ____ |  | |__\_ |__   ______
+ * \   __\/  _ \  \/  /  |/ ___\|  | |  || __ \ /  ___/
+ *  |  | (  <_> >    <|  \  \___|  |_|  || \_\ \\___ \ 
+ *  |__|  \____/__/\_ \__|\___  >____/__||___  /____  >
+ *                   \/       \/             \/     \/ 
+ *
+ * Copyright (c) 2006-2011 Karsten Schmidt
+ * 
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ * 
+ * http://creativecommons.org/licenses/LGPL/2.1/
+ * 
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ * 
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA
+ */
+package toxi.geom;
+
+import java.util.Arrays;
+import toxi.math.InterpolateStrategy;
+import toxi.math.MathUtils;
+
+/**
+ * A double precision, general, dynamically-resizable, one-dimensional vector
+ * class.
+ */
+public class GVector implements java.lang.Cloneable, java.io.Serializable {
+
+    private int length;
+
+    /**
+     *
+     */
+    public double[] values;
+
+    static final long serialVersionUID = 1L;
+
+    /**
+     * Constructs a new GVector of the specified length and initializes it by
+     * copying the specified number of elements from the specified array. The
+     * array must contain at least <code>length</code> elements (i.e.,
+     * <code>vector.length</code> >= <code>length</code>. The length of this new
+     * GVector is set to the specified length.
+     *
+     * @param vector The array from which the values will be copied.
+     * @param length The number of values copied from the array.
+     */
+    public GVector(double vector[], int length) {
+        this.length = length;
+        values = new double[length];
+        System.arraycopy(vector, 0, values, 0, length);
+    }
+
+    /**
+     * Constructs a new GVector from the specified array elements. The length of
+     * this GVector is set to the length of the specified array. The array
+     * elements are copied into this new GVector.
+     *
+     * @param vector the values for the new GVector.
+     */
+    public GVector(double[] vector) {
+        length = vector.length;
+        values = new double[vector.length];
+        System.arraycopy(vector, 0, values, 0, length);
+    }
+
+    /**
+     * Constructs a new GVector from the specified vector. The vector elements
+     * are copied into this new GVector.
+     *
+     * @param vector the source GVector for this new GVector.
+     */
+    public GVector(GVector vector) {
+        values = new double[vector.length];
+        length = vector.length;
+        System.arraycopy(vector.values, 0, values, 0, length);
+    }
+
+    /**
+     * Constructs a new GVector of the specified length with all vector elements
+     * initialized to 0.
+     *
+     * @param length the number of elements in this GVector.
+     */
+    public GVector(int length) {
+        this.length = length;
+        values = new double[length];
+        for (int i = 0; i < length; i++) {
+            values[i] = 0.0;
+        }
+    }
+
+    /**
+     * Constructs a new GVector and copies the initial values from the specified
+     * tuple.
+     *
+     * @param v the source for the new GVector's initial values
+     */
+    public GVector(ReadonlyVec2D v) {
+        values = new double[]{
+            v.x(), v.y()
+        };
+        length = 2;
+    }
+
+    /**
+     * Constructs a new GVector and copies the initial values from the specified
+     * tuple.
+     *
+     * @param v the source for the new GVector's initial values
+     */
+    public GVector(ReadonlyVec3D v) {
+        values = new double[]{
+            v.x(), v.y(), v.z()
+        };
+        length = 3;
+    }
+
+    /**
+     * Constructs a new GVector and copies the initial values from the specified
+     * tuple.
+     *
+     * @param v the source for the new GVector's initial values
+     */
+    public GVector(ReadonlyVec4D v) {
+        values = new double[]{
+            v.x(), v.y(), v.z(), v.w()
+        };
+        length = 4;
+    }
+
+    /**
+     * Creates the vector sum of this vector and the given one (must be equal
+     * sized). Returns result as new vector.
+     *
+     * @param v
+     * @return new vector
+     */
+    public final GVector add(GVector v) {
+        if (length != v.length) {
+            throw new MatrixSizeException();
+        }
+        double[] tmp = new double[length];
+        for (int i = 0; i < length; i++) {
+            tmp[i] = values[i] + v.values[i];
+        }
+        return new GVector(tmp);
+    }
+
+    /**
+     * Sets the value of this vector to sum of itself and the specified vector
+     *
+     * @param vector the second vector
+     * @return itself
+     */
+    public final GVector addSelf(GVector vector) {
+        if (length != vector.length) {
+            throw new MatrixSizeException();
+        }
+        for (int i = 0; i < length; i++) {
+            this.values[i] += vector.values[i];
+        }
+        return this;
+    }
+
+    /**
+     * Returns the (n-space) angle in radians between this vector and the vector
+     * parameter; the return value is constrained to the range [0,PI].
+     *
+     * @param v The other vector
+     * @return The angle in radians in the range [0,PI]
+     */
+    public final double angleBetween(GVector v) {
+        return (Math.acos(this.dot(v) / (this.magnitude() * v.magnitude())));
+    }
+
+    /**
+     * LU Decomposition Back Solve; this method takes the LU matrix and the
+     * permutation vector produced by the GMatrix method LUD and solves the
+     * equation (LU)*x = b by placing the solution vector x into this vector.
+     * This vector should be the same length or longer than b.
+     *
+     * @param LU The matrix into which the lower and upper decompostions have
+     * been placed
+     * @param b The b vector in the equation (LU)*x = b
+     * @param permutation The row permuations that were necessary to produce the
+     * LU matrix parameter
+     */
+    public final void backSolveLUD(GMatrix LU, GVector b, GVector permutation) {
+        int size = LU.nRow * LU.nCol;
+
+        double[] temp = new double[size];
+        double[] result = new double[size];
+        int[] row_perm = new int[b.size()];
+        int i, j;
+
+        if (LU.nRow != b.size()) {
+            throw new MatrixSizeException();
+        }
+
+        if (LU.nRow != permutation.size()) {
+            throw new MatrixSizeException();
+        }
+
+        if (LU.nRow != LU.nCol) {
+            throw new MatrixSizeException();
+        }
+
+        for (i = 0; i < LU.nRow; i++) {
+            for (j = 0; j < LU.nCol; j++) {
+                temp[i * LU.nCol + j] = LU.values[i][j];
+            }
+        }
+
+        for (i = 0; i < LU.nRow; i++) {
+            result[i * LU.nCol] = b.values[i];
+        }
+        for (i = 0; i < LU.nCol; i++) {
+            row_perm[i] = (int) permutation.values[i];
+        }
+
+        GMatrix.backSubstituteLU(LU.nRow, temp, row_perm, result);
+
+        for (i = 0; i < LU.nRow; i++) {
+            this.values[i] = result[i * LU.nCol];
+        }
+    }
+
+    /**
+     * Solves for x in Ax = b, where x is this vector (nx1), A is mxn, b is mx1,
+     * and A = U*W*transpose(V); U,W,V must be precomputed and can be found by
+     * taking the singular value decomposition (SVD) of A using the method SVD
+     * found in the GMatrix class.
+     *
+     * @param U The U matrix produced by the GMatrix method SVD
+     * @param W The W matrix produced by the GMatrix method SVD
+     * @param V The V matrix produced by the GMatrix method SVD
+     * @param b The b vector in the linear equation Ax = b
+     */
+    public final void backSolveSVD(GMatrix U, GMatrix W, GMatrix V, GVector b) {
+        if (!(U.nRow == b.size() && U.nRow == U.nCol && U.nRow == W.nRow)) {
+            throw new MatrixSizeException();
+        }
+        if (!(W.nCol == values.length && W.nCol == V.nCol && W.nCol == V.nRow)) {
+            throw new MatrixSizeException();
+        }
+        GMatrix tmp = new GMatrix(U.nRow, W.nCol);
+        tmp.mul(U, V);
+        tmp.mulTransposeRight(U, W);
+        tmp.invert();
+        mul(tmp, b);
+    }
+
+    /**
+     * Creates a new object of the same class as this object.
+     *
+     * @return a clone of this instance.
+     * @exception OutOfMemoryError if there is not enough memory.
+     * @see java.lang.Cloneable
+     * @since vecmath 1.3
+     */
+    
+    
+//    public Object clone() {
+//        GVector v = null;
+//        try {
+//            v = (GVector) super.clone();
+//        } catch (CloneNotSupportedException e) {
+//            throw new InternalError();
+//        }
+//        v.values = new double[length];
+//        System.arraycopy(values, 0, v.values, 0, length);
+//        return v;
+//    }
+
+    /**
+     * Returns the dot product of this vector and vector v.
+     *
+     * @param v the other vector
+     * @return the dot product of this and v
+     */
+    public final double dot(GVector v) {
+        if (length != v.length) {
+            throw new MatrixSizeException();
+        }
+        double result = 0.0;
+        for (int i = 0; i < length; i++) {
+            result += values[i] * v.values[i];
+        }
+        return result;
+    }
+
+    /**
+     * Returns true if all of the data members of GVector vector1 are equal to
+     * the corresponding data members in this GVector.
+     *
+     * @param vector1 The vector with which the comparison is made.
+     * @return true or false
+     */
+    public boolean equals(GVector vector1) {
+        try {
+            if (length != vector1.length) {
+                return false;
+            }
+            for (int i = 0; i < length; i++) {
+                if (values[i] != vector1.values[i]) {
+                    return false;
+                }
+            }
+            return true;
+        } catch (NullPointerException e) {
+            return false;
+        }
+    }
+
+    /**
+     * Returns true if the Object o1 is of type GMatrix and all of the data
+     * members of o1 are equal to the corresponding data members in this
+     * GMatrix.
+     *
+     * @param o1 The object with which the comparison is made.
+     * @return true or false
+     */
+    @Override
+    public boolean equals(Object o1) {
+        if (o1 instanceof GVector) {
+            GVector v2 = (GVector) o1;
+            if (length != v2.length) {
+                return false;
+            }
+            for (int i = 0; i < length; i++) {
+                if (values[i] != v2.values[i]) {
+                    return false;
+                }
+            }
+            return true;
+        }
+        return false;
+    }
+    
+    /**
+     * Returns a hash code value based on the data values in this object. Two
+     * different GVector objects with identical data values (i.e.,
+     * GVector.equals returns true) will return the same hash number. Two
+     * GVector objects with different data members may return the same hash
+     * value, although this is not likely.
+     *
+     * @return the integer hash code value
+     */
+
+    @Override
+    public int hashCode() {
+        int hash = 5;
+        hash = 43 * hash + this.length;
+        hash = 43 * hash + Arrays.hashCode(this.values);
+        return hash;
+    }
+
+    /**
+     * Returns true if the L-infinite distance between this vector and vector v
+     * is less than or equal to the tolerance parameter, otherwise returns
+     * false. The L-infinite distance is equal to MAX[abs(x1-x2), abs(y1-y2), .
+     * . . ].
+     *
+     * @param v The vector to be compared to this vector
+     * @param tolerance the threshold value
+     * @return
+     */
+    public boolean equalsWithTolerance(GVector v, double tolerance) {
+        if (v instanceof GVector) {
+            double diff;
+            if (length != v.length) {
+                return false;
+            }
+            for (int i = 0; i < length; i++) {
+                diff = values[i] - v.values[i];
+                if ((diff < 0 ? -diff : diff) > tolerance) {
+                    return false;
+                }
+            }
+            return true;
+        } 
+            return false;        
+    }
+
+    /**
+     * Retrieves the value at the specified index value of this vector.
+     *
+     * @param index the index of the element to retrieve (zero indexed)
+     * @return the value at the indexed element
+     */
+    public final double get(int index) {
+        return values[index];
+    }
+
+    /**
+     * Linearly interpolates this vector to the target vector and places the
+     * result into a new instance: result = this + (target-this)*alpha. The
+     * target vector needs to be equal sized.
+     *
+     * @param v the target vector
+     * @param alpha the alpha interpolation parameter
+     * @return result as new vector
+     */
+    public final GVector interpolateTo(GVector v, double alpha) {
+        if (length != v.length) {
+            throw new MatrixSizeException();
+        }
+        return new GVector(this).interpolateToSelf(v, alpha);
+    }
+
+    /**
+     * Interpolates the vector towards the given target vector, using the given
+     * {@link InterpolateStrategy}. The target vector needs to be equal sized.
+     *
+     * @param v target vector
+     * @param alpha interpolation factor (should be in the range 0..1)
+     * @param strategy InterpolateStrategy instance
+     *
+     * @return result as new vector
+     */
+    public final GVector interpolateTo(GVector v, double alpha,
+            InterpolateStrategy strategy) {
+        if (length != v.length) {
+            throw new MatrixSizeException();
+        }
+        return new GVector(this).interpolateToSelf(v, alpha, strategy);
+    }
+
+    /**
+     * Linearly interpolates this vector to the target vector and places result
+     * in this vector. result = this + (target-this)*alpha. The target vector
+     * needs to be equal sized.
+     *
+     * @param v the target vector
+     * @param alpha the alpha interpolation parameter
+     * @return 
+     */
+    public final GVector interpolateToSelf(GVector v, double alpha) {
+        if (v.length != length) {
+            throw new MatrixSizeException();
+        }
+        for (int i = 0; i < length; i++) {
+            values[i] += (v.values[i] - values[i]) * alpha;
+        }
+        return this;
+    }
+
+    /**
+     * Interpolates the vector towards the given target vector, using the given
+     * {@link InterpolateStrategy}. The target vector needs to be equal sized.
+     *
+     * @param v target vector
+     * @param alpha interpolation factor (should be in the range 0..1)
+     * @param strategy InterpolateStrategy instance
+     *
+     * @return itself, result overrides current vector
+     */
+    public final GVector interpolateToSelf(GVector v, double alpha,
+            InterpolateStrategy strategy) {
+        if (v.length != length) {
+            throw new MatrixSizeException();
+        }
+        for (int i = 0; i < length; i++) {
+            values[i] = strategy.interpolate(values[i], v.values[i], alpha);
+        }
+        return this;
+    }
+
+    /**
+     * Negates the value of this vector: this = -this.
+     */
+    public final void invert() {
+        for (int i = 0; i < length; i++) {
+            this.values[i] *= -1.0;
+        }
+    }
+
+    /**
+     * Returns the square root of the sum of the squares of this vector (its
+     * length in n-dimensional space).
+     *
+     * @return length of this vector
+     */
+    public final double magnitude() {
+        double sq = 0.0;
+        for (int i = 0; i < length; i++) {
+            sq += values[i] * values[i];
+        }
+        return Math.sqrt(sq);
+    }
+
+    /**
+     * Returns the sum of the squares of this vector (its length squared in
+     * n-dimensional space).
+     *
+     * @return length squared of this vector
+     */
+    public final double magSquared() {
+        double sq = 0.0;
+        for (int i = 0; i < length; i++) {
+            sq += values[i] * values[i];
+        }
+        return sq;
+    }
+
+    /**
+     * Multiplies matrix m1 times Vector v1 and places the result into this
+     * vector (this = m1*v1).
+     *
+     * @param m1 The matrix in the multiplication
+     * @param v1 The vector that is multiplied
+     */
+    public final void mul(GMatrix m1, GVector v1) {
+        if (m1.getNumCol() != v1.length) {
+            throw new MatrixSizeException();
+        }
+
+        if (length != m1.getNumRow()) {
+            throw new MatrixSizeException();
+        }
+
+        double v[];
+        if (v1 != this) {
+            v = v1.values;
+        } else {
+            v = values.clone();
+        }
+
+        for (int j = length - 1; j >= 0; j--) {
+            values[j] = 0.0;
+            for (int i = v1.length - 1; i >= 0; i--) {
+                values[j] += m1.values[j][i] * v[i];
+            }
+        }
+    }
+
+    /**
+     * Multiplies the transpose of vector v1 (ie, v1 becomes a row vector with
+     * respect to the multiplication) times matrix m1 and places the result into
+     * this vector (this = transpose(v1)*m1). The result is technically a row
+     * vector, but the GVector class only knows about column vectors, and so the
+     * result is stored as a column vector.
+     *
+     * @param m1 The matrix in the multiplication
+     * @param v1 The vector that is temporarily transposed
+     */
+    public final void mul(GVector v1, GMatrix m1) {
+        if (m1.getNumRow() != v1.length) {
+            throw new MatrixSizeException();
+        }
+
+        if (length != m1.getNumCol()) {
+            throw new MatrixSizeException();
+        }
+
+        double v[];
+        if (v1 != this) {
+            v = v1.values;
+        } else {
+            v = values.clone();
+        }
+
+        for (int j = length - 1; j >= 0; j--) {
+            values[j] = 0.0;
+            for (int i = v1.length - 1; i >= 0; i--) {
+                values[j] += m1.values[i][j] * v[i];
+            }
+        }
+    }
+
+    /**
+     * Normalizes this vector in place.
+     */
+    public final void normalize() {
+        double mag = magnitude();
+        if (mag > MathUtils.EPS) {
+            double invMag = 1.0 / mag;
+            for (int i = 0; i < length; i++) {
+                values[i] = values[i] * invMag;
+            }
+        }
+    }
+
+    /**
+     * Scales this vector by the scale factor s and returns result as new
+     * vector.
+     *
+     * @param s the scalar value
+     * @return new vector
+     */
+    public final GVector scale(double s) {
+        double[] tmp = new double[length];
+        for (int i = 0; i < length; i++) {
+            tmp[i] = values[i] * s;
+        }
+        return new GVector(tmp);
+    }
+
+    /**
+     * Scales the values of this vector with the values of the given vector
+     * vector (this = this * vector). Returns result as new vector.
+     *
+     * @param v scale vector
+     * @return new vector
+     */
+    public final GVector scale(GVector v) {
+        if (length != v.length) {
+            throw new MatrixSizeException();
+        }
+        double[] tmp = new double[length];
+        for (int i = 0; i < length; i++) {
+            tmp[i] = values[i] * v.values[i];
+        }
+        return new GVector(tmp);
+    }
+
+    /**
+     * Scales this vector by the scale factor s.
+     *
+     * @param s the scalar value
+     * @return itself
+     */
+    public final GVector scaleSelf(double s) {
+        for (int i = 0; i < length; i++) {
+            values[i] = values[i] * s;
+        }
+        return this;
+    }
+
+    /**
+     * Scales the values of this vector with the values of the given vector
+     * vector (this = this * vector).
+     *
+     * @param v scale vector
+     * @return itself
+     */
+    public final GVector scaleSelf(GVector v) {
+        if (length != v.length) {
+            throw new MatrixSizeException();
+        }
+        for (int i = 0; i < length; i++) {
+            this.values[i] *= v.values[i];
+        }
+        return this;
+    }
+
+    /**
+     * Sets the values of this vector to the values found in the array
+     * parameter. If the array is shorter than the number of values in this
+     * vector the remaining values are zeroed. If the array is longer, only the
+     * first values up to to the vector length are copied.
+     *
+     * @param vector the source array
+     * @return
+     */
+    public final GVector set(double[] vector) {
+        int i;
+        if (vector.length >= length) {
+            for (i = 0; i < length; i++) {
+                values[i] = vector[i];
+            }
+        } else {
+            for (i = 0; i < vector.length; i++) {
+                values[i] = vector[i];
+            }
+            for (i = vector.length; i < length; i++) {
+                values[i] = 0.0;
+            }
+        }
+        return this;
+    }
+
+    /**
+     * Sets the value of this vector to the values found in vector vector.
+     *
+     * @param vector the source vector
+     * @return
+     */
+    public final GVector set(GVector vector) {
+        return set(vector.values);
+    }
+
+    /**
+     * Sets the value of this vector to the values in tuple
+     *
+     * @param tuple the source for the new GVector's new values
+     * @return
+     */
+    public final GVector set(ReadonlyVec2D tuple) {
+        return set(new double[]{
+            tuple.x(), tuple.y()
+        });
+    }
+
+    /**
+     * Sets the value of this vector to the values in tuple
+     *
+     * @param tuple the source for the new GVector's new values
+     * @return
+     */
+    public final GVector set(ReadonlyVec3D tuple) {
+        return set(new double[]{
+            tuple.x(), tuple.y(), tuple.z()
+        });
+    }
+
+    /**
+     * Sets the value of this vector to the values in tuple
+     *
+     * @param tuple the source for the new GVector's new values
+     * @return itself
+     */
+    public final GVector set(ReadonlyVec4D tuple) {
+        return set(new double[]{
+            tuple.x(), tuple.y(), tuple.w()
+        });
+    }
+
+    /**
+     * Modifies the value at the specified index of this vector.
+     *
+     * @param index the index if the element to modify (zero indexed)
+     * @param value the new vector element value
+     * @return
+     */
+    public final GVector setElement(int index, double value) {
+        values[index] = value;
+        return this;
+    }
+
+    /**
+     * Changes the size of this vector dynamically. If the size is increased no
+     * data values will be lost. If the size is decreased, only those data
+     * values whose vector positions were eliminated will be lost.
+     *
+     * @param length number of desired elements in this vector
+     * @return
+     */
+    public final GVector setSize(int length) {
+        double[] tmp = new double[length];
+        int max;
+        if (this.length < length) {
+            max = this.length;
+        } else {
+            max = length;
+        }
+        System.arraycopy(values, 0, tmp, 0, max);
+        this.length = length;
+        values = tmp;
+        return this;
+    }
+
+    /**
+     * Returns the number of elements in this vector.
+     *
+     * @return number of elements in this vector
+     */
+    public final int size() {
+        return values.length;
+    }
+
+    /**
+     * Creates the vector difference of this vector and the given one (must be
+     * equal sized). Returns result as new vector.
+     *
+     * @param v
+     * @return new vector
+     */
+    public final GVector sub(GVector v) {
+        if (length != v.length) {
+            throw new MatrixSizeException();
+        }
+        double[] tmp = new double[length];
+        for (int i = 0; i < length; i++) {
+            tmp[i] = values[i] - v.values[i];
+        }
+        return new GVector(tmp);
+    }
+
+    /**
+     * Sets the value of this vector to the vector difference of itself and
+     * vector (this = this - vector).
+     *
+     * @param vector the other vector
+     * @return
+     */
+    public final GVector subSelf(GVector vector) {
+        if (length != vector.length) {
+            throw new MatrixSizeException();
+        }
+        for (int i = 0; i < length; i++) {
+            this.values[i] -= vector.values[i];
+        }
+        return this;
+    }
+
+    /**
+     * Returns a string that contains the values of this GVector.
+     *
+     * @return the String representation
+     */
+    @Override
+    public String toString() {
+        StringBuilder buffer = new StringBuilder(length * 8);
+        for (int i = 0; i < length; i++) {
+            buffer.append(values[i]).append(" ");
+        }
+        return buffer.toString();
+    }
+
+    /**
+     * Sets all the values in this vector to zero.
+     *
+     * @return
+     */
+    public final GVector zero() {
+        for (int i = 0; i < length; i++) {
+            this.values[i] = 0.0;
+        }
+        return this;
+    }
+}