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    *      https://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  /*
19   * This is not the original file distributed by the Apache Software Foundation
20   * It has been modified by the Hipparchus project
21   */
22  
23  package org.hipparchus.geometry.euclidean.threed;
24  
25  import org.hipparchus.exception.LocalizedCoreFormats;
26  import org.hipparchus.exception.MathIllegalArgumentException;
27  import org.hipparchus.exception.MathRuntimeException;
28  import org.hipparchus.geometry.Point;
29  import org.hipparchus.geometry.Space;
30  import org.hipparchus.geometry.Vector;
31  import org.hipparchus.util.FastMath;
32  import org.hipparchus.util.MathArrays;
33  import org.hipparchus.util.MathUtils;
34  import org.hipparchus.util.SinCos;
35  
36  import java.io.Serializable;
37  import java.text.NumberFormat;
38  
39  /**
40   * This class implements vectors in a three-dimensional space.
41   * <p>Instance of this class are guaranteed to be immutable.</p>
42   */
43  public class Vector3D implements Serializable, Vector<Euclidean3D, Vector3D> {
44  
45      /** Null vector (coordinates: 0, 0, 0). */
46      public static final Vector3D ZERO   = new Vector3D(0, 0, 0);
47  
48      /** First canonical vector (coordinates: 1, 0, 0). */
49      public static final Vector3D PLUS_I = new Vector3D(1, 0, 0);
50  
51      /** Opposite of the first canonical vector (coordinates: -1, 0, 0). */
52      public static final Vector3D MINUS_I = new Vector3D(-1, 0, 0);
53  
54      /** Second canonical vector (coordinates: 0, 1, 0). */
55      public static final Vector3D PLUS_J = new Vector3D(0, 1, 0);
56  
57      /** Opposite of the second canonical vector (coordinates: 0, -1, 0). */
58      public static final Vector3D MINUS_J = new Vector3D(0, -1, 0);
59  
60      /** Third canonical vector (coordinates: 0, 0, 1). */
61      public static final Vector3D PLUS_K = new Vector3D(0, 0, 1);
62  
63      /** Opposite of the third canonical vector (coordinates: 0, 0, -1).  */
64      public static final Vector3D MINUS_K = new Vector3D(0, 0, -1);
65  
66      // CHECKSTYLE: stop ConstantName
67      /** A vector with all coordinates set to NaN. */
68      public static final Vector3D NaN = new Vector3D(Double.NaN, Double.NaN, Double.NaN);
69      // CHECKSTYLE: resume ConstantName
70  
71      /** A vector with all coordinates set to positive infinity. */
72      public static final Vector3D POSITIVE_INFINITY =
73          new Vector3D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
74  
75      /** A vector with all coordinates set to negative infinity. */
76      public static final Vector3D NEGATIVE_INFINITY =
77          new Vector3D(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY);
78  
79      /** Serializable version identifier. */
80      private static final long serialVersionUID = 1313493323784566947L;
81  
82      /** Abscissa. */
83      private final double x;
84  
85      /** Ordinate. */
86      private final double y;
87  
88      /** Height. */
89      private final double z;
90  
91      /** Simple constructor.
92       * Build a vector from its coordinates
93       * @param x abscissa
94       * @param y ordinate
95       * @param z height
96       * @see #getX()
97       * @see #getY()
98       * @see #getZ()
99       */
100     public Vector3D(double x, double y, double z) {
101         this.x = x;
102         this.y = y;
103         this.z = z;
104     }
105 
106     /** Simple constructor.
107      * Build a vector from its coordinates
108      * @param v coordinates array
109      * @exception MathIllegalArgumentException if array does not have 3 elements
110      * @see #toArray()
111      */
112     public Vector3D(double[] v) throws MathIllegalArgumentException {
113         if (v.length != 3) {
114             throw new MathIllegalArgumentException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
115                                                    v.length, 3);
116         }
117         this.x = v[0];
118         this.y = v[1];
119         this.z = v[2];
120     }
121 
122     /** Simple constructor.
123      * Build a vector from its azimuthal coordinates
124      * @param alpha azimuth (&alpha;) around Z
125      *              (0 is +X, &pi;/2 is +Y, &pi; is -X and 3&pi;/2 is -Y)
126      * @param delta elevation (&delta;) above (XY) plane, from -&pi;/2 to +&pi;/2
127      * @see #getAlpha()
128      * @see #getDelta()
129      */
130     public Vector3D(double alpha, double delta) {
131         SinCos sinCosAlpha = FastMath.sinCos(alpha);
132         SinCos sinCosDelta = FastMath.sinCos(delta);
133         this.x = sinCosAlpha.cos() * sinCosDelta.cos();
134         this.y = sinCosAlpha.sin() * sinCosDelta.cos();
135         this.z = sinCosDelta.sin();
136     }
137 
138     /** Multiplicative constructor
139      * Build a vector from another one and a scale factor.
140      * The vector built will be a * u
141      * @param a scale factor
142      * @param u base (unscaled) vector
143      */
144     public Vector3D(double a, Vector3D u) {
145         this.x = a * u.x;
146         this.y = a * u.y;
147         this.z = a * u.z;
148     }
149 
150     /** Linear constructor
151      * Build a vector from two other ones and corresponding scale factors.
152      * The vector built will be a1 * u1 + a2 * u2
153      * @param a1 first scale factor
154      * @param u1 first base (unscaled) vector
155      * @param a2 second scale factor
156      * @param u2 second base (unscaled) vector
157      */
158     public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2) {
159         this.x = MathArrays.linearCombination(a1, u1.x, a2, u2.x);
160         this.y = MathArrays.linearCombination(a1, u1.y, a2, u2.y);
161         this.z = MathArrays.linearCombination(a1, u1.z, a2, u2.z);
162     }
163 
164     /** Linear constructor
165      * Build a vector from three other ones and corresponding scale factors.
166      * The vector built will be a1 * u1 + a2 * u2 + a3 * u3
167      * @param a1 first scale factor
168      * @param u1 first base (unscaled) vector
169      * @param a2 second scale factor
170      * @param u2 second base (unscaled) vector
171      * @param a3 third scale factor
172      * @param u3 third base (unscaled) vector
173      */
174     public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2,
175                     double a3, Vector3D u3) {
176         this.x = MathArrays.linearCombination(a1, u1.x, a2, u2.x, a3, u3.x);
177         this.y = MathArrays.linearCombination(a1, u1.y, a2, u2.y, a3, u3.y);
178         this.z = MathArrays.linearCombination(a1, u1.z, a2, u2.z, a3, u3.z);
179     }
180 
181     /** Linear constructor
182      * Build a vector from four other ones and corresponding scale factors.
183      * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4
184      * @param a1 first scale factor
185      * @param u1 first base (unscaled) vector
186      * @param a2 second scale factor
187      * @param u2 second base (unscaled) vector
188      * @param a3 third scale factor
189      * @param u3 third base (unscaled) vector
190      * @param a4 fourth scale factor
191      * @param u4 fourth base (unscaled) vector
192      */
193     public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2,
194                     double a3, Vector3D u3, double a4, Vector3D u4) {
195         this.x = MathArrays.linearCombination(a1, u1.x, a2, u2.x, a3, u3.x, a4, u4.x);
196         this.y = MathArrays.linearCombination(a1, u1.y, a2, u2.y, a3, u3.y, a4, u4.y);
197         this.z = MathArrays.linearCombination(a1, u1.z, a2, u2.z, a3, u3.z, a4, u4.z);
198     }
199 
200     /** Get the abscissa of the vector.
201      * @return abscissa of the vector
202      * @see #Vector3D(double, double, double)
203      */
204     public double getX() {
205         return x;
206     }
207 
208     /** Get the ordinate of the vector.
209      * @return ordinate of the vector
210      * @see #Vector3D(double, double, double)
211      */
212     public double getY() {
213         return y;
214     }
215 
216     /** Get the height of the vector.
217      * @return height of the vector
218      * @see #Vector3D(double, double, double)
219      */
220     public double getZ() {
221         return z;
222     }
223 
224     /** Get the vector coordinates as a dimension 3 array.
225      * @return vector coordinates
226      * @see #Vector3D(double[])
227      */
228     public double[] toArray() {
229         return new double[] { x, y, z };
230     }
231 
232     /** {@inheritDoc} */
233     @Override
234     public Space getSpace() {
235         return Euclidean3D.getInstance();
236     }
237 
238     /** {@inheritDoc} */
239     @Override
240     public Vector3D getZero() {
241         return ZERO;
242     }
243 
244     /** {@inheritDoc} */
245     @Override
246     public double getNorm1() {
247         return FastMath.abs(x) + FastMath.abs(y) + FastMath.abs(z);
248     }
249 
250     /** {@inheritDoc} */
251     @Override
252     public double getNorm() {
253         // there are no cancellation problems here, so we use the straightforward formula
254         return FastMath.sqrt (x * x + y * y + z * z);
255     }
256 
257     /** {@inheritDoc} */
258     @Override
259     public double getNormSq() {
260         // there are no cancellation problems here, so we use the straightforward formula
261         return x * x + y * y + z * z;
262     }
263 
264     /** {@inheritDoc} */
265     @Override
266     public double getNormInf() {
267         return FastMath.max(FastMath.max(FastMath.abs(x), FastMath.abs(y)), FastMath.abs(z));
268     }
269 
270     /** Get the azimuth of the vector.
271      * @return azimuth (&alpha;) of the vector, between -&pi; and +&pi;
272      * @see #Vector3D(double, double)
273      */
274     public double getAlpha() {
275         return FastMath.atan2(y, x);
276     }
277 
278     /** Get the elevation of the vector.
279      * @return elevation (&delta;) of the vector, between -&pi;/2 and +&pi;/2
280      * @see #Vector3D(double, double)
281      */
282     public double getDelta() {
283         return FastMath.asin(z / getNorm());
284     }
285 
286     /** {@inheritDoc} */
287     @Override
288     public Vector3D add(final Vector<Euclidean3D, Vector3D> v) {
289         final Vector3D v3 = (Vector3D) v;
290         return new Vector3D(x + v3.x, y + v3.y, z + v3.z);
291     }
292 
293     /** {@inheritDoc} */
294     @Override
295     public Vector3D add(double factor, final Vector<Euclidean3D, Vector3D> v) {
296         return new Vector3D(1, this, factor, (Vector3D) v);
297     }
298 
299     /** {@inheritDoc} */
300     @Override
301     public Vector3D subtract(final Vector<Euclidean3D, Vector3D> v) {
302         final Vector3D v3 = (Vector3D) v;
303         return new Vector3D(x - v3.x, y - v3.y, z - v3.z);
304     }
305 
306     /** {@inheritDoc} */
307     @Override
308     public Vector3D subtract(final double factor, final Vector<Euclidean3D, Vector3D> v) {
309         return new Vector3D(1, this, -factor, (Vector3D) v);
310     }
311 
312     /** Get a vector orthogonal to the instance.
313      * <p>There are an infinite number of normalized vectors orthogonal
314      * to the instance. This method picks up one of them almost
315      * arbitrarily. It is useful when one needs to compute a reference
316      * frame with one of the axes in a predefined direction. The
317      * following example shows how to build a frame having the k axis
318      * aligned with the known vector u :
319      * </p>
320      * <pre><code>
321      *   Vector3D k = u.normalize();
322      *   Vector3D i = k.orthogonal();
323      *   Vector3D j = Vector3D.crossProduct(k, i);
324      * </code></pre>
325      * @return a new normalized vector orthogonal to the instance
326      * @exception MathRuntimeException if the norm of the instance is null
327      */
328     public Vector3D orthogonal() throws MathRuntimeException {
329 
330         double threshold = 0.6 * getNorm();
331         if (threshold == 0) {
332             throw new MathRuntimeException(LocalizedCoreFormats.ZERO_NORM);
333         }
334 
335         if (FastMath.abs(x) <= threshold) {
336             double inverse  = 1 / FastMath.sqrt(y * y + z * z);
337             return new Vector3D(0, inverse * z, -inverse * y);
338         } else if (FastMath.abs(y) <= threshold) {
339             double inverse  = 1 / FastMath.sqrt(x * x + z * z);
340             return new Vector3D(-inverse * z, 0, inverse * x);
341         }
342         double inverse  = 1 / FastMath.sqrt(x * x + y * y);
343         return new Vector3D(inverse * y, -inverse * x, 0);
344 
345     }
346 
347     /** Compute the angular separation between two vectors.
348      * <p>This method computes the angular separation between two
349      * vectors using the dot product for well separated vectors and the
350      * cross product for almost aligned vectors. This allows to have a
351      * good accuracy in all cases, even for vectors very close to each
352      * other.</p>
353      * @param v1 first vector
354      * @param v2 second vector
355      * @return angular separation between v1 and v2
356      * @exception MathRuntimeException if either vector has a null norm
357      */
358     public static double angle(Vector3D v1, Vector3D v2) throws MathRuntimeException {
359 
360         double normProduct = v1.getNorm() * v2.getNorm();
361         if (normProduct == 0) {
362             throw new MathRuntimeException(LocalizedCoreFormats.ZERO_NORM);
363         }
364 
365         double dot = v1.dotProduct(v2);
366         double threshold = normProduct * 0.9999;
367         if ((dot < -threshold) || (dot > threshold)) {
368             // the vectors are almost aligned, compute using the sine
369             Vector3D v3 = crossProduct(v1, v2);
370             if (dot >= 0) {
371                 return FastMath.asin(v3.getNorm() / normProduct);
372             }
373             return FastMath.PI - FastMath.asin(v3.getNorm() / normProduct);
374         }
375 
376         // the vectors are sufficiently separated to use the cosine
377         return FastMath.acos(dot / normProduct);
378 
379     }
380 
381     /** {@inheritDoc} */
382     @Override
383     public Vector3D negate() {
384         return new Vector3D(-x, -y, -z);
385     }
386 
387     /** {@inheritDoc} */
388     @Override
389     public Vector3D scalarMultiply(double a) {
390         return new Vector3D(a * x, a * y, a * z);
391     }
392 
393     /** {@inheritDoc} */
394     @Override
395     public boolean isNaN() {
396         return Double.isNaN(x) || Double.isNaN(y) || Double.isNaN(z);
397     }
398 
399     /** {@inheritDoc} */
400     @Override
401     public boolean isInfinite() {
402         return !isNaN() && (Double.isInfinite(x) || Double.isInfinite(y) || Double.isInfinite(z));
403     }
404 
405     /**
406      * Test for the equality of two 3D vectors.
407      * <p>
408      * If all coordinates of two 3D vectors are exactly the same, and none are
409      * {@code Double.NaN}, the two 3D vectors are considered to be equal.
410      * </p>
411      * <p>
412      * {@code NaN} coordinates are considered to affect globally the vector
413      * and be equals to each other - i.e, if either (or all) coordinates of the
414      * 3D vector are equal to {@code Double.NaN}, the 3D vector is equal to
415      * {@link #NaN}.
416      * </p>
417      *
418      * @param other Object to test for equality to this
419      * @return true if two 3D vector objects are equal, false if
420      *         object is null, not an instance of Vector3D, or
421      *         not equal to this Vector3D instance
422      *
423      */
424     @Override
425     public boolean equals(Object other) {
426 
427         if (this == other) {
428             return true;
429         }
430 
431         if (other instanceof Vector3D) {
432             final Vector3D rhs = (Vector3D)other;
433             return x == rhs.x && y == rhs.y && z == rhs.z || isNaN() && rhs.isNaN();
434         }
435 
436         return false;
437 
438     }
439 
440     /**
441      * Test for the equality of two 3D vectors.
442      * <p>
443      * If all coordinates of two 3D vectors are exactly the same, and none are
444      * {@code NaN}, the two 3D vectors are considered to be equal.
445      * </p>
446      * <p>
447      * In compliance with IEEE754 handling, if any coordinates of any of the
448      * two vectors are {@code NaN}, then the vectors are considered different.
449      * This implies that {@link #NaN Vector3D.NaN}.equals({@link #NaN Vector3D.NaN})
450      * returns {@code false} despite the instance is checked against itself.
451      * </p>
452      *
453      * @param other Object to test for equality to this
454      * @return true if two 3D vector objects are equal, false if
455      *         object is null, not an instance of Vector3D, or
456      *         not equal to this Vector3D instance
457      * @since 2.1
458      */
459     public boolean equalsIeee754(Object other) {
460 
461         if (this == other && !isNaN()) {
462             return true;
463         }
464 
465         if (other instanceof Vector3D) {
466             final Vector3D rhs = (Vector3D) other;
467             return x == rhs.x && y == rhs.y && z == rhs.z;
468         }
469 
470         return false;
471 
472     }
473 
474     /**
475      * Get a hashCode for the 3D vector.
476      * <p>
477      * All NaN values have the same hash code.</p>
478      *
479      * @return a hash code value for this object
480      */
481     @Override
482     public int hashCode() {
483         if (isNaN()) {
484             return 642;
485         }
486         return 643 * (164 * MathUtils.hash(x) +  3 * MathUtils.hash(y) +  MathUtils.hash(z));
487     }
488 
489     /** {@inheritDoc}
490      * <p>
491      * The implementation uses specific multiplication and addition
492      * algorithms to preserve accuracy and reduce cancellation effects.
493      * It should be very accurate even for nearly orthogonal vectors.
494      * </p>
495      * @see MathArrays#linearCombination(double, double, double, double, double, double)
496      */
497     @Override
498     public double dotProduct(final Vector<Euclidean3D, Vector3D> v) {
499         final Vector3D v3 = (Vector3D) v;
500         return MathArrays.linearCombination(x, v3.x, y, v3.y, z, v3.z);
501     }
502 
503     /** Compute the cross-product of the instance with another vector.
504      * @param v other vector
505      * @return the cross product this ^ v as a new Vector3D
506      */
507     public Vector3D crossProduct(final Vector<Euclidean3D, Vector3D> v) {
508         final Vector3D v3 = (Vector3D) v;
509         return new Vector3D(MathArrays.linearCombination(y, v3.z, -z, v3.y),
510                             MathArrays.linearCombination(z, v3.x, -x, v3.z),
511                             MathArrays.linearCombination(x, v3.y, -y, v3.x));
512     }
513 
514     /** {@inheritDoc} */
515     @Override
516     public double distance1(Vector<Euclidean3D, Vector3D> v) {
517         final Vector3D v3 = (Vector3D) v;
518         final double dx = FastMath.abs(v3.x - x);
519         final double dy = FastMath.abs(v3.y - y);
520         final double dz = FastMath.abs(v3.z - z);
521         return dx + dy + dz;
522     }
523 
524     /** {@inheritDoc} */
525     @Override
526     public double distance(Point<Euclidean3D> v) {
527         final Vector3D v3 = (Vector3D) v;
528         final double dx = v3.x - x;
529         final double dy = v3.y - y;
530         final double dz = v3.z - z;
531         return FastMath.sqrt(dx * dx + dy * dy + dz * dz);
532     }
533 
534     /** {@inheritDoc} */
535     @Override
536     public double distanceInf(Vector<Euclidean3D, Vector3D> v) {
537         final Vector3D v3 = (Vector3D) v;
538         final double dx = FastMath.abs(v3.x - x);
539         final double dy = FastMath.abs(v3.y - y);
540         final double dz = FastMath.abs(v3.z - z);
541         return FastMath.max(FastMath.max(dx, dy), dz);
542     }
543 
544     /** {@inheritDoc} */
545     @Override
546     public double distanceSq(Vector<Euclidean3D, Vector3D> v) {
547         final Vector3D v3 = (Vector3D) v;
548         final double dx = v3.x - x;
549         final double dy = v3.y - y;
550         final double dz = v3.z - z;
551         return dx * dx + dy * dy + dz * dz;
552     }
553 
554     /** Compute the dot-product of two vectors.
555      * @param v1 first vector
556      * @param v2 second vector
557      * @return the dot product v1.v2
558      */
559     public static double dotProduct(Vector3D v1, Vector3D v2) {
560         return v1.dotProduct(v2);
561     }
562 
563     /** Compute the cross-product of two vectors.
564      * @param v1 first vector
565      * @param v2 second vector
566      * @return the cross product v1 ^ v2 as a new Vector
567      */
568     public static Vector3D crossProduct(final Vector3D v1, final Vector3D v2) {
569         return v1.crossProduct(v2);
570     }
571 
572     /** Compute the distance between two vectors according to the L<sub>1</sub> norm.
573      * <p>Calling this method is equivalent to calling:
574      * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate
575      * vector is built</p>
576      * @param v1 first vector
577      * @param v2 second vector
578      * @return the distance between v1 and v2 according to the L<sub>1</sub> norm
579      */
580     public static double distance1(Vector3D v1, Vector3D v2) {
581         return v1.distance1(v2);
582     }
583 
584     /** Compute the distance between two vectors according to the L<sub>2</sub> norm.
585      * <p>Calling this method is equivalent to calling:
586      * <code>v1.subtract(v2).getNorm()</code> except that no intermediate
587      * vector is built</p>
588      * @param v1 first vector
589      * @param v2 second vector
590      * @return the distance between v1 and v2 according to the L<sub>2</sub> norm
591      */
592     public static double distance(Vector3D v1, Vector3D v2) {
593         return v1.distance(v2);
594     }
595 
596     /** Compute the distance between two vectors according to the L<sub>&infin;</sub> norm.
597      * <p>Calling this method is equivalent to calling:
598      * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate
599      * vector is built</p>
600      * @param v1 first vector
601      * @param v2 second vector
602      * @return the distance between v1 and v2 according to the L<sub>&infin;</sub> norm
603      */
604     public static double distanceInf(Vector3D v1, Vector3D v2) {
605         return v1.distanceInf(v2);
606     }
607 
608     /** Compute the square of the distance between two vectors.
609      * <p>Calling this method is equivalent to calling:
610      * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate
611      * vector is built</p>
612      * @param v1 first vector
613      * @param v2 second vector
614      * @return the square of the distance between v1 and v2
615      */
616     public static double distanceSq(Vector3D v1, Vector3D v2) {
617         return v1.distanceSq(v2);
618     }
619 
620     /** Get a string representation of this vector.
621      * @return a string representation of this vector
622      */
623     @Override
624     public String toString() {
625         return Vector3DFormat.getVector3DFormat().format(this);
626     }
627 
628     /** {@inheritDoc} */
629     @Override
630     public String toString(final NumberFormat format) {
631         return new Vector3DFormat(format).format(this);
632     }
633 
634 }