1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * The ASF licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.apache.commons.geometry.euclidean.threed;
18
19 import java.util.Arrays;
20 import java.util.Comparator;
21 import java.util.Iterator;
22 import java.util.function.UnaryOperator;
23
24 import org.apache.commons.geometry.core.internal.DoubleFunction3N;
25 import org.apache.commons.geometry.core.internal.SimpleTupleFormat;
26 import org.apache.commons.geometry.euclidean.EuclideanVectorSum;
27 import org.apache.commons.geometry.euclidean.MultiDimensionalEuclideanVector;
28 import org.apache.commons.geometry.euclidean.internal.Vectors;
29 import org.apache.commons.numbers.core.Precision;
30
31 /** This class represents vectors and points in three-dimensional Euclidean space.
32 * Instances of this class are guaranteed to be immutable.
33 */
34 public class Vector3D extends MultiDimensionalEuclideanVector<Vector3D> {
35
36 /** Zero (null) vector (coordinates: 0, 0, 0). */
37 public static final Vector3D ZERO = new Vector3D(0, 0, 0);
38
39 /** A vector with all coordinates set to NaN. */
40 public static final Vector3D NaN = new Vector3D(Double.NaN, Double.NaN, Double.NaN);
41
42 /** A vector with all coordinates set to positive infinity. */
43 public static final Vector3D POSITIVE_INFINITY =
44 new Vector3D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
45
46 /** A vector with all coordinates set to negative infinity. */
47 public static final Vector3D NEGATIVE_INFINITY =
48 new Vector3D(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY);
49
50 /** Comparator that sorts vectors in component-wise ascending order.
51 * Vectors are only considered equal if their coordinates match exactly.
52 * Null arguments are evaluated as being greater than non-null arguments.
53 */
54 public static final Comparator<Vector3D> COORDINATE_ASCENDING_ORDER = (a, b) -> {
55 int cmp = 0;
56
57 if (a != null && b != null) {
58 cmp = Double.compare(a.getX(), b.getX());
59 if (cmp == 0) {
60 cmp = Double.compare(a.getY(), b.getY());
61 if (cmp == 0) {
62 cmp = Double.compare(a.getZ(), b.getZ());
63 }
64 }
65 } else if (a != null) {
66 cmp = -1;
67 } else if (b != null) {
68 cmp = 1;
69 }
70
71 return cmp;
72 };
73
74 /** X coordinate value (abscissa). */
75 private final double x;
76
77 /** Y coordinate value (ordinate). */
78 private final double y;
79
80 /** Z coordinate value (height). */
81 private final double z;
82
83 /** Simple constructor.
84 * Build a vector from its coordinates
85 * @param x x coordinate value
86 * @param y y coordinate value
87 * @param z z coordinate value
88 */
89 private Vector3D(final double x, final double y, final double z) {
90 this.x = x;
91 this.y = y;
92 this.z = z;
93 }
94
95 /** Return the x coordinate value (abscissa) of the instance.
96 * @return the x coordinate value
97 */
98 public double getX() {
99 return x;
100 }
101
102 /** Return the y coordinate value (ordinate) of the instance.
103 * @return the y coordinate value
104 */
105 public double getY() {
106 return y;
107 }
108
109 /** Returns the z coordinate value (height) of the instance.
110 * @return the z coordinate value
111 */
112 public double getZ() {
113 return z;
114 }
115
116 /** Get the coordinates for this instance as a dimension 3 array.
117 * @return the coordinates for this instance
118 */
119 public double[] toArray() {
120 return new double[]{x, y, z};
121 }
122
123 /** {@inheritDoc} */
124 @Override
125 public int getDimension() {
126 return 3;
127 }
128
129 /** {@inheritDoc} */
130 @Override
131 public boolean isNaN() {
132 return Double.isNaN(x) || Double.isNaN(y) || Double.isNaN(z);
133 }
134
135 /** {@inheritDoc} */
136 @Override
137 public boolean isInfinite() {
138 return !isNaN() && (Double.isInfinite(x) || Double.isInfinite(y) || Double.isInfinite(z));
139 }
140
141 /** {@inheritDoc} */
142 @Override
143 public boolean isFinite() {
144 return Double.isFinite(x) && Double.isFinite(y) && Double.isFinite(z);
145 }
146
147 /** {@inheritDoc} */
148 @Override
149 public Vector3D getZero() {
150 return ZERO;
151 }
152
153 /** {@inheritDoc} */
154 @Override
155 public Vector3D vectorTo(final Vector3D v) {
156 return v.subtract(this);
157 }
158
159 /** {@inheritDoc} */
160 @Override
161 public Unit directionTo(final Vector3D v) {
162 return vectorTo(v).normalize();
163 }
164
165 /** {@inheritDoc} */
166 @Override
167 public Vector3D lerp(final Vector3D p, final double t) {
168 return Sum.create()
169 .addScaled(1.0 - t, this)
170 .addScaled(t, p).get();
171 }
172
173 /** {@inheritDoc} */
174 @Override
175 public double norm() {
176 return Vectors.norm(x, y, z);
177 }
178
179 /** {@inheritDoc} */
180 @Override
181 public double normSq() {
182 return Vectors.normSq(x, y, z);
183 }
184
185 /** {@inheritDoc} */
186 @Override
187 public Vector3D withNorm(final double magnitude) {
188 final double m = magnitude / getCheckedNorm();
189
190 return new Vector3D(
191 m * x,
192 m * y,
193 m * z
194 );
195 }
196
197 /** {@inheritDoc} */
198 @Override
199 public Vector3D add(final Vector3D v) {
200 return new Vector3D(
201 x + v.x,
202 y + v.y,
203 z + v.z
204 );
205 }
206
207 /** {@inheritDoc} */
208 @Override
209 public Vector3D add(final double factor, final Vector3D v) {
210 return new Vector3D(
211 x + (factor * v.x),
212 y + (factor * v.y),
213 z + (factor * v.z)
214 );
215 }
216
217 /** {@inheritDoc} */
218 @Override
219 public Vector3D subtract(final Vector3D v) {
220 return new Vector3D(
221 x - v.x,
222 y - v.y,
223 z - v.z
224 );
225 }
226
227 /** {@inheritDoc} */
228 @Override
229 public Vector3D subtract(final double factor, final Vector3D v) {
230 return new Vector3D(
231 x - (factor * v.x),
232 y - (factor * v.y),
233 z - (factor * v.z)
234 );
235 }
236
237 /** {@inheritDoc} */
238 @Override
239 public Vector3D negate() {
240 return new Vector3D(-x, -y, -z);
241 }
242
243 /** {@inheritDoc} */
244 @Override
245 public Unit normalize() {
246 return Unit.from(x, y, z);
247 }
248
249 /** {@inheritDoc} */
250 @Override
251 public Unit normalizeOrNull() {
252 return Unit.tryCreateNormalized(x, y, z, false);
253 }
254
255 /** {@inheritDoc} */
256 @Override
257 public Vector3D multiply(final double a) {
258 return new Vector3D(a * x, a * y, a * z);
259 }
260
261 /** {@inheritDoc} */
262 @Override
263 public double distance(final Vector3D v) {
264 return Vectors.norm(
265 x - v.x,
266 y - v.y,
267 z - v.z
268 );
269 }
270
271 /** {@inheritDoc} */
272 @Override
273 public double distanceSq(final Vector3D v) {
274 return Vectors.normSq(
275 x - v.x,
276 y - v.y,
277 z - v.z
278 );
279 }
280
281 /** {@inheritDoc}
282 * <p>
283 * The implementation uses specific multiplication and addition
284 * algorithms to preserve accuracy and reduce cancellation effects.
285 * It should be very accurate even for nearly orthogonal vectors.
286 * </p>
287 * @see org.apache.commons.numbers.core.Sum
288 */
289 @Override
290 public double dot(final Vector3D v) {
291 return Vectors.linearCombination(
292 x, v.x,
293 y, v.y,
294 z, v.z);
295 }
296
297 /** {@inheritDoc}
298 * <p>This method computes the angular separation between two
299 * vectors using the dot product for well separated vectors and the
300 * cross product for almost aligned vectors. This allows to have a
301 * good accuracy in all cases, even for vectors very close to each
302 * other.</p>
303 */
304 @Override
305 public double angle(final Vector3D v) {
306 final double normProduct = getCheckedNorm() * v.getCheckedNorm();
307
308 final double dot = dot(v);
309 final double threshold = normProduct * 0.99;
310 if ((dot < -threshold) || (dot > threshold)) {
311 // the vectors are almost aligned, compute using the sine
312 final Vector3D cross = cross(v);
313 if (dot >= 0) {
314 return Math.asin(cross.norm() / normProduct);
315 }
316 return Math.PI - Math.asin(cross.norm() / normProduct);
317 }
318
319 // the vectors are sufficiently separated to use the cosine
320 return Math.acos(dot / normProduct);
321 }
322
323 /** {@inheritDoc} */
324 @Override
325 public Vector3D project(final Vector3D base) {
326 return getComponent(base, false, Vector3D::new);
327 }
328
329 /** {@inheritDoc} */
330 @Override
331 public Vector3D reject(final Vector3D base) {
332 return getComponent(base, true, Vector3D::new);
333 }
334
335 /** {@inheritDoc}
336 * <p>There are an infinite number of normalized vectors orthogonal
337 * to the instance. This method picks up one of them almost
338 * arbitrarily. It is useful when one needs to compute a reference
339 * frame with one of the axes in a predefined direction. The
340 * following example shows how to build a frame having the k axis
341 * aligned with the known vector u :
342 * <pre><code>
343 * Vector3D k = u.normalize();
344 * Vector3D i = k.orthogonal();
345 * Vector3D j = k.cross(i);
346 * </code></pre>
347 * @return a unit vector orthogonal to the instance
348 * @throws IllegalArgumentException if the norm of the instance
349 * is zero, NaN, or infinite
350 */
351 @Override
352 public Vector3D.Unit orthogonal() {
353 final double threshold = 0.6 * getCheckedNorm();
354
355 final double inverse;
356 if (Math.abs(x) <= threshold) {
357 inverse = 1 / Vectors.norm(y, z);
358 return new Unit(0, inverse * z, -inverse * y);
359 } else if (Math.abs(y) <= threshold) {
360 inverse = 1 / Vectors.norm(x, z);
361 return new Unit(-inverse * z, 0, inverse * x);
362 }
363 inverse = 1 / Vectors.norm(x, y);
364 return new Unit(inverse * y, -inverse * x, 0);
365 }
366
367 /** {@inheritDoc} */
368 @Override
369 public Vector3D.Unit orthogonal(final Vector3D dir) {
370 return dir.getComponent(this, true, Vector3D.Unit::from);
371 }
372
373 /** Compute the cross-product of the instance with another vector.
374 * @param v other vector
375 * @return the cross product this ^ v as a new Vector3D
376 */
377 public Vector3D cross(final Vector3D v) {
378 return new Vector3D(Vectors.linearCombination(y, v.z, -z, v.y),
379 Vectors.linearCombination(z, v.x, -x, v.z),
380 Vectors.linearCombination(x, v.y, -y, v.x));
381 }
382
383 /** Convenience method to apply a function to this vector. This
384 * can be used to transform the vector inline with other methods.
385 * @param fn the function to apply
386 * @return the transformed vector
387 */
388 public Vector3D transform(final UnaryOperator<Vector3D> fn) {
389 return fn.apply(this);
390 }
391
392 /** {@inheritDoc} */
393 @Override
394 public boolean eq(final Vector3D vec, final Precision.DoubleEquivalence precision) {
395 return precision.eq(x, vec.x) &&
396 precision.eq(y, vec.y) &&
397 precision.eq(z, vec.z);
398 }
399
400 /**
401 * Get a hashCode for the vector.
402 * <p>All NaN values have the same hash code.</p>
403 *
404 * @return a hash code value for this object
405 */
406 @Override
407 public int hashCode() {
408 if (isNaN()) {
409 return 642;
410 }
411 return 643 * (164 * Double.hashCode(x) + 3 * Double.hashCode(y) + Double.hashCode(z));
412 }
413
414 /**d
415 * Test for the equality of two vector instances.
416 * <p>
417 * If all coordinates of two vectors are exactly the same, and none are
418 * <code>Double.NaN</code>, the two instances are considered to be equal.
419 * </p>
420 * <p>
421 * <code>NaN</code> coordinates are considered to globally affect the vector
422 * and be equal to each other - i.e, if either (or all) coordinates of the
423 * vector are equal to <code>Double.NaN</code>, the vector is equal to
424 * {@link #NaN}.
425 * </p>
426 *
427 * @param other Object to test for equality to this
428 * @return true if two Vector3D objects are equal, false if
429 * object is null, not an instance of Vector3D, or
430 * not equal to this Vector3D instance
431 *
432 */
433 @Override
434 public boolean equals(final Object other) {
435 if (this == other) {
436 return true;
437 }
438 if (other instanceof Vector3D) {
439 final Vector3D rhs = (Vector3D) other;
440 if (rhs.isNaN()) {
441 return this.isNaN();
442 }
443
444 return Double.compare(x, rhs.x) == 0 &&
445 Double.compare(y, rhs.y) == 0 &&
446 Double.compare(z, rhs.z) == 0;
447 }
448 return false;
449 }
450
451 /** {@inheritDoc} */
452 @Override
453 public String toString() {
454 return SimpleTupleFormat.getDefault().format(x, y, z);
455 }
456
457 /** Returns a component of the current instance relative to the given base
458 * vector. If {@code reject} is true, the vector rejection is returned; otherwise,
459 * the projection is returned.
460 * @param base The base vector
461 * @param reject If true, the rejection of this instance from {@code base} is
462 * returned. If false, the projection of this instance onto {@code base}
463 * is returned.
464 * @param factory factory function used to build the final vector
465 * @param <V> Vector implementation type
466 * @return The projection or rejection of this instance relative to {@code base},
467 * depending on the value of {@code reject}.
468 * @throws IllegalArgumentException if {@code base} has a zero, NaN,
469 * or infinite norm
470 */
471 private <V extends Vector3D> V getComponent(final Vector3D base, final boolean reject,
472 final DoubleFunction3N<V> factory) {
473 final double aDotB = dot(base);
474
475 // We need to check the norm value here to ensure that it's legal. However, we don't
476 // want to incur the cost or floating point error of getting the actual norm and then
477 // multiplying it again to get the square norm. So, we'll just check the squared norm
478 // directly. This will produce the same error result as checking the actual norm since
479 // Math.sqrt(0.0) == 0.0, Math.sqrt(Double.NaN) == Double.NaN and
480 // Math.sqrt(Double.POSITIVE_INFINITY) == Double.POSITIVE_INFINITY.
481 final double baseMagSq = Vectors.checkedNorm(base.normSq());
482
483 final double scale = aDotB / baseMagSq;
484
485 final double projX = scale * base.x;
486 final double projY = scale * base.y;
487 final double projZ = scale * base.z;
488
489 if (reject) {
490 return factory.apply(x - projX, y - projY, z - projZ);
491 }
492
493 return factory.apply(projX, projY, projZ);
494 }
495
496 /** Returns a vector with the given coordinate values.
497 * @param x x coordinate value
498 * @param y y coordinate value
499 * @param z z coordinate value
500 * @return vector instance
501 */
502 public static Vector3D of(final double x, final double y, final double z) {
503 return new Vector3D(x, y, z);
504 }
505
506 /** Creates a vector from the coordinates in the given 3-element array.
507 * @param v coordinates array
508 * @return new vector
509 * @exception IllegalArgumentException if the array does not have 3 elements
510 */
511 public static Vector3D of(final double[] v) {
512 if (v.length != 3) {
513 throw new IllegalArgumentException("Dimension mismatch: " + v.length + " != 3");
514 }
515 return new Vector3D(v[0], v[1], v[2]);
516 }
517
518 /** Parses the given string and returns a new vector instance. The expected string
519 * format is the same as that returned by {@link #toString()}.
520 * @param str the string to parse
521 * @return vector instance represented by the string
522 * @throws IllegalArgumentException if the given string has an invalid format
523 */
524 public static Vector3D parse(final String str) {
525 return SimpleTupleFormat.getDefault().parse(str, Vector3D::new);
526 }
527
528 /** Return a vector containing the maximum component values from all input vectors.
529 * @param first first vector
530 * @param more additional vectors
531 * @return a vector containing the maximum component values from all input vectors
532 */
533 public static Vector3D max(final Vector3D first, final Vector3D... more) {
534 return computeMax(first, Arrays.asList(more).iterator());
535 }
536
537 /** Return a vector containing the maximum component values from all input vectors.
538 * @param vecs input vectors
539 * @return a vector containing the maximum component values from all input vectors
540 * @throws IllegalArgumentException if the argument does not contain any vectors
541 */
542 public static Vector3D max(final Iterable<Vector3D> vecs) {
543 final Iterator<Vector3D> it = vecs.iterator();
544 if (!it.hasNext()) {
545 throw new IllegalArgumentException("Cannot compute vector max: no vectors given");
546 }
547
548 return computeMax(it.next(), it);
549 }
550
551 /** Internal method for computing a max vector.
552 * @param first first vector
553 * @param more iterator with additional vectors
554 * @return vector containing the maximum component values of all input vectors
555 */
556 private static Vector3D computeMax(final Vector3D first, final Iterator<? extends Vector3D> more) {
557 double x = first.getX();
558 double y = first.getY();
559 double z = first.getZ();
560
561 Vector3D vec;
562 while (more.hasNext()) {
563 vec = more.next();
564
565 x = Math.max(x, vec.getX());
566 y = Math.max(y, vec.getY());
567 z = Math.max(z, vec.getZ());
568 }
569
570 return Vector3D.of(x, y, z);
571 }
572
573 /** Return a vector containing the minimum component values from all input vectors.
574 * @param first first vector
575 * @param more additional vectors
576 * @return a vector containing the minimum component values from all input vectors
577 */
578 public static Vector3D min(final Vector3D first, final Vector3D... more) {
579 return computeMin(first, Arrays.asList(more).iterator());
580 }
581
582 /** Return a vector containing the minimum component values from all input vectors.
583 * @param vecs input vectors
584 * @return a vector containing the minimum component values from all input vectors
585 * @throws IllegalArgumentException if the argument does not contain any vectors
586 */
587 public static Vector3D min(final Iterable<Vector3D> vecs) {
588 final Iterator<Vector3D> it = vecs.iterator();
589 if (!it.hasNext()) {
590 throw new IllegalArgumentException("Cannot compute vector min: no vectors given");
591 }
592
593 return computeMin(it.next(), it);
594 }
595
596 /** Internal method for computing a min vector.
597 * @param first first vector
598 * @param more iterator with additional vectors
599 * @return vector containing the minimum component values of all input vectors
600 */
601 private static Vector3D computeMin(final Vector3D first, final Iterator<? extends Vector3D> more) {
602 double x = first.getX();
603 double y = first.getY();
604 double z = first.getZ();
605
606 Vector3D vec;
607 while (more.hasNext()) {
608 vec = more.next();
609
610 x = Math.min(x, vec.getX());
611 y = Math.min(y, vec.getY());
612 z = Math.min(z, vec.getZ());
613 }
614
615 return Vector3D.of(x, y, z);
616 }
617
618 /** Compute the centroid of the given points. The centroid is the arithmetic mean position of a set
619 * of points.
620 * @param first first point
621 * @param more additional points
622 * @return the centroid of the given points
623 */
624 public static Vector3D centroid(final Vector3D first, final Vector3D... more) {
625 return computeCentroid(first, Arrays.asList(more).iterator());
626 }
627
628 /** Compute the centroid of the given points. The centroid is the arithmetic mean position of a set
629 * of points.
630 * @param pts the points to compute the centroid of
631 * @return the centroid of the given points
632 * @throws IllegalArgumentException if the argument contains no points
633 */
634 public static Vector3D centroid(final Iterable<Vector3D> pts) {
635 final Iterator<Vector3D> it = pts.iterator();
636 if (!it.hasNext()) {
637 throw new IllegalArgumentException("Cannot compute centroid: no points given");
638 }
639
640 return computeCentroid(it.next(), it);
641 }
642
643 /** Internal method for computing the centroid of a set of points.
644 * @param first first point
645 * @param more iterator with additional points
646 * @return the centroid of the point set
647 */
648 private static Vector3D computeCentroid(final Vector3D first, final Iterator<? extends Vector3D> more) {
649 final Sum sum = Sum.of(first);
650 int count = 1;
651
652 while (more.hasNext()) {
653 sum.add(more.next());
654 ++count;
655 }
656
657 return sum.get().multiply(1.0 / count);
658 }
659
660 /**
661 * Represents unit vectors.
662 * This allows optimizations for certain operations.
663 */
664 public static final class Unit extends Vector3D {
665 /** Unit vector (coordinates: 1, 0, 0). */
666 public static final Unit PLUS_X = new Unit(1d, 0d, 0d);
667 /** Negation of unit vector (coordinates: -1, 0, 0). */
668 public static final Unit MINUS_X = new Unit(-1d, 0d, 0d);
669 /** Unit vector (coordinates: 0, 1, 0). */
670 public static final Unit PLUS_Y = new Unit(0d, 1d, 0d);
671 /** Negation of unit vector (coordinates: 0, -1, 0). */
672 public static final Unit MINUS_Y = new Unit(0d, -1d, 0d);
673 /** Unit vector (coordinates: 0, 0, 1). */
674 public static final Unit PLUS_Z = new Unit(0d, 0d, 1d);
675 /** Negation of unit vector (coordinates: 0, 0, -1). */
676 public static final Unit MINUS_Z = new Unit(0d, 0d, -1d);
677
678 /** Maximum coordinate value for computing normalized vectors
679 * with raw, unscaled values.
680 */
681 private static final double UNSCALED_MAX = 0x1.0p+500;
682
683 /** Factor used to scale up coordinate values in order to produce
684 * normalized coordinates without overflow or underflow.
685 */
686 private static final double SCALE_UP_FACTOR = 0x1.0p+600;
687
688 /** Factor used to scale down coordinate values in order to produce
689 * normalized coordinates without overflow or underflow.
690 */
691 private static final double SCALE_DOWN_FACTOR = 0x1.0p-600;
692
693 /** Simple constructor. Callers are responsible for ensuring that the given
694 * values represent a normalized vector.
695 * @param x x coordinate value
696 * @param y x coordinate value
697 * @param z x coordinate value
698 */
699 private Unit(final double x, final double y, final double z) {
700 super(x, y, z);
701 }
702
703 /** {@inheritDoc} */
704 @Override
705 public double norm() {
706 return 1;
707 }
708
709 /** {@inheritDoc} */
710 @Override
711 public double normSq() {
712 return 1;
713 }
714
715 /** {@inheritDoc} */
716 @Override
717 public Unit normalize() {
718 return this;
719 }
720
721 /** {@inheritDoc} */
722 @Override
723 public Unit normalizeOrNull() {
724 return this;
725 }
726
727 /** {@inheritDoc} */
728 @Override
729 public Vector3D withNorm(final double mag) {
730 return multiply(mag);
731 }
732
733 /** {@inheritDoc} */
734 @Override
735 public Unit negate() {
736 return new Unit(-getX(), -getY(), -getZ());
737 }
738
739 /** Create a normalized vector.
740 * @param x Vector coordinate.
741 * @param y Vector coordinate.
742 * @param z Vector coordinate.
743 * @return a vector whose norm is 1.
744 * @throws IllegalArgumentException if the norm of the given value is zero, NaN,
745 * or infinite
746 */
747 public static Unit from(final double x, final double y, final double z) {
748 return tryCreateNormalized(x, y, z, true);
749 }
750
751 /** Create a normalized vector.
752 * @param v Vector.
753 * @return a vector whose norm is 1.
754 * @throws IllegalArgumentException if the norm of the given value is zero, NaN,
755 * or infinite
756 */
757 public static Unit from(final Vector3D v) {
758 return v instanceof Unit ?
759 (Unit) v :
760 from(v.getX(), v.getY(), v.getZ());
761 }
762
763 /** Attempt to create a normalized vector from the given coordinate values. If {@code throwOnFailure}
764 * is true, an exception is thrown if a normalized vector cannot be created. Otherwise, null
765 * is returned.
766 * @param x x coordinate
767 * @param y y coordinate
768 * @param z z coordinate
769 * @param throwOnFailure if true, an exception will be thrown if a normalized vector cannot be created
770 * @return normalized vector or null if one cannot be created and {@code throwOnFailure}
771 * is false
772 * @throws IllegalArgumentException if the computed norm is zero, NaN, or infinite
773 */
774 private static Unit tryCreateNormalized(final double x, final double y, final double z,
775 final boolean throwOnFailure) {
776
777 // Compute the inverse norm directly. If the result is a non-zero real number,
778 // then we can go ahead and construct the unit vector immediately. If not,
779 // we'll do some extra work for edge cases.
780 final double norm = Math.sqrt((x * x) + (y * y) + (z * z));
781 final double normInv = 1.0 / norm;
782 if (Vectors.isRealNonZero(normInv)) {
783 return new Unit(
784 x * normInv,
785 y * normInv,
786 z * normInv);
787 }
788
789 // Direct computation did not work. Try scaled versions of the coordinates
790 // to handle overflow and underflow.
791 final double scaledX;
792 final double scaledY;
793 final double scaledZ;
794
795 final double maxCoord = Math.max(Math.max(Math.abs(x), Math.abs(y)), Math.abs(z));
796 if (maxCoord > UNSCALED_MAX) {
797 scaledX = x * SCALE_DOWN_FACTOR;
798 scaledY = y * SCALE_DOWN_FACTOR;
799 scaledZ = z * SCALE_DOWN_FACTOR;
800 } else {
801 scaledX = x * SCALE_UP_FACTOR;
802 scaledY = y * SCALE_UP_FACTOR;
803 scaledZ = z * SCALE_UP_FACTOR;
804 }
805
806 final double scaledNormInv = 1.0 / Math.sqrt(
807 (scaledX * scaledX) +
808 (scaledY * scaledY) +
809 (scaledZ * scaledZ));
810
811 if (Vectors.isRealNonZero(scaledNormInv)) {
812 return new Unit(
813 scaledX * scaledNormInv,
814 scaledY * scaledNormInv,
815 scaledZ * scaledNormInv);
816 } else if (throwOnFailure) {
817 throw Vectors.illegalNorm(norm);
818 }
819 return null;
820 }
821 }
822
823 /** Class used to create high-accuracy sums of vectors. Each vector component is
824 * summed using an instance of {@link org.apache.commons.numbers.core.Sum}.
825 *
826 * <p>This class is mutable and not thread-safe.
827 * @see org.apache.commons.numbers.core.Sum
828 */
829 public static final class Sum extends EuclideanVectorSum<Vector3D> {
830 /** X component sum. */
831 private final org.apache.commons.numbers.core.Sum xsum;
832 /** Y component sum. */
833 private final org.apache.commons.numbers.core.Sum ysum;
834 /** Z component sum. */
835 private final org.apache.commons.numbers.core.Sum zsum;
836
837 /** Construct a new instance with the given initial value.
838 * @param initial initial value
839 */
840 Sum(final Vector3D initial) {
841 this.xsum = org.apache.commons.numbers.core.Sum.of(initial.x);
842 this.ysum = org.apache.commons.numbers.core.Sum.of(initial.y);
843 this.zsum = org.apache.commons.numbers.core.Sum.of(initial.z);
844 }
845
846 /** {@inheritDoc} */
847 @Override
848 public Sum add(final Vector3D vec) {
849 xsum.add(vec.x);
850 ysum.add(vec.y);
851 zsum.add(vec.z);
852 return this;
853 }
854
855 /** {@inheritDoc} */
856 @Override
857 public Sum addScaled(final double scale, final Vector3D vec) {
858 xsum.addProduct(scale, vec.x);
859 ysum.addProduct(scale, vec.y);
860 zsum.addProduct(scale, vec.z);
861 return this;
862 }
863
864 /** {@inheritDoc} */
865 @Override
866 public Vector3D get() {
867 return Vector3D.of(
868 xsum.getAsDouble(),
869 ysum.getAsDouble(),
870 zsum.getAsDouble());
871 }
872
873 /** Create a new instance with an initial value set to the {@link Vector3D#ZERO zero vector}.
874 * @return new instance set to zero
875 */
876 public static Sum create() {
877 return new Sum(Vector3D.ZERO);
878 }
879
880 /** Construct a new instance with an initial value set to the argument.
881 * @param initial initial sum value
882 * @return new instance
883 */
884 public static Sum of(final Vector3D initial) {
885 return new Sum(initial);
886 }
887
888 /** Construct a new instance from multiple values.
889 * @param first first vector
890 * @param more additional vectors
891 * @return new instance
892 */
893 public static Sum of(final Vector3D first, final Vector3D... more) {
894 final Sum s = new Sum(first);
895 for (final Vector3D v : more) {
896 s.add(v);
897 }
898 return s;
899 }
900 }
901 }