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