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