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 018package org.apache.commons.math3.geometry.euclidean.threed; 019 020import java.io.Serializable; 021import java.text.NumberFormat; 022 023import org.apache.commons.math3.exception.DimensionMismatchException; 024import org.apache.commons.math3.exception.MathArithmeticException; 025import org.apache.commons.math3.exception.util.LocalizedFormats; 026import org.apache.commons.math3.geometry.Point; 027import org.apache.commons.math3.geometry.Space; 028import org.apache.commons.math3.geometry.Vector; 029import org.apache.commons.math3.util.FastMath; 030import org.apache.commons.math3.util.MathArrays; 031import org.apache.commons.math3.util.MathUtils; 032 033/** 034 * This class implements vectors in a three-dimensional space. 035 * <p>Instance of this class are guaranteed to be immutable.</p> 036 * @since 1.2 037 */ 038public 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 (FastMath.abs(x) <= threshold) { 327 double inverse = 1 / FastMath.sqrt(y * y + z * z); 328 return new Vector3D(0, inverse * z, -inverse * y); 329 } else if (FastMath.abs(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 return distance((Point<Euclidean3D>) v); 480 } 481 482 /** {@inheritDoc} */ 483 public double distance(Point<Euclidean3D> v) { 484 final Vector3D v3 = (Vector3D) v; 485 final double dx = v3.x - x; 486 final double dy = v3.y - y; 487 final double dz = v3.z - z; 488 return FastMath.sqrt(dx * dx + dy * dy + dz * dz); 489 } 490 491 /** {@inheritDoc} */ 492 public double distanceInf(Vector<Euclidean3D> v) { 493 final Vector3D v3 = (Vector3D) v; 494 final double dx = FastMath.abs(v3.x - x); 495 final double dy = FastMath.abs(v3.y - y); 496 final double dz = FastMath.abs(v3.z - z); 497 return FastMath.max(FastMath.max(dx, dy), dz); 498 } 499 500 /** {@inheritDoc} */ 501 public double distanceSq(Vector<Euclidean3D> v) { 502 final Vector3D v3 = (Vector3D) v; 503 final double dx = v3.x - x; 504 final double dy = v3.y - y; 505 final double dz = v3.z - z; 506 return dx * dx + dy * dy + dz * dz; 507 } 508 509 /** Compute the dot-product of two vectors. 510 * @param v1 first vector 511 * @param v2 second vector 512 * @return the dot product v1.v2 513 */ 514 public static double dotProduct(Vector3D v1, Vector3D v2) { 515 return v1.dotProduct(v2); 516 } 517 518 /** Compute the cross-product of two vectors. 519 * @param v1 first vector 520 * @param v2 second vector 521 * @return the cross product v1 ^ v2 as a new Vector 522 */ 523 public static Vector3D crossProduct(final Vector3D v1, final Vector3D v2) { 524 return v1.crossProduct(v2); 525 } 526 527 /** Compute the distance between two vectors according to the L<sub>1</sub> norm. 528 * <p>Calling this method is equivalent to calling: 529 * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate 530 * vector is built</p> 531 * @param v1 first vector 532 * @param v2 second vector 533 * @return the distance between v1 and v2 according to the L<sub>1</sub> norm 534 */ 535 public static double distance1(Vector3D v1, Vector3D v2) { 536 return v1.distance1(v2); 537 } 538 539 /** Compute the distance between two vectors according to the L<sub>2</sub> norm. 540 * <p>Calling this method is equivalent to calling: 541 * <code>v1.subtract(v2).getNorm()</code> except that no intermediate 542 * vector is built</p> 543 * @param v1 first vector 544 * @param v2 second vector 545 * @return the distance between v1 and v2 according to the L<sub>2</sub> norm 546 */ 547 public static double distance(Vector3D v1, Vector3D v2) { 548 return v1.distance(v2); 549 } 550 551 /** Compute the distance between two vectors according to the L<sub>∞</sub> norm. 552 * <p>Calling this method is equivalent to calling: 553 * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate 554 * vector is built</p> 555 * @param v1 first vector 556 * @param v2 second vector 557 * @return the distance between v1 and v2 according to the L<sub>∞</sub> norm 558 */ 559 public static double distanceInf(Vector3D v1, Vector3D v2) { 560 return v1.distanceInf(v2); 561 } 562 563 /** Compute the square of the distance between two vectors. 564 * <p>Calling this method is equivalent to calling: 565 * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate 566 * vector is built</p> 567 * @param v1 first vector 568 * @param v2 second vector 569 * @return the square of the distance between v1 and v2 570 */ 571 public static double distanceSq(Vector3D v1, Vector3D v2) { 572 return v1.distanceSq(v2); 573 } 574 575 /** Get a string representation of this vector. 576 * @return a string representation of this vector 577 */ 578 @Override 579 public String toString() { 580 return Vector3DFormat.getInstance().format(this); 581 } 582 583 /** {@inheritDoc} */ 584 public String toString(final NumberFormat format) { 585 return new Vector3DFormat(format).format(this); 586 } 587 588}