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.RealFieldElement; 024import org.apache.commons.math3.exception.DimensionMismatchException; 025import org.apache.commons.math3.exception.MathArithmeticException; 026import org.apache.commons.math3.exception.util.LocalizedFormats; 027import org.apache.commons.math3.util.FastMath; 028import org.apache.commons.math3.util.MathArrays; 029 030/** 031 * This class is a re-implementation of {@link Vector3D} using {@link RealFieldElement}. 032 * <p>Instance of this class are guaranteed to be immutable.</p> 033 * @param <T> the type of the field elements 034 * @since 3.2 035 */ 036public class FieldVector3D<T extends RealFieldElement<T>> implements Serializable { 037 038 /** Serializable version identifier. */ 039 private static final long serialVersionUID = 20130224L; 040 041 /** Abscissa. */ 042 private final T x; 043 044 /** Ordinate. */ 045 private final T y; 046 047 /** Height. */ 048 private final T z; 049 050 /** Simple constructor. 051 * Build a vector from its coordinates 052 * @param x abscissa 053 * @param y ordinate 054 * @param z height 055 * @see #getX() 056 * @see #getY() 057 * @see #getZ() 058 */ 059 public FieldVector3D(final T x, final T y, final T z) { 060 this.x = x; 061 this.y = y; 062 this.z = z; 063 } 064 065 /** Simple constructor. 066 * Build a vector from its coordinates 067 * @param v coordinates array 068 * @exception DimensionMismatchException if array does not have 3 elements 069 * @see #toArray() 070 */ 071 public FieldVector3D(final T[] v) throws DimensionMismatchException { 072 if (v.length != 3) { 073 throw new DimensionMismatchException(v.length, 3); 074 } 075 this.x = v[0]; 076 this.y = v[1]; 077 this.z = v[2]; 078 } 079 080 /** Simple constructor. 081 * Build a vector from its azimuthal coordinates 082 * @param alpha azimuth (α) around Z 083 * (0 is +X, π/2 is +Y, π is -X and 3π/2 is -Y) 084 * @param delta elevation (δ) above (XY) plane, from -π/2 to +π/2 085 * @see #getAlpha() 086 * @see #getDelta() 087 */ 088 public FieldVector3D(final T alpha, final T delta) { 089 T cosDelta = delta.cos(); 090 this.x = alpha.cos().multiply(cosDelta); 091 this.y = alpha.sin().multiply(cosDelta); 092 this.z = delta.sin(); 093 } 094 095 /** Multiplicative constructor 096 * Build a vector from another one and a scale factor. 097 * The vector built will be a * u 098 * @param a scale factor 099 * @param u base (unscaled) vector 100 */ 101 public FieldVector3D(final T a, final FieldVector3D<T>u) { 102 this.x = a.multiply(u.x); 103 this.y = a.multiply(u.y); 104 this.z = a.multiply(u.z); 105 } 106 107 /** Multiplicative constructor 108 * Build a vector from another one and a scale factor. 109 * The vector built will be a * u 110 * @param a scale factor 111 * @param u base (unscaled) vector 112 */ 113 public FieldVector3D(final T a, final Vector3D u) { 114 this.x = a.multiply(u.getX()); 115 this.y = a.multiply(u.getY()); 116 this.z = a.multiply(u.getZ()); 117 } 118 119 /** Multiplicative constructor 120 * Build a vector from another one and a scale factor. 121 * The vector built will be a * u 122 * @param a scale factor 123 * @param u base (unscaled) vector 124 */ 125 public FieldVector3D(final double a, final FieldVector3D<T> u) { 126 this.x = u.x.multiply(a); 127 this.y = u.y.multiply(a); 128 this.z = u.z.multiply(a); 129 } 130 131 /** Linear constructor 132 * Build a vector from two other ones and corresponding scale factors. 133 * The vector built will be a1 * u1 + a2 * u2 134 * @param a1 first scale factor 135 * @param u1 first base (unscaled) vector 136 * @param a2 second scale factor 137 * @param u2 second base (unscaled) vector 138 */ 139 public FieldVector3D(final T a1, final FieldVector3D<T> u1, 140 final T a2, final FieldVector3D<T> u2) { 141 final T prototype = a1; 142 this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX()); 143 this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY()); 144 this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ()); 145 } 146 147 /** Linear constructor 148 * Build a vector from two other ones and corresponding scale factors. 149 * The vector built will be a1 * u1 + a2 * u2 150 * @param a1 first scale factor 151 * @param u1 first base (unscaled) vector 152 * @param a2 second scale factor 153 * @param u2 second base (unscaled) vector 154 */ 155 public FieldVector3D(final T a1, final Vector3D u1, 156 final T a2, final Vector3D u2) { 157 final T prototype = a1; 158 this.x = prototype.linearCombination(u1.getX(), a1, u2.getX(), a2); 159 this.y = prototype.linearCombination(u1.getY(), a1, u2.getY(), a2); 160 this.z = prototype.linearCombination(u1.getZ(), a1, u2.getZ(), a2); 161 } 162 163 /** Linear constructor 164 * Build a vector from two other ones and corresponding scale factors. 165 * The vector built will be a1 * u1 + a2 * u2 166 * @param a1 first scale factor 167 * @param u1 first base (unscaled) vector 168 * @param a2 second scale factor 169 * @param u2 second base (unscaled) vector 170 */ 171 public FieldVector3D(final double a1, final FieldVector3D<T> u1, 172 final double a2, final FieldVector3D<T> u2) { 173 final T prototype = u1.getX(); 174 this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX()); 175 this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY()); 176 this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ()); 177 } 178 179 /** Linear constructor 180 * Build a vector from three other ones and corresponding scale factors. 181 * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 182 * @param a1 first scale factor 183 * @param u1 first base (unscaled) vector 184 * @param a2 second scale factor 185 * @param u2 second base (unscaled) vector 186 * @param a3 third scale factor 187 * @param u3 third base (unscaled) vector 188 */ 189 public FieldVector3D(final T a1, final FieldVector3D<T> u1, 190 final T a2, final FieldVector3D<T> u2, 191 final T a3, final FieldVector3D<T> u3) { 192 final T prototype = a1; 193 this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX(), a3, u3.getX()); 194 this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY(), a3, u3.getY()); 195 this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ(), a3, u3.getZ()); 196 } 197 198 /** Linear constructor 199 * Build a vector from three other ones and corresponding scale factors. 200 * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 201 * @param a1 first scale factor 202 * @param u1 first base (unscaled) vector 203 * @param a2 second scale factor 204 * @param u2 second base (unscaled) vector 205 * @param a3 third scale factor 206 * @param u3 third base (unscaled) vector 207 */ 208 public FieldVector3D(final T a1, final Vector3D u1, 209 final T a2, final Vector3D u2, 210 final T a3, final Vector3D u3) { 211 final T prototype = a1; 212 this.x = prototype.linearCombination(u1.getX(), a1, u2.getX(), a2, u3.getX(), a3); 213 this.y = prototype.linearCombination(u1.getY(), a1, u2.getY(), a2, u3.getY(), a3); 214 this.z = prototype.linearCombination(u1.getZ(), a1, u2.getZ(), a2, u3.getZ(), a3); 215 } 216 217 /** Linear constructor 218 * Build a vector from three other ones and corresponding scale factors. 219 * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 220 * @param a1 first scale factor 221 * @param u1 first base (unscaled) vector 222 * @param a2 second scale factor 223 * @param u2 second base (unscaled) vector 224 * @param a3 third scale factor 225 * @param u3 third base (unscaled) vector 226 */ 227 public FieldVector3D(final double a1, final FieldVector3D<T> u1, 228 final double a2, final FieldVector3D<T> u2, 229 final double a3, final FieldVector3D<T> u3) { 230 final T prototype = u1.getX(); 231 this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX(), a3, u3.getX()); 232 this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY(), a3, u3.getY()); 233 this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ(), a3, u3.getZ()); 234 } 235 236 /** Linear constructor 237 * Build a vector from four other ones and corresponding scale factors. 238 * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4 239 * @param a1 first scale factor 240 * @param u1 first base (unscaled) vector 241 * @param a2 second scale factor 242 * @param u2 second base (unscaled) vector 243 * @param a3 third scale factor 244 * @param u3 third base (unscaled) vector 245 * @param a4 fourth scale factor 246 * @param u4 fourth base (unscaled) vector 247 */ 248 public FieldVector3D(final T a1, final FieldVector3D<T> u1, 249 final T a2, final FieldVector3D<T> u2, 250 final T a3, final FieldVector3D<T> u3, 251 final T a4, final FieldVector3D<T> u4) { 252 final T prototype = a1; 253 this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX(), a3, u3.getX(), a4, u4.getX()); 254 this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY(), a3, u3.getY(), a4, u4.getY()); 255 this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ(), a3, u3.getZ(), a4, u4.getZ()); 256 } 257 258 /** Linear constructor 259 * Build a vector from four other ones and corresponding scale factors. 260 * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4 261 * @param a1 first scale factor 262 * @param u1 first base (unscaled) vector 263 * @param a2 second scale factor 264 * @param u2 second base (unscaled) vector 265 * @param a3 third scale factor 266 * @param u3 third base (unscaled) vector 267 * @param a4 fourth scale factor 268 * @param u4 fourth base (unscaled) vector 269 */ 270 public FieldVector3D(final T a1, final Vector3D u1, 271 final T a2, final Vector3D u2, 272 final T a3, final Vector3D u3, 273 final T a4, final Vector3D u4) { 274 final T prototype = a1; 275 this.x = prototype.linearCombination(u1.getX(), a1, u2.getX(), a2, u3.getX(), a3, u4.getX(), a4); 276 this.y = prototype.linearCombination(u1.getY(), a1, u2.getY(), a2, u3.getY(), a3, u4.getY(), a4); 277 this.z = prototype.linearCombination(u1.getZ(), a1, u2.getZ(), a2, u3.getZ(), a3, u4.getZ(), a4); 278 } 279 280 /** Linear constructor 281 * Build a vector from four other ones and corresponding scale factors. 282 * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4 283 * @param a1 first scale factor 284 * @param u1 first base (unscaled) vector 285 * @param a2 second scale factor 286 * @param u2 second base (unscaled) vector 287 * @param a3 third scale factor 288 * @param u3 third base (unscaled) vector 289 * @param a4 fourth scale factor 290 * @param u4 fourth base (unscaled) vector 291 */ 292 public FieldVector3D(final double a1, final FieldVector3D<T> u1, 293 final double a2, final FieldVector3D<T> u2, 294 final double a3, final FieldVector3D<T> u3, 295 final double a4, final FieldVector3D<T> u4) { 296 final T prototype = u1.getX(); 297 this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX(), a3, u3.getX(), a4, u4.getX()); 298 this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY(), a3, u3.getY(), a4, u4.getY()); 299 this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ(), a3, u3.getZ(), a4, u4.getZ()); 300 } 301 302 /** Get the abscissa of the vector. 303 * @return abscissa of the vector 304 * @see #FieldVector3D(RealFieldElement, RealFieldElement, RealFieldElement) 305 */ 306 public T getX() { 307 return x; 308 } 309 310 /** Get the ordinate of the vector. 311 * @return ordinate of the vector 312 * @see #FieldVector3D(RealFieldElement, RealFieldElement, RealFieldElement) 313 */ 314 public T getY() { 315 return y; 316 } 317 318 /** Get the height of the vector. 319 * @return height of the vector 320 * @see #FieldVector3D(RealFieldElement, RealFieldElement, RealFieldElement) 321 */ 322 public T getZ() { 323 return z; 324 } 325 326 /** Get the vector coordinates as a dimension 3 array. 327 * @return vector coordinates 328 * @see #FieldVector3D(RealFieldElement[]) 329 */ 330 public T[] toArray() { 331 final T[] array = MathArrays.buildArray(x.getField(), 3); 332 array[0] = x; 333 array[1] = y; 334 array[2] = z; 335 return array; 336 } 337 338 /** Convert to a constant vector without derivatives. 339 * @return a constant vector 340 */ 341 public Vector3D toVector3D() { 342 return new Vector3D(x.getReal(), y.getReal(), z.getReal()); 343 } 344 345 /** Get the L<sub>1</sub> norm for the vector. 346 * @return L<sub>1</sub> norm for the vector 347 */ 348 public T getNorm1() { 349 return x.abs().add(y.abs()).add(z.abs()); 350 } 351 352 /** Get the L<sub>2</sub> norm for the vector. 353 * @return Euclidean norm for the vector 354 */ 355 public T getNorm() { 356 // there are no cancellation problems here, so we use the straightforward formula 357 return x.multiply(x).add(y.multiply(y)).add(z.multiply(z)).sqrt(); 358 } 359 360 /** Get the square of the norm for the vector. 361 * @return square of the Euclidean norm for the vector 362 */ 363 public T getNormSq() { 364 // there are no cancellation problems here, so we use the straightforward formula 365 return x.multiply(x).add(y.multiply(y)).add(z.multiply(z)); 366 } 367 368 /** Get the L<sub>∞</sub> norm for the vector. 369 * @return L<sub>∞</sub> norm for the vector 370 */ 371 public T getNormInf() { 372 final T xAbs = x.abs(); 373 final T yAbs = y.abs(); 374 final T zAbs = z.abs(); 375 if (xAbs.getReal() <= yAbs.getReal()) { 376 if (yAbs.getReal() <= zAbs.getReal()) { 377 return zAbs; 378 } else { 379 return yAbs; 380 } 381 } else { 382 if (xAbs.getReal() <= zAbs.getReal()) { 383 return zAbs; 384 } else { 385 return xAbs; 386 } 387 } 388 } 389 390 /** Get the azimuth of the vector. 391 * @return azimuth (α) of the vector, between -π and +π 392 * @see #FieldVector3D(RealFieldElement, RealFieldElement) 393 */ 394 public T getAlpha() { 395 return y.atan2(x); 396 } 397 398 /** Get the elevation of the vector. 399 * @return elevation (δ) of the vector, between -π/2 and +π/2 400 * @see #FieldVector3D(RealFieldElement, RealFieldElement) 401 */ 402 public T getDelta() { 403 return z.divide(getNorm()).asin(); 404 } 405 406 /** Add a vector to the instance. 407 * @param v vector to add 408 * @return a new vector 409 */ 410 public FieldVector3D<T> add(final FieldVector3D<T> v) { 411 return new FieldVector3D<T>(x.add(v.x), y.add(v.y), z.add(v.z)); 412 } 413 414 /** Add a vector to the instance. 415 * @param v vector to add 416 * @return a new vector 417 */ 418 public FieldVector3D<T> add(final Vector3D v) { 419 return new FieldVector3D<T>(x.add(v.getX()), y.add(v.getY()), z.add(v.getZ())); 420 } 421 422 /** Add a scaled vector to the instance. 423 * @param factor scale factor to apply to v before adding it 424 * @param v vector to add 425 * @return a new vector 426 */ 427 public FieldVector3D<T> add(final T factor, final FieldVector3D<T> v) { 428 return new FieldVector3D<T>(x.getField().getOne(), this, factor, v); 429 } 430 431 /** Add a scaled vector to the instance. 432 * @param factor scale factor to apply to v before adding it 433 * @param v vector to add 434 * @return a new vector 435 */ 436 public FieldVector3D<T> add(final T factor, final Vector3D v) { 437 return new FieldVector3D<T>(x.add(factor.multiply(v.getX())), 438 y.add(factor.multiply(v.getY())), 439 z.add(factor.multiply(v.getZ()))); 440 } 441 442 /** Add a scaled vector to the instance. 443 * @param factor scale factor to apply to v before adding it 444 * @param v vector to add 445 * @return a new vector 446 */ 447 public FieldVector3D<T> add(final double factor, final FieldVector3D<T> v) { 448 return new FieldVector3D<T>(1.0, this, factor, v); 449 } 450 451 /** Add a scaled vector to the instance. 452 * @param factor scale factor to apply to v before adding it 453 * @param v vector to add 454 * @return a new vector 455 */ 456 public FieldVector3D<T> add(final double factor, final Vector3D v) { 457 return new FieldVector3D<T>(x.add(factor * v.getX()), 458 y.add(factor * v.getY()), 459 z.add(factor * v.getZ())); 460 } 461 462 /** Subtract a vector from the instance. 463 * @param v vector to subtract 464 * @return a new vector 465 */ 466 public FieldVector3D<T> subtract(final FieldVector3D<T> v) { 467 return new FieldVector3D<T>(x.subtract(v.x), y.subtract(v.y), z.subtract(v.z)); 468 } 469 470 /** Subtract a vector from the instance. 471 * @param v vector to subtract 472 * @return a new vector 473 */ 474 public FieldVector3D<T> subtract(final Vector3D v) { 475 return new FieldVector3D<T>(x.subtract(v.getX()), y.subtract(v.getY()), z.subtract(v.getZ())); 476 } 477 478 /** Subtract a scaled vector from the instance. 479 * @param factor scale factor to apply to v before subtracting it 480 * @param v vector to subtract 481 * @return a new vector 482 */ 483 public FieldVector3D<T> subtract(final T factor, final FieldVector3D<T> v) { 484 return new FieldVector3D<T>(x.getField().getOne(), this, factor.negate(), v); 485 } 486 487 /** Subtract a scaled vector from the instance. 488 * @param factor scale factor to apply to v before subtracting it 489 * @param v vector to subtract 490 * @return a new vector 491 */ 492 public FieldVector3D<T> subtract(final T factor, final Vector3D v) { 493 return new FieldVector3D<T>(x.subtract(factor.multiply(v.getX())), 494 y.subtract(factor.multiply(v.getY())), 495 z.subtract(factor.multiply(v.getZ()))); 496 } 497 498 /** Subtract a scaled vector from the instance. 499 * @param factor scale factor to apply to v before subtracting it 500 * @param v vector to subtract 501 * @return a new vector 502 */ 503 public FieldVector3D<T> subtract(final double factor, final FieldVector3D<T> v) { 504 return new FieldVector3D<T>(1.0, this, -factor, v); 505 } 506 507 /** Subtract a scaled vector from the instance. 508 * @param factor scale factor to apply to v before subtracting it 509 * @param v vector to subtract 510 * @return a new vector 511 */ 512 public FieldVector3D<T> subtract(final double factor, final Vector3D v) { 513 return new FieldVector3D<T>(x.subtract(factor * v.getX()), 514 y.subtract(factor * v.getY()), 515 z.subtract(factor * v.getZ())); 516 } 517 518 /** Get a normalized vector aligned with the instance. 519 * @return a new normalized vector 520 * @exception MathArithmeticException if the norm is zero 521 */ 522 public FieldVector3D<T> normalize() throws MathArithmeticException { 523 final T s = getNorm(); 524 if (s.getReal() == 0) { 525 throw new MathArithmeticException(LocalizedFormats.CANNOT_NORMALIZE_A_ZERO_NORM_VECTOR); 526 } 527 return scalarMultiply(s.reciprocal()); 528 } 529 530 /** Get a vector orthogonal to the instance. 531 * <p>There are an infinite number of normalized vectors orthogonal 532 * to the instance. This method picks up one of them almost 533 * arbitrarily. It is useful when one needs to compute a reference 534 * frame with one of the axes in a predefined direction. The 535 * following example shows how to build a frame having the k axis 536 * aligned with the known vector u : 537 * <pre><code> 538 * Vector3D k = u.normalize(); 539 * Vector3D i = k.orthogonal(); 540 * Vector3D j = Vector3D.crossProduct(k, i); 541 * </code></pre></p> 542 * @return a new normalized vector orthogonal to the instance 543 * @exception MathArithmeticException if the norm of the instance is null 544 */ 545 public FieldVector3D<T> orthogonal() throws MathArithmeticException { 546 547 final double threshold = 0.6 * getNorm().getReal(); 548 if (threshold == 0) { 549 throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); 550 } 551 552 if (FastMath.abs(x.getReal()) <= threshold) { 553 final T inverse = y.multiply(y).add(z.multiply(z)).sqrt().reciprocal(); 554 return new FieldVector3D<T>(inverse.getField().getZero(), inverse.multiply(z), inverse.multiply(y).negate()); 555 } else if (FastMath.abs(y.getReal()) <= threshold) { 556 final T inverse = x.multiply(x).add(z.multiply(z)).sqrt().reciprocal(); 557 return new FieldVector3D<T>(inverse.multiply(z).negate(), inverse.getField().getZero(), inverse.multiply(x)); 558 } else { 559 final T inverse = x.multiply(x).add(y.multiply(y)).sqrt().reciprocal(); 560 return new FieldVector3D<T>(inverse.multiply(y), inverse.multiply(x).negate(), inverse.getField().getZero()); 561 } 562 563 } 564 565 /** Compute the angular separation between two vectors. 566 * <p>This method computes the angular separation between two 567 * vectors using the dot product for well separated vectors and the 568 * cross product for almost aligned vectors. This allows to have a 569 * good accuracy in all cases, even for vectors very close to each 570 * other.</p> 571 * @param v1 first vector 572 * @param v2 second vector 573 * @param <T> the type of the field elements 574 * @return angular separation between v1 and v2 575 * @exception MathArithmeticException if either vector has a null norm 576 */ 577 public static <T extends RealFieldElement<T>> T angle(final FieldVector3D<T> v1, final FieldVector3D<T> v2) 578 throws MathArithmeticException { 579 580 final T normProduct = v1.getNorm().multiply(v2.getNorm()); 581 if (normProduct.getReal() == 0) { 582 throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); 583 } 584 585 final T dot = dotProduct(v1, v2); 586 final double threshold = normProduct.getReal() * 0.9999; 587 if ((dot.getReal() < -threshold) || (dot.getReal() > threshold)) { 588 // the vectors are almost aligned, compute using the sine 589 FieldVector3D<T> v3 = crossProduct(v1, v2); 590 if (dot.getReal() >= 0) { 591 return v3.getNorm().divide(normProduct).asin(); 592 } 593 return v3.getNorm().divide(normProduct).asin().subtract(FastMath.PI).negate(); 594 } 595 596 // the vectors are sufficiently separated to use the cosine 597 return dot.divide(normProduct).acos(); 598 599 } 600 601 /** Compute the angular separation between two vectors. 602 * <p>This method computes the angular separation between two 603 * vectors using the dot product for well separated vectors and the 604 * cross product for almost aligned vectors. This allows to have a 605 * good accuracy in all cases, even for vectors very close to each 606 * other.</p> 607 * @param v1 first vector 608 * @param v2 second vector 609 * @param <T> the type of the field elements 610 * @return angular separation between v1 and v2 611 * @exception MathArithmeticException if either vector has a null norm 612 */ 613 public static <T extends RealFieldElement<T>> T angle(final FieldVector3D<T> v1, final Vector3D v2) 614 throws MathArithmeticException { 615 616 final T normProduct = v1.getNorm().multiply(v2.getNorm()); 617 if (normProduct.getReal() == 0) { 618 throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); 619 } 620 621 final T dot = dotProduct(v1, v2); 622 final double threshold = normProduct.getReal() * 0.9999; 623 if ((dot.getReal() < -threshold) || (dot.getReal() > threshold)) { 624 // the vectors are almost aligned, compute using the sine 625 FieldVector3D<T> v3 = crossProduct(v1, v2); 626 if (dot.getReal() >= 0) { 627 return v3.getNorm().divide(normProduct).asin(); 628 } 629 return v3.getNorm().divide(normProduct).asin().subtract(FastMath.PI).negate(); 630 } 631 632 // the vectors are sufficiently separated to use the cosine 633 return dot.divide(normProduct).acos(); 634 635 } 636 637 /** Compute the angular separation between two vectors. 638 * <p>This method computes the angular separation between two 639 * vectors using the dot product for well separated vectors and the 640 * cross product for almost aligned vectors. This allows to have a 641 * good accuracy in all cases, even for vectors very close to each 642 * other.</p> 643 * @param v1 first vector 644 * @param v2 second vector 645 * @param <T> the type of the field elements 646 * @return angular separation between v1 and v2 647 * @exception MathArithmeticException if either vector has a null norm 648 */ 649 public static <T extends RealFieldElement<T>> T angle(final Vector3D v1, final FieldVector3D<T> v2) 650 throws MathArithmeticException { 651 return angle(v2, v1); 652 } 653 654 /** Get the opposite of the instance. 655 * @return a new vector which is opposite to the instance 656 */ 657 public FieldVector3D<T> negate() { 658 return new FieldVector3D<T>(x.negate(), y.negate(), z.negate()); 659 } 660 661 /** Multiply the instance by a scalar. 662 * @param a scalar 663 * @return a new vector 664 */ 665 public FieldVector3D<T> scalarMultiply(final T a) { 666 return new FieldVector3D<T>(x.multiply(a), y.multiply(a), z.multiply(a)); 667 } 668 669 /** Multiply the instance by a scalar. 670 * @param a scalar 671 * @return a new vector 672 */ 673 public FieldVector3D<T> scalarMultiply(final double a) { 674 return new FieldVector3D<T>(x.multiply(a), y.multiply(a), z.multiply(a)); 675 } 676 677 /** 678 * Returns true if any coordinate of this vector is NaN; false otherwise 679 * @return true if any coordinate of this vector is NaN; false otherwise 680 */ 681 public boolean isNaN() { 682 return Double.isNaN(x.getReal()) || Double.isNaN(y.getReal()) || Double.isNaN(z.getReal()); 683 } 684 685 /** 686 * Returns true if any coordinate of this vector is infinite and none are NaN; 687 * false otherwise 688 * @return true if any coordinate of this vector is infinite and none are NaN; 689 * false otherwise 690 */ 691 public boolean isInfinite() { 692 return !isNaN() && (Double.isInfinite(x.getReal()) || Double.isInfinite(y.getReal()) || Double.isInfinite(z.getReal())); 693 } 694 695 /** 696 * Test for the equality of two 3D vectors. 697 * <p> 698 * If all coordinates of two 3D vectors are exactly the same, and none of their 699 * {@link RealFieldElement#getReal() real part} are <code>NaN</code>, the 700 * two 3D vectors are considered to be equal. 701 * </p> 702 * <p> 703 * <code>NaN</code> coordinates are considered to affect globally the vector 704 * and be equals to each other - i.e, if either (or all) real part of the 705 * coordinates of the 3D vector are <code>NaN</code>, the 3D vector is <code>NaN</code>. 706 * </p> 707 * 708 * @param other Object to test for equality to this 709 * @return true if two 3D vector objects are equal, false if 710 * object is null, not an instance of Vector3D, or 711 * not equal to this Vector3D instance 712 * 713 */ 714 @Override 715 public boolean equals(Object other) { 716 717 if (this == other) { 718 return true; 719 } 720 721 if (other instanceof FieldVector3D) { 722 @SuppressWarnings("unchecked") 723 final FieldVector3D<T> rhs = (FieldVector3D<T>) other; 724 if (rhs.isNaN()) { 725 return this.isNaN(); 726 } 727 728 return x.equals(rhs.x) && y.equals(rhs.y) && z.equals(rhs.z); 729 730 } 731 return false; 732 } 733 734 /** 735 * Get a hashCode for the 3D vector. 736 * <p> 737 * All NaN values have the same hash code.</p> 738 * 739 * @return a hash code value for this object 740 */ 741 @Override 742 public int hashCode() { 743 if (isNaN()) { 744 return 409; 745 } 746 return 311 * (107 * x.hashCode() + 83 * y.hashCode() + z.hashCode()); 747 } 748 749 /** Compute the dot-product of the instance and another vector. 750 * <p> 751 * The implementation uses specific multiplication and addition 752 * algorithms to preserve accuracy and reduce cancellation effects. 753 * It should be very accurate even for nearly orthogonal vectors. 754 * </p> 755 * @see MathArrays#linearCombination(double, double, double, double, double, double) 756 * @param v second vector 757 * @return the dot product this.v 758 */ 759 public T dotProduct(final FieldVector3D<T> v) { 760 return x.linearCombination(x, v.x, y, v.y, z, v.z); 761 } 762 763 /** Compute the dot-product of the instance and another vector. 764 * <p> 765 * The implementation uses specific multiplication and addition 766 * algorithms to preserve accuracy and reduce cancellation effects. 767 * It should be very accurate even for nearly orthogonal vectors. 768 * </p> 769 * @see MathArrays#linearCombination(double, double, double, double, double, double) 770 * @param v second vector 771 * @return the dot product this.v 772 */ 773 public T dotProduct(final Vector3D v) { 774 return x.linearCombination(v.getX(), x, v.getY(), y, v.getZ(), z); 775 } 776 777 /** Compute the cross-product of the instance with another vector. 778 * @param v other vector 779 * @return the cross product this ^ v as a new Vector3D 780 */ 781 public FieldVector3D<T> crossProduct(final FieldVector3D<T> v) { 782 return new FieldVector3D<T>(x.linearCombination(y, v.z, z.negate(), v.y), 783 y.linearCombination(z, v.x, x.negate(), v.z), 784 z.linearCombination(x, v.y, y.negate(), v.x)); 785 } 786 787 /** Compute the cross-product of the instance with another vector. 788 * @param v other vector 789 * @return the cross product this ^ v as a new Vector3D 790 */ 791 public FieldVector3D<T> crossProduct(final Vector3D v) { 792 return new FieldVector3D<T>(x.linearCombination(v.getZ(), y, -v.getY(), z), 793 y.linearCombination(v.getX(), z, -v.getZ(), x), 794 z.linearCombination(v.getY(), x, -v.getX(), y)); 795 } 796 797 /** Compute the distance between the instance and another vector according to the L<sub>1</sub> norm. 798 * <p>Calling this method is equivalent to calling: 799 * <code>q.subtract(p).getNorm1()</code> except that no intermediate 800 * vector is built</p> 801 * @param v second vector 802 * @return the distance between the instance and p according to the L<sub>1</sub> norm 803 */ 804 public T distance1(final FieldVector3D<T> v) { 805 final T dx = v.x.subtract(x).abs(); 806 final T dy = v.y.subtract(y).abs(); 807 final T dz = v.z.subtract(z).abs(); 808 return dx.add(dy).add(dz); 809 } 810 811 /** Compute the distance between the instance and another vector according to the L<sub>1</sub> norm. 812 * <p>Calling this method is equivalent to calling: 813 * <code>q.subtract(p).getNorm1()</code> except that no intermediate 814 * vector is built</p> 815 * @param v second vector 816 * @return the distance between the instance and p according to the L<sub>1</sub> norm 817 */ 818 public T distance1(final Vector3D v) { 819 final T dx = x.subtract(v.getX()).abs(); 820 final T dy = y.subtract(v.getY()).abs(); 821 final T dz = z.subtract(v.getZ()).abs(); 822 return dx.add(dy).add(dz); 823 } 824 825 /** Compute the distance between the instance and another vector according to the L<sub>2</sub> norm. 826 * <p>Calling this method is equivalent to calling: 827 * <code>q.subtract(p).getNorm()</code> except that no intermediate 828 * vector is built</p> 829 * @param v second vector 830 * @return the distance between the instance and p according to the L<sub>2</sub> norm 831 */ 832 public T distance(final FieldVector3D<T> v) { 833 final T dx = v.x.subtract(x); 834 final T dy = v.y.subtract(y); 835 final T dz = v.z.subtract(z); 836 return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)).sqrt(); 837 } 838 839 /** Compute the distance between the instance and another vector according to the L<sub>2</sub> norm. 840 * <p>Calling this method is equivalent to calling: 841 * <code>q.subtract(p).getNorm()</code> except that no intermediate 842 * vector is built</p> 843 * @param v second vector 844 * @return the distance between the instance and p according to the L<sub>2</sub> norm 845 */ 846 public T distance(final Vector3D v) { 847 final T dx = x.subtract(v.getX()); 848 final T dy = y.subtract(v.getY()); 849 final T dz = z.subtract(v.getZ()); 850 return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)).sqrt(); 851 } 852 853 /** Compute the distance between the instance and another vector according to the L<sub>∞</sub> norm. 854 * <p>Calling this method is equivalent to calling: 855 * <code>q.subtract(p).getNormInf()</code> except that no intermediate 856 * vector is built</p> 857 * @param v second vector 858 * @return the distance between the instance and p according to the L<sub>∞</sub> norm 859 */ 860 public T distanceInf(final FieldVector3D<T> v) { 861 final T dx = v.x.subtract(x).abs(); 862 final T dy = v.y.subtract(y).abs(); 863 final T dz = v.z.subtract(z).abs(); 864 if (dx.getReal() <= dy.getReal()) { 865 if (dy.getReal() <= dz.getReal()) { 866 return dz; 867 } else { 868 return dy; 869 } 870 } else { 871 if (dx.getReal() <= dz.getReal()) { 872 return dz; 873 } else { 874 return dx; 875 } 876 } 877 } 878 879 /** Compute the distance between the instance and another vector according to the L<sub>∞</sub> norm. 880 * <p>Calling this method is equivalent to calling: 881 * <code>q.subtract(p).getNormInf()</code> except that no intermediate 882 * vector is built</p> 883 * @param v second vector 884 * @return the distance between the instance and p according to the L<sub>∞</sub> norm 885 */ 886 public T distanceInf(final Vector3D v) { 887 final T dx = x.subtract(v.getX()).abs(); 888 final T dy = y.subtract(v.getY()).abs(); 889 final T dz = z.subtract(v.getZ()).abs(); 890 if (dx.getReal() <= dy.getReal()) { 891 if (dy.getReal() <= dz.getReal()) { 892 return dz; 893 } else { 894 return dy; 895 } 896 } else { 897 if (dx.getReal() <= dz.getReal()) { 898 return dz; 899 } else { 900 return dx; 901 } 902 } 903 } 904 905 /** Compute the square of the distance between the instance and another vector. 906 * <p>Calling this method is equivalent to calling: 907 * <code>q.subtract(p).getNormSq()</code> except that no intermediate 908 * vector is built</p> 909 * @param v second vector 910 * @return the square of the distance between the instance and p 911 */ 912 public T distanceSq(final FieldVector3D<T> v) { 913 final T dx = v.x.subtract(x); 914 final T dy = v.y.subtract(y); 915 final T dz = v.z.subtract(z); 916 return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)); 917 } 918 919 /** Compute the square of the distance between the instance and another vector. 920 * <p>Calling this method is equivalent to calling: 921 * <code>q.subtract(p).getNormSq()</code> except that no intermediate 922 * vector is built</p> 923 * @param v second vector 924 * @return the square of the distance between the instance and p 925 */ 926 public T distanceSq(final Vector3D v) { 927 final T dx = x.subtract(v.getX()); 928 final T dy = y.subtract(v.getY()); 929 final T dz = z.subtract(v.getZ()); 930 return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)); 931 } 932 933 /** Compute the dot-product of two vectors. 934 * @param v1 first vector 935 * @param v2 second vector 936 * @param <T> the type of the field elements 937 * @return the dot product v1.v2 938 */ 939 public static <T extends RealFieldElement<T>> T dotProduct(final FieldVector3D<T> v1, 940 final FieldVector3D<T> v2) { 941 return v1.dotProduct(v2); 942 } 943 944 /** Compute the dot-product of two vectors. 945 * @param v1 first vector 946 * @param v2 second vector 947 * @param <T> the type of the field elements 948 * @return the dot product v1.v2 949 */ 950 public static <T extends RealFieldElement<T>> T dotProduct(final FieldVector3D<T> v1, 951 final Vector3D v2) { 952 return v1.dotProduct(v2); 953 } 954 955 /** Compute the dot-product of two vectors. 956 * @param v1 first vector 957 * @param v2 second vector 958 * @param <T> the type of the field elements 959 * @return the dot product v1.v2 960 */ 961 public static <T extends RealFieldElement<T>> T dotProduct(final Vector3D v1, 962 final FieldVector3D<T> v2) { 963 return v2.dotProduct(v1); 964 } 965 966 /** Compute the cross-product of two vectors. 967 * @param v1 first vector 968 * @param v2 second vector 969 * @param <T> the type of the field elements 970 * @return the cross product v1 ^ v2 as a new Vector 971 */ 972 public static <T extends RealFieldElement<T>> FieldVector3D<T> crossProduct(final FieldVector3D<T> v1, 973 final FieldVector3D<T> v2) { 974 return v1.crossProduct(v2); 975 } 976 977 /** Compute the cross-product of two vectors. 978 * @param v1 first vector 979 * @param v2 second vector 980 * @param <T> the type of the field elements 981 * @return the cross product v1 ^ v2 as a new Vector 982 */ 983 public static <T extends RealFieldElement<T>> FieldVector3D<T> crossProduct(final FieldVector3D<T> v1, 984 final Vector3D v2) { 985 return v1.crossProduct(v2); 986 } 987 988 /** Compute the cross-product of two vectors. 989 * @param v1 first vector 990 * @param v2 second vector 991 * @param <T> the type of the field elements 992 * @return the cross product v1 ^ v2 as a new Vector 993 */ 994 public static <T extends RealFieldElement<T>> FieldVector3D<T> crossProduct(final Vector3D v1, 995 final FieldVector3D<T> v2) { 996 return new FieldVector3D<T>(v2.x.linearCombination(v1.getY(), v2.z, -v1.getZ(), v2.y), 997 v2.y.linearCombination(v1.getZ(), v2.x, -v1.getX(), v2.z), 998 v2.z.linearCombination(v1.getX(), v2.y, -v1.getY(), v2.x)); 999 } 1000 1001 /** Compute the distance between two vectors according to the L<sub>1</sub> norm. 1002 * <p>Calling this method is equivalent to calling: 1003 * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate 1004 * vector is built</p> 1005 * @param v1 first vector 1006 * @param v2 second vector 1007 * @param <T> the type of the field elements 1008 * @return the distance between v1 and v2 according to the L<sub>1</sub> norm 1009 */ 1010 public static <T extends RealFieldElement<T>> T distance1(final FieldVector3D<T> v1, 1011 final FieldVector3D<T> v2) { 1012 return v1.distance1(v2); 1013 } 1014 1015 /** Compute the distance between two vectors according to the L<sub>1</sub> norm. 1016 * <p>Calling this method is equivalent to calling: 1017 * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate 1018 * vector is built</p> 1019 * @param v1 first vector 1020 * @param v2 second vector 1021 * @param <T> the type of the field elements 1022 * @return the distance between v1 and v2 according to the L<sub>1</sub> norm 1023 */ 1024 public static <T extends RealFieldElement<T>> T distance1(final FieldVector3D<T> v1, 1025 final Vector3D v2) { 1026 return v1.distance1(v2); 1027 } 1028 1029 /** Compute the distance between two vectors according to the L<sub>1</sub> norm. 1030 * <p>Calling this method is equivalent to calling: 1031 * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate 1032 * vector is built</p> 1033 * @param v1 first vector 1034 * @param v2 second vector 1035 * @param <T> the type of the field elements 1036 * @return the distance between v1 and v2 according to the L<sub>1</sub> norm 1037 */ 1038 public static <T extends RealFieldElement<T>> T distance1(final Vector3D v1, 1039 final FieldVector3D<T> v2) { 1040 return v2.distance1(v1); 1041 } 1042 1043 /** Compute the distance between two vectors according to the L<sub>2</sub> norm. 1044 * <p>Calling this method is equivalent to calling: 1045 * <code>v1.subtract(v2).getNorm()</code> except that no intermediate 1046 * vector is built</p> 1047 * @param v1 first vector 1048 * @param v2 second vector 1049 * @param <T> the type of the field elements 1050 * @return the distance between v1 and v2 according to the L<sub>2</sub> norm 1051 */ 1052 public static <T extends RealFieldElement<T>> T distance(final FieldVector3D<T> v1, 1053 final FieldVector3D<T> v2) { 1054 return v1.distance(v2); 1055 } 1056 1057 /** Compute the distance between two vectors according to the L<sub>2</sub> norm. 1058 * <p>Calling this method is equivalent to calling: 1059 * <code>v1.subtract(v2).getNorm()</code> except that no intermediate 1060 * vector is built</p> 1061 * @param v1 first vector 1062 * @param v2 second vector 1063 * @param <T> the type of the field elements 1064 * @return the distance between v1 and v2 according to the L<sub>2</sub> norm 1065 */ 1066 public static <T extends RealFieldElement<T>> T distance(final FieldVector3D<T> v1, 1067 final Vector3D v2) { 1068 return v1.distance(v2); 1069 } 1070 1071 /** Compute the distance between two vectors according to the L<sub>2</sub> norm. 1072 * <p>Calling this method is equivalent to calling: 1073 * <code>v1.subtract(v2).getNorm()</code> except that no intermediate 1074 * vector is built</p> 1075 * @param v1 first vector 1076 * @param v2 second vector 1077 * @param <T> the type of the field elements 1078 * @return the distance between v1 and v2 according to the L<sub>2</sub> norm 1079 */ 1080 public static <T extends RealFieldElement<T>> T distance(final Vector3D v1, 1081 final FieldVector3D<T> v2) { 1082 return v2.distance(v1); 1083 } 1084 1085 /** Compute the distance between two vectors according to the L<sub>∞</sub> norm. 1086 * <p>Calling this method is equivalent to calling: 1087 * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate 1088 * vector is built</p> 1089 * @param v1 first vector 1090 * @param v2 second vector 1091 * @param <T> the type of the field elements 1092 * @return the distance between v1 and v2 according to the L<sub>∞</sub> norm 1093 */ 1094 public static <T extends RealFieldElement<T>> T distanceInf(final FieldVector3D<T> v1, 1095 final FieldVector3D<T> v2) { 1096 return v1.distanceInf(v2); 1097 } 1098 1099 /** Compute the distance between two vectors according to the L<sub>∞</sub> norm. 1100 * <p>Calling this method is equivalent to calling: 1101 * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate 1102 * vector is built</p> 1103 * @param v1 first vector 1104 * @param v2 second vector 1105 * @param <T> the type of the field elements 1106 * @return the distance between v1 and v2 according to the L<sub>∞</sub> norm 1107 */ 1108 public static <T extends RealFieldElement<T>> T distanceInf(final FieldVector3D<T> v1, 1109 final Vector3D v2) { 1110 return v1.distanceInf(v2); 1111 } 1112 1113 /** Compute the distance between two vectors according to the L<sub>∞</sub> norm. 1114 * <p>Calling this method is equivalent to calling: 1115 * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate 1116 * vector is built</p> 1117 * @param v1 first vector 1118 * @param v2 second vector 1119 * @param <T> the type of the field elements 1120 * @return the distance between v1 and v2 according to the L<sub>∞</sub> norm 1121 */ 1122 public static <T extends RealFieldElement<T>> T distanceInf(final Vector3D v1, 1123 final FieldVector3D<T> v2) { 1124 return v2.distanceInf(v1); 1125 } 1126 1127 /** Compute the square of the distance between two vectors. 1128 * <p>Calling this method is equivalent to calling: 1129 * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate 1130 * vector is built</p> 1131 * @param v1 first vector 1132 * @param v2 second vector 1133 * @param <T> the type of the field elements 1134 * @return the square of the distance between v1 and v2 1135 */ 1136 public static <T extends RealFieldElement<T>> T distanceSq(final FieldVector3D<T> v1, 1137 final FieldVector3D<T> v2) { 1138 return v1.distanceSq(v2); 1139 } 1140 1141 /** Compute the square of the distance between two vectors. 1142 * <p>Calling this method is equivalent to calling: 1143 * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate 1144 * vector is built</p> 1145 * @param v1 first vector 1146 * @param v2 second vector 1147 * @param <T> the type of the field elements 1148 * @return the square of the distance between v1 and v2 1149 */ 1150 public static <T extends RealFieldElement<T>> T distanceSq(final FieldVector3D<T> v1, 1151 final Vector3D v2) { 1152 return v1.distanceSq(v2); 1153 } 1154 1155 /** Compute the square of the distance between two vectors. 1156 * <p>Calling this method is equivalent to calling: 1157 * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate 1158 * vector is built</p> 1159 * @param v1 first vector 1160 * @param v2 second vector 1161 * @param <T> the type of the field elements 1162 * @return the square of the distance between v1 and v2 1163 */ 1164 public static <T extends RealFieldElement<T>> T distanceSq(final Vector3D v1, 1165 final FieldVector3D<T> v2) { 1166 return v2.distanceSq(v1); 1167 } 1168 1169 /** Get a string representation of this vector. 1170 * @return a string representation of this vector 1171 */ 1172 @Override 1173 public String toString() { 1174 return Vector3DFormat.getInstance().format(toVector3D()); 1175 } 1176 1177 /** Get a string representation of this vector. 1178 * @param format the custom format for components 1179 * @return a string representation of this vector 1180 */ 1181 public String toString(final NumberFormat format) { 1182 return new Vector3DFormat(format).format(toVector3D()); 1183 } 1184 1185}