View Javadoc

1   /*
2    * Licensed to the Apache Software Foundation (ASF) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * The ASF licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *      http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  
18  package org.apache.commons.math.geometry;
19  
20  import java.io.Serializable;
21  
22  import org.apache.commons.math.MathRuntimeException;
23  import org.apache.commons.math.util.MathUtils;
24  
25  /** 
26   * This class implements vectors in a three-dimensional space.
27   * <p>Instance of this class are guaranteed to be immutable.</p>
28   * @version $Revision: 769880 $ $Date: 2009-04-29 15:10:01 -0400 (Wed, 29 Apr 2009) $
29   * @since 1.2
30   */
31  
32  public class Vector3D
33    implements Serializable {
34  
35    /** Null vector (coordinates: 0, 0, 0). */
36    public static final Vector3D ZERO   = new Vector3D(0, 0, 0);
37  
38    /** First canonical vector (coordinates: 1, 0, 0). */
39    public static final Vector3D PLUS_I = new Vector3D(1, 0, 0);
40  
41    /** Opposite of the first canonical vector (coordinates: -1, 0, 0). */
42    public static final Vector3D MINUS_I = new Vector3D(-1, 0, 0);
43  
44    /** Second canonical vector (coordinates: 0, 1, 0). */
45    public static final Vector3D PLUS_J = new Vector3D(0, 1, 0);
46  
47    /** Opposite of the second canonical vector (coordinates: 0, -1, 0). */
48    public static final Vector3D MINUS_J = new Vector3D(0, -1, 0);
49  
50    /** Third canonical vector (coordinates: 0, 0, 1). */
51    public static final Vector3D PLUS_K = new Vector3D(0, 0, 1);
52  
53    /** Opposite of the third canonical vector (coordinates: 0, 0, -1).  */
54    public static final Vector3D MINUS_K = new Vector3D(0, 0, -1);
55  
56    /** A vector with all coordinates set to NaN. */
57    public static final Vector3D NaN = new Vector3D(Double.NaN, Double.NaN, Double.NaN);
58  
59    /** A vector with all coordinates set to positive infinity. */
60    public static final Vector3D POSITIVE_INFINITY =
61        new Vector3D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
62  
63    /** A vector with all coordinates set to negative infinity. */
64    public static final Vector3D NEGATIVE_INFINITY =
65        new Vector3D(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY);
66  
67    /** Default format. */
68    private static final Vector3DFormat DEFAULT_FORMAT =
69        Vector3DFormat.getInstance();
70  
71    /** Serializable version identifier. */
72    private static final long serialVersionUID = 5133268763396045979L;
73  
74    /** Abscissa. */
75    private final double x;
76  
77    /** Ordinate. */
78    private final double y;
79  
80    /** Height. */
81    private final double z;
82  
83    /** Simple constructor.
84     * Build a vector from its coordinates
85     * @param x abscissa
86     * @param y ordinate
87     * @param z height
88     * @see #getX()
89     * @see #getY()
90     * @see #getZ()
91     */
92    public Vector3D(double x, double y, double z) {
93      this.x = x;
94      this.y = y;
95      this.z = z;
96    }
97  
98    /** Simple constructor.
99     * 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 }