    001    /*
    002     * Licensed to the Apache Software Foundation (ASF) under one or more
    003     * contributor license agreements.  See the NOTICE file distributed with
    004     * this work for additional information regarding copyright ownership.
    005     * The ASF licenses this file to You under the Apache License, Version 2.0
    006     * (the "License"); you may not use this file except in compliance with
    007     * the License.  You may obtain a copy of the License at
    008     *
    009     *      http://www.apache.org/licenses/LICENSE-2.0
    010     *
    011     * Unless required by applicable law or agreed to in writing, software
    012     * distributed under the License is distributed on an "AS IS" BASIS,
    013     * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    014     * See the License for the specific language governing permissions and
    015     * limitations under the License.
    016     */
    017
    018    package org.apache.commons.math3.geometry.euclidean.threed;
    019
    020    import java.io.Serializable;
    021    import java.text.NumberFormat;
    022
    023    import org.apache.commons.math3.exception.DimensionMismatchException;
    024    import org.apache.commons.math3.exception.MathArithmeticException;
    025    import org.apache.commons.math3.exception.util.LocalizedFormats;
    026    import org.apache.commons.math3.geometry.Vector;
    027    import org.apache.commons.math3.geometry.Space;
    028    import org.apache.commons.math3.util.FastMath;
    029    import org.apache.commons.math3.util.MathUtils;
    030    import org.apache.commons.math3.util.MathArrays;
    031
    032    /**
    033     * This class implements vectors in a three-dimensional space.
    034     * <p>Instance of this class are guaranteed to be immutable.</p>
    035     * @version $Id: Vector3D.java 1416643 2012-12-03 19:37:14Z tn $
    036     * @since 1.2
    037     */
    038    public class Vector3D implements Serializable, Vector<Euclidean3D> {
    039
    040        /** Null vector (coordinates: 0, 0, 0). */
    041        public static final Vector3D ZERO   = new Vector3D(0, 0, 0);
    042
    043        /** First canonical vector (coordinates: 1, 0, 0). */
    044        public static final Vector3D PLUS_I = new Vector3D(1, 0, 0);
    045
    046        /** Opposite of the first canonical vector (coordinates: -1, 0, 0). */
    047        public static final Vector3D MINUS_I = new Vector3D(-1, 0, 0);
    048
    049        /** Second canonical vector (coordinates: 0, 1, 0). */
    050        public static final Vector3D PLUS_J = new Vector3D(0, 1, 0);
    051
    052        /** Opposite of the second canonical vector (coordinates: 0, -1, 0). */
    053        public static final Vector3D MINUS_J = new Vector3D(0, -1, 0);
    054
    055        /** Third canonical vector (coordinates: 0, 0, 1). */
    056        public static final Vector3D PLUS_K = new Vector3D(0, 0, 1);
    057
    058        /** Opposite of the third canonical vector (coordinates: 0, 0, -1).  */
    059        public static final Vector3D MINUS_K = new Vector3D(0, 0, -1);
    060
    061        // CHECKSTYLE: stop ConstantName
    062        /** A vector with all coordinates set to NaN. */
    063        public static final Vector3D NaN = new Vector3D(Double.NaN, Double.NaN, Double.NaN);
    064        // CHECKSTYLE: resume ConstantName
    065
    066        /** A vector with all coordinates set to positive infinity. */
    067        public static final Vector3D POSITIVE_INFINITY =
    068            new Vector3D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
    069
    070        /** A vector with all coordinates set to negative infinity. */
    071        public static final Vector3D NEGATIVE_INFINITY =
    072            new Vector3D(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY);
    073
    074        /** Serializable version identifier. */
    075        private static final long serialVersionUID = 1313493323784566947L;
    076
    077        /** Abscissa. */
    078        private final double x;
    079
    080        /** Ordinate. */
    081        private final double y;
    082
    083        /** Height. */
    084        private final double z;
    085
    086        /** Simple constructor.
    087         * Build a vector from its coordinates
    088         * @param x abscissa
    089         * @param y ordinate
    090         * @param z height
    091         * @see #getX()
    092         * @see #getY()
    093         * @see #getZ()
    094         */
    095        public Vector3D(double x, double y, double z) {
    096            this.x = x;
    097            this.y = y;
    098            this.z = z;
    099        }
    100
    101        /** Simple constructor.
    102         * Build a vector from its coordinates
    103         * @param v coordinates array
    104         * @exception DimensionMismatchException if array does not have 3 elements
    105         * @see #toArray()
    106         */
    107        public Vector3D(double[] v) throws DimensionMismatchException {
    108            if (v.length != 3) {
    109                throw new DimensionMismatchException(v.length, 3);
    110            }
    111            this.x = v[0];
    112            this.y = v[1];
    113            this.z = v[2];
    114        }
    115
    116        /** Simple constructor.
    117         * Build a vector from its azimuthal coordinates
    118         * @param alpha azimuth (&alpha;) around Z
    119         *              (0 is +X, &pi;/2 is +Y, &pi; is -X and 3&pi;/2 is -Y)
    120         * @param delta elevation (&delta;) above (XY) plane, from -&pi;/2 to +&pi;/2
    121         * @see #getAlpha()
    122         * @see #getDelta()
    123         */
    124        public Vector3D(double alpha, double delta) {
    125            double cosDelta = FastMath.cos(delta);
    126            this.x = FastMath.cos(alpha) * cosDelta;
    127            this.y = FastMath.sin(alpha) * cosDelta;
    128            this.z = FastMath.sin(delta);
    129        }
    130
    131        /** Multiplicative constructor
    132         * Build a vector from another one and a scale factor.
    133         * The vector built will be a * u
    134         * @param a scale factor
    135         * @param u base (unscaled) vector
    136         */
    137        public Vector3D(double a, Vector3D u) {
    138            this.x = a * u.x;
    139            this.y = a * u.y;
    140            this.z = a * u.z;
    141        }
    142
    143        /** Linear constructor
    144         * Build a vector from two other ones and corresponding scale factors.
    145         * The vector built will be a1 * u1 + a2 * u2
    146         * @param a1 first scale factor
    147         * @param u1 first base (unscaled) vector
    148         * @param a2 second scale factor
    149         * @param u2 second base (unscaled) vector
    150         */
    151        public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2) {
    152            this.x = MathArrays.linearCombination(a1, u1.x, a2, u2.x);
    153            this.y = MathArrays.linearCombination(a1, u1.y, a2, u2.y);
    154            this.z = MathArrays.linearCombination(a1, u1.z, a2, u2.z);
    155        }
    156
    157        /** Linear constructor
    158         * Build a vector from three other ones and corresponding scale factors.
    159         * The vector built will be a1 * u1 + a2 * u2 + a3 * u3
    160         * @param a1 first scale factor
    161         * @param u1 first base (unscaled) vector
    162         * @param a2 second scale factor
    163         * @param u2 second base (unscaled) vector
    164         * @param a3 third scale factor
    165         * @param u3 third base (unscaled) vector
    166         */
    167        public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2,
    168                        double a3, Vector3D u3) {
    169            this.x = MathArrays.linearCombination(a1, u1.x, a2, u2.x, a3, u3.x);
    170            this.y = MathArrays.linearCombination(a1, u1.y, a2, u2.y, a3, u3.y);
    171            this.z = MathArrays.linearCombination(a1, u1.z, a2, u2.z, a3, u3.z);
    172        }
    173
    174        /** Linear constructor
    175         * Build a vector from four other ones and corresponding scale factors.
    176         * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4
    177         * @param a1 first scale factor
    178         * @param u1 first base (unscaled) vector
    179         * @param a2 second scale factor
    180         * @param u2 second base (unscaled) vector
    181         * @param a3 third scale factor
    182         * @param u3 third base (unscaled) vector
    183         * @param a4 fourth scale factor
    184         * @param u4 fourth base (unscaled) vector
    185         */
    186        public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2,
    187                        double a3, Vector3D u3, double a4, Vector3D u4) {
    188            this.x = MathArrays.linearCombination(a1, u1.x, a2, u2.x, a3, u3.x, a4, u4.x);
    189            this.y = MathArrays.linearCombination(a1, u1.y, a2, u2.y, a3, u3.y, a4, u4.y);
    190            this.z = MathArrays.linearCombination(a1, u1.z, a2, u2.z, a3, u3.z, a4, u4.z);
    191        }
    192
    193        /** Get the abscissa of the vector.
    194         * @return abscissa of the vector
    195         * @see #Vector3D(double, double, double)
    196         */
    197        public double getX() {
    198            return x;
    199        }
    200
    201        /** Get the ordinate of the vector.
    202         * @return ordinate of the vector
    203         * @see #Vector3D(double, double, double)
    204         */
    205        public double getY() {
    206            return y;
    207        }
    208
    209        /** Get the height of the vector.
    210         * @return height of the vector
    211         * @see #Vector3D(double, double, double)
    212         */
    213        public double getZ() {
    214            return z;
    215        }
    216
    217        /** Get the vector coordinates as a dimension 3 array.
    218         * @return vector coordinates
    219         * @see #Vector3D(double[])
    220         */
    221        public double[] toArray() {
    222            return new double[] { x, y, z };
    223        }
    224
    225        /** {@inheritDoc} */
    226        public Space getSpace() {
    227            return Euclidean3D.getInstance();
    228        }
    229
    230        /** {@inheritDoc} */
    231        public Vector3D getZero() {
    232            return ZERO;
    233        }
    234
    235        /** {@inheritDoc} */
    236        public double getNorm1() {
    237            return FastMath.abs(x) + FastMath.abs(y) + FastMath.abs(z);
    238        }
    239
    240        /** {@inheritDoc} */
    241        public double getNorm() {
    242            // there are no cancellation problems here, so we use the straightforward formula
    243            return FastMath.sqrt (x * x + y * y + z * z);
    244        }
    245
    246        /** {@inheritDoc} */
    247        public double getNormSq() {
    248            // there are no cancellation problems here, so we use the straightforward formula
    249            return x * x + y * y + z * z;
    250        }
    251
    252        /** {@inheritDoc} */
    253        public double getNormInf() {
    254            return FastMath.max(FastMath.max(FastMath.abs(x), FastMath.abs(y)), FastMath.abs(z));
    255        }
    256
    257        /** Get the azimuth of the vector.
    258         * @return azimuth (&alpha;) of the vector, between -&pi; and +&pi;
    259         * @see #Vector3D(double, double)
    260         */
    261        public double getAlpha() {
    262            return FastMath.atan2(y, x);
    263        }
    264
    265        /** Get the elevation of the vector.
    266         * @return elevation (&delta;) of the vector, between -&pi;/2 and +&pi;/2
    267         * @see #Vector3D(double, double)
    268         */
    269        public double getDelta() {
    270            return FastMath.asin(z / getNorm());
    271        }
    272
    273        /** {@inheritDoc} */
    274        public Vector3D add(final Vector<Euclidean3D> v) {
    275            final Vector3D v3 = (Vector3D) v;
    276            return new Vector3D(x + v3.x, y + v3.y, z + v3.z);
    277        }
    278
    279        /** {@inheritDoc} */
    280        public Vector3D add(double factor, final Vector<Euclidean3D> v) {
    281            return new Vector3D(1, this, factor, (Vector3D) v);
    282        }
    283
    284        /** {@inheritDoc} */
    285        public Vector3D subtract(final Vector<Euclidean3D> v) {
    286            final Vector3D v3 = (Vector3D) v;
    287            return new Vector3D(x - v3.x, y - v3.y, z - v3.z);
    288        }
    289
    290        /** {@inheritDoc} */
    291        public Vector3D subtract(final double factor, final Vector<Euclidean3D> v) {
    292            return new Vector3D(1, this, -factor, (Vector3D) v);
    293        }
    294
    295        /** {@inheritDoc} */
    296        public Vector3D normalize() throws MathArithmeticException {
    297            double s = getNorm();
    298            if (s == 0) {
    299                throw new MathArithmeticException(LocalizedFormats.CANNOT_NORMALIZE_A_ZERO_NORM_VECTOR);
    300            }
    301            return scalarMultiply(1 / s);
    302        }
    303
    304        /** Get a vector orthogonal to the instance.
    305         * <p>There are an infinite number of normalized vectors orthogonal
    306         * to the instance. This method picks up one of them almost
    307         * arbitrarily. It is useful when one needs to compute a reference
    308         * frame with one of the axes in a predefined direction. The
    309         * following example shows how to build a frame having the k axis
    310         * aligned with the known vector u :
    311         * <pre><code>
    312         *   Vector3D k = u.normalize();
    313         *   Vector3D i = k.orthogonal();
    314         *   Vector3D j = Vector3D.crossProduct(k, i);
    315         * </code></pre></p>
    316         * @return a new normalized vector orthogonal to the instance
    317         * @exception MathArithmeticException if the norm of the instance is null
    318         */
    319        public Vector3D orthogonal() throws MathArithmeticException {
    320
    321            double threshold = 0.6 * getNorm();
    322            if (threshold == 0) {
    323                throw new MathArithmeticException(LocalizedFormats.ZERO_NORM);
    324            }
    325
    326            if ((x >= -threshold) && (x <= threshold)) {
    327                double inverse  = 1 / FastMath.sqrt(y * y + z * z);
    328                return new Vector3D(0, inverse * z, -inverse * y);
    329            } else if ((y >= -threshold) && (y <= threshold)) {
    330                double inverse  = 1 / FastMath.sqrt(x * x + z * z);
    331                return new Vector3D(-inverse * z, 0, inverse * x);
    332            }
    333            double inverse  = 1 / FastMath.sqrt(x * x + y * y);
    334            return new Vector3D(inverse * y, -inverse * x, 0);
    335
    336        }
    337
    338        /** Compute the angular separation between two vectors.
    339         * <p>This method computes the angular separation between two
    340         * vectors using the dot product for well separated vectors and the
    341         * cross product for almost aligned vectors. This allows to have a
    342         * good accuracy in all cases, even for vectors very close to each
    343         * other.</p>
    344         * @param v1 first vector
    345         * @param v2 second vector
    346         * @return angular separation between v1 and v2
    347         * @exception MathArithmeticException if either vector has a null norm
    348         */
    349        public static double angle(Vector3D v1, Vector3D v2) throws MathArithmeticException {
    350
    351            double normProduct = v1.getNorm() * v2.getNorm();
    352            if (normProduct == 0) {
    353                throw new MathArithmeticException(LocalizedFormats.ZERO_NORM);
    354            }
    355
    356            double dot = v1.dotProduct(v2);
    357            double threshold = normProduct * 0.9999;
    358            if ((dot < -threshold) || (dot > threshold)) {
    359                // the vectors are almost aligned, compute using the sine
    360                Vector3D v3 = crossProduct(v1, v2);
    361                if (dot >= 0) {
    362                    return FastMath.asin(v3.getNorm() / normProduct);
    363                }
    364                return FastMath.PI - FastMath.asin(v3.getNorm() / normProduct);
    365            }
    366
    367            // the vectors are sufficiently separated to use the cosine
    368            return FastMath.acos(dot / normProduct);
    369
    370        }
    371
    372        /** {@inheritDoc} */
    373        public Vector3D negate() {
    374            return new Vector3D(-x, -y, -z);
    375        }
    376
    377        /** {@inheritDoc} */
    378        public Vector3D scalarMultiply(double a) {
    379            return new Vector3D(a * x, a * y, a * z);
    380        }
    381
    382        /** {@inheritDoc} */
    383        public boolean isNaN() {
    384            return Double.isNaN(x) || Double.isNaN(y) || Double.isNaN(z);
    385        }
    386
    387        /** {@inheritDoc} */
    388        public boolean isInfinite() {
    389            return !isNaN() && (Double.isInfinite(x) || Double.isInfinite(y) || Double.isInfinite(z));
    390        }
    391
    392        /**
    393         * Test for the equality of two 3D vectors.
    394         * <p>
    395         * If all coordinates of two 3D vectors are exactly the same, and none are
    396         * <code>Double.NaN</code>, the two 3D vectors are considered to be equal.
    397         * </p>
    398         * <p>
    399         * <code>NaN</code> coordinates are considered to affect globally the vector
    400         * and be equals to each other - i.e, if either (or all) coordinates of the
    401         * 3D vector are equal to <code>Double.NaN</code>, the 3D vector is equal to
    402         * {@link #NaN}.
    403         * </p>
    404         *
    405         * @param other Object to test for equality to this
    406         * @return true if two 3D vector objects are equal, false if
    407         *         object is null, not an instance of Vector3D, or
    408         *         not equal to this Vector3D instance
    409         *
    410         */
    411        @Override
    412        public boolean equals(Object other) {
    413
    414            if (this == other) {
    415                return true;
    416            }
    417
    418            if (other instanceof Vector3D) {
    419                final Vector3D rhs = (Vector3D)other;
    420                if (rhs.isNaN()) {
    421                    return this.isNaN();
    422                }
    423
    424                return (x == rhs.x) && (y == rhs.y) && (z == rhs.z);
    425            }
    426            return false;
    427        }
    428
    429        /**
    430         * Get a hashCode for the 3D vector.
    431         * <p>
    432         * All NaN values have the same hash code.</p>
    433         *
    434         * @return a hash code value for this object
    435         */
    436        @Override
    437        public int hashCode() {
    438            if (isNaN()) {
    439                return 642;
    440            }
    441            return 643 * (164 * MathUtils.hash(x) +  3 * MathUtils.hash(y) +  MathUtils.hash(z));
    442        }
    443
    444        /** {@inheritDoc}
    445         * <p>
    446         * The implementation uses specific multiplication and addition
    447         * algorithms to preserve accuracy and reduce cancellation effects.
    448         * It should be very accurate even for nearly orthogonal vectors.
    449         * </p>
    450         * @see MathArrays#linearCombination(double, double, double, double, double, double)
    451         */
    452        public double dotProduct(final Vector<Euclidean3D> v) {
    453            final Vector3D v3 = (Vector3D) v;
    454            return MathArrays.linearCombination(x, v3.x, y, v3.y, z, v3.z);
    455        }
    456
    457        /** Compute the cross-product of the instance with another vector.
    458         * @param v other vector
    459         * @return the cross product this ^ v as a new Vector3D
    460         */
    461        public Vector3D crossProduct(final Vector<Euclidean3D> v) {
    462            final Vector3D v3 = (Vector3D) v;
    463            return new Vector3D(MathArrays.linearCombination(y, v3.z, -z, v3.y),
    464                                MathArrays.linearCombination(z, v3.x, -x, v3.z),
    465                                MathArrays.linearCombination(x, v3.y, -y, v3.x));
    466        }
    467
    468        /** {@inheritDoc} */
    469        public double distance1(Vector<Euclidean3D> v) {
    470            final Vector3D v3 = (Vector3D) v;
    471            final double dx = FastMath.abs(v3.x - x);
    472            final double dy = FastMath.abs(v3.y - y);
    473            final double dz = FastMath.abs(v3.z - z);
    474            return dx + dy + dz;
    475        }
    476
    477        /** {@inheritDoc} */
    478        public double distance(Vector<Euclidean3D> v) {
    479            final Vector3D v3 = (Vector3D) v;
    480            final double dx = v3.x - x;
    481            final double dy = v3.y - y;
    482            final double dz = v3.z - z;
    483            return FastMath.sqrt(dx * dx + dy * dy + dz * dz);
    484        }
    485
    486        /** {@inheritDoc} */
    487        public double distanceInf(Vector<Euclidean3D> v) {
    488            final Vector3D v3 = (Vector3D) v;
    489            final double dx = FastMath.abs(v3.x - x);
    490            final double dy = FastMath.abs(v3.y - y);
    491            final double dz = FastMath.abs(v3.z - z);
    492            return FastMath.max(FastMath.max(dx, dy), dz);
    493        }
    494
    495        /** {@inheritDoc} */
    496        public double distanceSq(Vector<Euclidean3D> v) {
    497            final Vector3D v3 = (Vector3D) v;
    498            final double dx = v3.x - x;
    499            final double dy = v3.y - y;
    500            final double dz = v3.z - z;
    501            return dx * dx + dy * dy + dz * dz;
    502        }
    503
    504        /** Compute the dot-product of two vectors.
    505         * @param v1 first vector
    506         * @param v2 second vector
    507         * @return the dot product v1.v2
    508         */
    509        public static double dotProduct(Vector3D v1, Vector3D v2) {
    510            return v1.dotProduct(v2);
    511        }
    512
    513        /** Compute the cross-product of two vectors.
    514         * @param v1 first vector
    515         * @param v2 second vector
    516         * @return the cross product v1 ^ v2 as a new Vector
    517         */
    518        public static Vector3D crossProduct(final Vector3D v1, final Vector3D v2) {
    519            return v1.crossProduct(v2);
    520        }
    521
    522        /** Compute the distance between two vectors according to the L<sub>1</sub> norm.
    523         * <p>Calling this method is equivalent to calling:
    524         * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate
    525         * vector is built</p>
    526         * @param v1 first vector
    527         * @param v2 second vector
    528         * @return the distance between v1 and v2 according to the L<sub>1</sub> norm
    529         */
    530        public static double distance1(Vector3D v1, Vector3D v2) {
    531            return v1.distance1(v2);
    532        }
    533
    534        /** Compute the distance between two vectors according to the L<sub>2</sub> norm.
    535         * <p>Calling this method is equivalent to calling:
    536         * <code>v1.subtract(v2).getNorm()</code> except that no intermediate
    537         * vector is built</p>
    538         * @param v1 first vector
    539         * @param v2 second vector
    540         * @return the distance between v1 and v2 according to the L<sub>2</sub> norm
    541         */
    542        public static double distance(Vector3D v1, Vector3D v2) {
    543            return v1.distance(v2);
    544        }
    545
    546        /** Compute the distance between two vectors according to the L<sub>&infin;</sub> norm.
    547         * <p>Calling this method is equivalent to calling:
    548         * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate
    549         * vector is built</p>
    550         * @param v1 first vector
    551         * @param v2 second vector
    552         * @return the distance between v1 and v2 according to the L<sub>&infin;</sub> norm
    553         */
    554        public static double distanceInf(Vector3D v1, Vector3D v2) {
    555            return v1.distanceInf(v2);
    556        }
    557
    558        /** Compute the square of the distance between two vectors.
    559         * <p>Calling this method is equivalent to calling:
    560         * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate
    561         * vector is built</p>
    562         * @param v1 first vector
    563         * @param v2 second vector
    564         * @return the square of the distance between v1 and v2
    565         */
    566        public static double distanceSq(Vector3D v1, Vector3D v2) {
    567            return v1.distanceSq(v2);
    568        }
    569
    570        /** Get a string representation of this vector.
    571         * @return a string representation of this vector
    572         */
    573        @Override
    574        public String toString() {
    575            return Vector3DFormat.getInstance().format(this);
    576        }
    577
    578        /** {@inheritDoc} */
    579        public String toString(final NumberFormat format) {
    580            return new Vector3DFormat(format).format(this);
    581        }
    582
    583    }