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