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.math3.geometry.euclidean.threed;
019
020 import java.io.Serializable;
021 import java.text.NumberFormat;
022
023 import org.apache.commons.math3.exception.DimensionMismatchException;
024 import org.apache.commons.math3.exception.MathArithmeticException;
025 import org.apache.commons.math3.exception.util.LocalizedFormats;
026 import org.apache.commons.math3.geometry.Vector;
027 import org.apache.commons.math3.geometry.Space;
028 import org.apache.commons.math3.util.FastMath;
029 import org.apache.commons.math3.util.MathUtils;
030 import 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 1416643 2012-12-03 19:37:14Z tn $
036 * @since 1.2
037 */
038 public 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 (α) around Z
119 * (0 is +X, π/2 is +Y, π is -X and 3π/2 is -Y)
120 * @param delta elevation (δ) above (XY) plane, from -π/2 to +π/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 (α) of the vector, between -π and +π
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 (δ) of the vector, between -π/2 and +π/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 ((x >= -threshold) && (x <= threshold)) {
327 double inverse = 1 / FastMath.sqrt(y * y + z * z);
328 return new Vector3D(0, inverse * z, -inverse * y);
329 } else if ((y >= -threshold) && (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>∞</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>∞</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 }