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 (α) around Z
101 * (0 is +X, π/2 is +Y, π is -X and 3π/2 is -Y)
102 * @param delta elevation (δ) above (XY) plane, from -π/2 to +π/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>∞</sub> norm for the vector.
221 * @return L<sub>∞</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 (α) of the vector, between -π and +π
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 (δ) of the vector, between -π/2 and +π/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>∞</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>∞</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 }