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