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    }