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.Vector;
027import org.apache.commons.math3.geometry.Space;
028import org.apache.commons.math3.util.FastMath;
029import org.apache.commons.math3.util.MathUtils;
030import 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 1447259 2013-02-18 13:56:39Z luc $
036 * @since 1.2
037 */
038public 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 (FastMath.abs(x) <= threshold) {
327            double inverse  = 1 / FastMath.sqrt(y * y + z * z);
328            return new Vector3D(0, inverse * z, -inverse * y);
329        } else if (FastMath.abs(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}