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