    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.math.geometry;
    019
    020    import java.io.Serializable;
    021
    022    import org.apache.commons.math.MathRuntimeException;
    023    import org.apache.commons.math.exception.util.LocalizedFormats;
    024    import org.apache.commons.math.util.MathUtils;
    025    import org.apache.commons.math.util.FastMath;
    026
    027    /**
    028     * This class implements vectors in a three-dimensional space.
    029     * <p>Instance of this class are guaranteed to be immutable.</p>
    030     * @version $Revision: 990655 $ $Date: 2010-08-29 23:49:40 +0200 (dim. 29 ao??t 2010) $
    031     * @since 1.2
    032     */
    033
    034    public class Vector3D
    035      implements Serializable {
    036
    037      /** Null vector (coordinates: 0, 0, 0). */
    038      public static final Vector3D ZERO   = new Vector3D(0, 0, 0);
    039
    040      /** First canonical vector (coordinates: 1, 0, 0). */
    041      public static final Vector3D PLUS_I = new Vector3D(1, 0, 0);
    042
    043      /** Opposite of the first canonical vector (coordinates: -1, 0, 0). */
    044      public static final Vector3D MINUS_I = new Vector3D(-1, 0, 0);
    045
    046      /** Second canonical vector (coordinates: 0, 1, 0). */
    047      public static final Vector3D PLUS_J = new Vector3D(0, 1, 0);
    048
    049      /** Opposite of the second canonical vector (coordinates: 0, -1, 0). */
    050      public static final Vector3D MINUS_J = new Vector3D(0, -1, 0);
    051
    052      /** Third canonical vector (coordinates: 0, 0, 1). */
    053      public static final Vector3D PLUS_K = new Vector3D(0, 0, 1);
    054
    055      /** Opposite of the third canonical vector (coordinates: 0, 0, -1).  */
    056      public static final Vector3D MINUS_K = new Vector3D(0, 0, -1);
    057
    058      // CHECKSTYLE: stop ConstantName
    059      /** A vector with all coordinates set to NaN. */
    060      public static final Vector3D NaN = new Vector3D(Double.NaN, Double.NaN, Double.NaN);
    061      // CHECKSTYLE: resume ConstantName
    062
    063      /** A vector with all coordinates set to positive infinity. */
    064      public static final Vector3D POSITIVE_INFINITY =
    065          new Vector3D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
    066
    067      /** A vector with all coordinates set to negative infinity. */
    068      public static final Vector3D NEGATIVE_INFINITY =
    069          new Vector3D(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY);
    070
    071      /** Default format. */
    072      private static final Vector3DFormat DEFAULT_FORMAT =
    073          Vector3DFormat.getInstance();
    074
    075      /** Serializable version identifier. */
    076      private static final long serialVersionUID = 5133268763396045979L;
    077
    078      /** Abscissa. */
    079      private final double x;
    080
    081      /** Ordinate. */
    082      private final double y;
    083
    084      /** Height. */
    085      private final double z;
    086
    087      /** Simple constructor.
    088       * Build a vector from its coordinates
    089       * @param x abscissa
    090       * @param y ordinate
    091       * @param z height
    092       * @see #getX()
    093       * @see #getY()
    094       * @see #getZ()
    095       */
    096      public Vector3D(double x, double y, double z) {
    097        this.x = x;
    098        this.y = y;
    099        this.z = z;
    100      }
    101
    102      /** Simple constructor.
    103       * Build a vector from its azimuthal coordinates
    104       * @param alpha azimuth (&alpha;) around Z
    105       *              (0 is +X, &pi;/2 is +Y, &pi; is -X and 3&pi;/2 is -Y)
    106       * @param delta elevation (&delta;) above (XY) plane, from -&pi;/2 to +&pi;/2
    107       * @see #getAlpha()
    108       * @see #getDelta()
    109       */
    110      public Vector3D(double alpha, double delta) {
    111        double cosDelta = FastMath.cos(delta);
    112        this.x = FastMath.cos(alpha) * cosDelta;
    113        this.y = FastMath.sin(alpha) * cosDelta;
    114        this.z = FastMath.sin(delta);
    115      }
    116
    117      /** Multiplicative constructor
    118       * Build a vector from another one and a scale factor.
    119       * The vector built will be a * u
    120       * @param a scale factor
    121       * @param u base (unscaled) vector
    122       */
    123      public Vector3D(double a, Vector3D u) {
    124        this.x = a * u.x;
    125        this.y = a * u.y;
    126        this.z = a * u.z;
    127      }
    128
    129      /** Linear constructor
    130       * Build a vector from two other ones and corresponding scale factors.
    131       * The vector built will be a1 * u1 + a2 * u2
    132       * @param a1 first scale factor
    133       * @param u1 first base (unscaled) vector
    134       * @param a2 second scale factor
    135       * @param u2 second base (unscaled) vector
    136       */
    137      public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2) {
    138        this.x = a1 * u1.x + a2 * u2.x;
    139        this.y = a1 * u1.y + a2 * u2.y;
    140        this.z = a1 * u1.z + a2 * u2.z;
    141      }
    142
    143      /** Linear constructor
    144       * Build a vector from three other ones and corresponding scale factors.
    145       * The vector built will be a1 * u1 + a2 * u2 + a3 * u3
    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       * @param a3 third scale factor
    151       * @param u3 third base (unscaled) vector
    152       */
    153      public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2,
    154                      double a3, Vector3D u3) {
    155        this.x = a1 * u1.x + a2 * u2.x + a3 * u3.x;
    156        this.y = a1 * u1.y + a2 * u2.y + a3 * u3.y;
    157        this.z = a1 * u1.z + a2 * u2.z + a3 * u3.z;
    158      }
    159
    160      /** Linear constructor
    161       * Build a vector from four other ones and corresponding scale factors.
    162       * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4
    163       * @param a1 first scale factor
    164       * @param u1 first base (unscaled) vector
    165       * @param a2 second scale factor
    166       * @param u2 second base (unscaled) vector
    167       * @param a3 third scale factor
    168       * @param u3 third base (unscaled) vector
    169       * @param a4 fourth scale factor
    170       * @param u4 fourth base (unscaled) vector
    171       */
    172      public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2,
    173                      double a3, Vector3D u3, double a4, Vector3D u4) {
    174        this.x = a1 * u1.x + a2 * u2.x + a3 * u3.x + a4 * u4.x;
    175        this.y = a1 * u1.y + a2 * u2.y + a3 * u3.y + a4 * u4.y;
    176        this.z = a1 * u1.z + a2 * u2.z + a3 * u3.z + a4 * u4.z;
    177      }
    178
    179      /** Get the abscissa of the vector.
    180       * @return abscissa of the vector
    181       * @see #Vector3D(double, double, double)
    182       */
    183      public double getX() {
    184        return x;
    185      }
    186
    187      /** Get the ordinate of the vector.
    188       * @return ordinate of the vector
    189       * @see #Vector3D(double, double, double)
    190       */
    191      public double getY() {
    192        return y;
    193      }
    194
    195      /** Get the height of the vector.
    196       * @return height of the vector
    197       * @see #Vector3D(double, double, double)
    198       */
    199      public double getZ() {
    200        return z;
    201      }
    202
    203      /** Get the L<sub>1</sub> norm for the vector.
    204       * @return L<sub>1</sub> norm for the vector
    205       */
    206      public double getNorm1() {
    207        return FastMath.abs(x) + FastMath.abs(y) + FastMath.abs(z);
    208      }
    209
    210      /** Get the L<sub>2</sub> norm for the vector.
    211       * @return euclidian norm for the vector
    212       */
    213      public double getNorm() {
    214        return FastMath.sqrt (x * x + y * y + z * z);
    215      }
    216
    217      /** Get the square of the norm for the vector.
    218       * @return square of the euclidian norm for the vector
    219       */
    220      public double getNormSq() {
    221        return x * x + y * y + z * z;
    222      }
    223
    224      /** Get the L<sub>&infin;</sub> norm for the vector.
    225       * @return L<sub>&infin;</sub> norm for the vector
    226       */
    227      public double getNormInf() {
    228        return FastMath.max(FastMath.max(FastMath.abs(x), FastMath.abs(y)), FastMath.abs(z));
    229      }
    230
    231      /** Get the azimuth of the vector.
    232       * @return azimuth (&alpha;) of the vector, between -&pi; and +&pi;
    233       * @see #Vector3D(double, double)
    234       */
    235      public double getAlpha() {
    236        return FastMath.atan2(y, x);
    237      }
    238
    239      /** Get the elevation of the vector.
    240       * @return elevation (&delta;) of the vector, between -&pi;/2 and +&pi;/2
    241       * @see #Vector3D(double, double)
    242       */
    243      public double getDelta() {
    244        return FastMath.asin(z / getNorm());
    245      }
    246
    247      /** Add a vector to the instance.
    248       * @param v vector to add
    249       * @return a new vector
    250       */
    251      public Vector3D add(Vector3D v) {
    252        return new Vector3D(x + v.x, y + v.y, z + v.z);
    253      }
    254
    255      /** Add a scaled vector to the instance.
    256       * @param factor scale factor to apply to v before adding it
    257       * @param v vector to add
    258       * @return a new vector
    259       */
    260      public Vector3D add(double factor, Vector3D v) {
    261        return new Vector3D(x + factor * v.x, y + factor * v.y, z + factor * v.z);
    262      }
    263
    264      /** Subtract a vector from the instance.
    265       * @param v vector to subtract
    266       * @return a new vector
    267       */
    268      public Vector3D subtract(Vector3D v) {
    269        return new Vector3D(x - v.x, y - v.y, z - v.z);
    270      }
    271
    272      /** Subtract a scaled vector from the instance.
    273       * @param factor scale factor to apply to v before subtracting it
    274       * @param v vector to subtract
    275       * @return a new vector
    276       */
    277      public Vector3D subtract(double factor, Vector3D v) {
    278        return new Vector3D(x - factor * v.x, y - factor * v.y, z - factor * v.z);
    279      }
    280
    281      /** Get a normalized vector aligned with the instance.
    282       * @return a new normalized vector
    283       * @exception ArithmeticException if the norm is zero
    284       */
    285      public Vector3D normalize() {
    286        double s = getNorm();
    287        if (s == 0) {
    288          throw MathRuntimeException.createArithmeticException(LocalizedFormats.CANNOT_NORMALIZE_A_ZERO_NORM_VECTOR);
    289        }
    290        return scalarMultiply(1 / s);
    291      }
    292
    293      /** Get a vector orthogonal to the instance.
    294       * <p>There are an infinite number of normalized vectors orthogonal
    295       * to the instance. This method picks up one of them almost
    296       * arbitrarily. It is useful when one needs to compute a reference
    297       * frame with one of the axes in a predefined direction. The
    298       * following example shows how to build a frame having the k axis
    299       * aligned with the known vector u :
    300       * <pre><code>
    301       *   Vector3D k = u.normalize();
    302       *   Vector3D i = k.orthogonal();
    303       *   Vector3D j = Vector3D.crossProduct(k, i);
    304       * </code></pre></p>
    305       * @return a new normalized vector orthogonal to the instance
    306       * @exception ArithmeticException if the norm of the instance is null
    307       */
    308      public Vector3D orthogonal() {
    309
    310        double threshold = 0.6 * getNorm();
    311        if (threshold == 0) {
    312          throw MathRuntimeException.createArithmeticException(LocalizedFormats.ZERO_NORM);
    313        }
    314
    315        if ((x >= -threshold) && (x <= threshold)) {
    316          double inverse  = 1 / FastMath.sqrt(y * y + z * z);
    317          return new Vector3D(0, inverse * z, -inverse * y);
    318        } else if ((y >= -threshold) && (y <= threshold)) {
    319          double inverse  = 1 / FastMath.sqrt(x * x + z * z);
    320          return new Vector3D(-inverse * z, 0, inverse * x);
    321        }
    322        double inverse  = 1 / FastMath.sqrt(x * x + y * y);
    323        return new Vector3D(inverse * y, -inverse * x, 0);
    324
    325      }
    326
    327      /** Compute the angular separation between two vectors.
    328       * <p>This method computes the angular separation between two
    329       * vectors using the dot product for well separated vectors and the
    330       * cross product for almost aligned vectors. This allows to have a
    331       * good accuracy in all cases, even for vectors very close to each
    332       * other.</p>
    333       * @param v1 first vector
    334       * @param v2 second vector
    335       * @return angular separation between v1 and v2
    336       * @exception ArithmeticException if either vector has a null norm
    337       */
    338      public static double angle(Vector3D v1, Vector3D v2) {
    339
    340        double normProduct = v1.getNorm() * v2.getNorm();
    341        if (normProduct == 0) {
    342          throw MathRuntimeException.createArithmeticException(LocalizedFormats.ZERO_NORM);
    343        }
    344
    345        double dot = dotProduct(v1, v2);
    346        double threshold = normProduct * 0.9999;
    347        if ((dot < -threshold) || (dot > threshold)) {
    348          // the vectors are almost aligned, compute using the sine
    349          Vector3D v3 = crossProduct(v1, v2);
    350          if (dot >= 0) {
    351            return FastMath.asin(v3.getNorm() / normProduct);
    352          }
    353          return FastMath.PI - FastMath.asin(v3.getNorm() / normProduct);
    354        }
    355
    356        // the vectors are sufficiently separated to use the cosine
    357        return FastMath.acos(dot / normProduct);
    358
    359      }
    360
    361      /** Get the opposite of the instance.
    362       * @return a new vector which is opposite to the instance
    363       */
    364      public Vector3D negate() {
    365        return new Vector3D(-x, -y, -z);
    366      }
    367
    368      /** Multiply the instance by a scalar
    369       * @param a scalar
    370       * @return a new vector
    371       */
    372      public Vector3D scalarMultiply(double a) {
    373        return new Vector3D(a * x, a * y, a * z);
    374      }
    375
    376      /**
    377       * Returns true if any coordinate of this vector is NaN; false otherwise
    378       * @return  true if any coordinate of this vector is NaN; false otherwise
    379       */
    380      public boolean isNaN() {
    381          return Double.isNaN(x) || Double.isNaN(y) || Double.isNaN(z);
    382      }
    383
    384      /**
    385       * Returns true if any coordinate of this vector is infinite and none are NaN;
    386       * false otherwise
    387       * @return  true if any coordinate of this vector is infinite and none are NaN;
    388       * false otherwise
    389       */
    390      public boolean isInfinite() {
    391          return !isNaN() && (Double.isInfinite(x) || Double.isInfinite(y) || Double.isInfinite(z));
    392      }
    393
    394      /**
    395       * Test for the equality of two 3D vectors.
    396       * <p>
    397       * If all coordinates of two 3D vectors are exactly the same, and none are
    398       * <code>Double.NaN</code>, the two 3D vectors are considered to be equal.
    399       * </p>
    400       * <p>
    401       * <code>NaN</code> coordinates are considered to affect globally the vector
    402       * and be equals to each other - i.e, if either (or all) coordinates of the
    403       * 3D vector are equal to <code>Double.NaN</code>, the 3D vector is equal to
    404       * {@link #NaN}.
    405       * </p>
    406       *
    407       * @param other Object to test for equality to this
    408       * @return true if two 3D vector objects are equal, false if
    409       *         object is null, not an instance of Vector3D, or
    410       *         not equal to this Vector3D instance
    411       *
    412       */
    413      @Override
    414      public boolean equals(Object other) {
    415
    416        if (this == other) {
    417          return true;
    418        }
    419
    420        if (other instanceof Vector3D) {
    421          final Vector3D rhs = (Vector3D)other;
    422          if (rhs.isNaN()) {
    423              return this.isNaN();
    424          }
    425
    426          return (x == rhs.x) && (y == rhs.y) && (z == rhs.z);
    427        }
    428        return false;
    429      }
    430
    431      /**
    432       * Get a hashCode for the 3D vector.
    433       * <p>
    434       * All NaN values have the same hash code.</p>
    435       *
    436       * @return a hash code value for this object
    437       */
    438      @Override
    439      public int hashCode() {
    440          if (isNaN()) {
    441              return 8;
    442          }
    443          return 31 * (23 * MathUtils.hash(x) +  19 * MathUtils.hash(y) +  MathUtils.hash(z));
    444      }
    445
    446      /** Compute the dot-product of two vectors.
    447       * @param v1 first vector
    448       * @param v2 second vector
    449       * @return the dot product v1.v2
    450       */
    451      public static double dotProduct(Vector3D v1, Vector3D v2) {
    452        return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
    453      }
    454
    455      /** Compute the cross-product of two vectors.
    456       * @param v1 first vector
    457       * @param v2 second vector
    458       * @return the cross product v1 ^ v2 as a new Vector
    459       */
    460      public static Vector3D crossProduct(Vector3D v1, Vector3D v2) {
    461        return new Vector3D(v1.y * v2.z - v1.z * v2.y,
    462                            v1.z * v2.x - v1.x * v2.z,
    463                            v1.x * v2.y - v1.y * v2.x);
    464      }
    465
    466      /** Compute the distance between two vectors according to the L<sub>1</sub> norm.
    467       * <p>Calling this method is equivalent to calling:
    468       * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate
    469       * vector is built</p>
    470       * @param v1 first vector
    471       * @param v2 second vector
    472       * @return the distance between v1 and v2 according to the L<sub>1</sub> norm
    473       */
    474      public static double distance1(Vector3D v1, Vector3D v2) {
    475        final double dx = FastMath.abs(v2.x - v1.x);
    476        final double dy = FastMath.abs(v2.y - v1.y);
    477        final double dz = FastMath.abs(v2.z - v1.z);
    478        return dx + dy + dz;
    479      }
    480
    481      /** Compute the distance between two vectors according to the L<sub>2</sub> norm.
    482       * <p>Calling this method is equivalent to calling:
    483       * <code>v1.subtract(v2).getNorm()</code> except that no intermediate
    484       * vector is built</p>
    485       * @param v1 first vector
    486       * @param v2 second vector
    487       * @return the distance between v1 and v2 according to the L<sub>2</sub> norm
    488       */
    489      public static double distance(Vector3D v1, Vector3D v2) {
    490        final double dx = v2.x - v1.x;
    491        final double dy = v2.y - v1.y;
    492        final double dz = v2.z - v1.z;
    493        return FastMath.sqrt(dx * dx + dy * dy + dz * dz);
    494      }
    495
    496      /** Compute the distance between two vectors according to the L<sub>&infin;</sub> norm.
    497       * <p>Calling this method is equivalent to calling:
    498       * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate
    499       * vector is built</p>
    500       * @param v1 first vector
    501       * @param v2 second vector
    502       * @return the distance between v1 and v2 according to the L<sub>&infin;</sub> norm
    503       */
    504      public static double distanceInf(Vector3D v1, Vector3D v2) {
    505        final double dx = FastMath.abs(v2.x - v1.x);
    506        final double dy = FastMath.abs(v2.y - v1.y);
    507        final double dz = FastMath.abs(v2.z - v1.z);
    508        return FastMath.max(FastMath.max(dx, dy), dz);
    509      }
    510
    511      /** Compute the square of the distance between two vectors.
    512       * <p>Calling this method is equivalent to calling:
    513       * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate
    514       * vector is built</p>
    515       * @param v1 first vector
    516       * @param v2 second vector
    517       * @return the square of the distance between v1 and v2
    518       */
    519      public static double distanceSq(Vector3D v1, Vector3D v2) {
    520        final double dx = v2.x - v1.x;
    521        final double dy = v2.y - v1.y;
    522        final double dz = v2.z - v1.z;
    523        return dx * dx + dy * dy + dz * dz;
    524      }
    525
    526      /** Get a string representation of this vector.
    527       * @return a string representation of this vector
    528       */
    529      @Override
    530      public String toString() {
    531          return DEFAULT_FORMAT.format(this);
    532      }
    533
    534    }