Vector3D.java

  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;

  19. import java.util.Arrays;
  20. import java.util.Comparator;
  21. import java.util.Iterator;
  22. import java.util.function.UnaryOperator;

  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;

  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> {

  36.     /** Zero (null) vector (coordinates: 0, 0, 0). */
  37.     public static final Vector3D ZERO = new Vector3D(0, 0, 0);

  39.     /** A vector with all coordinates set to NaN. */
  40.     public static final Vector3D NaN = new Vector3D(Double.NaN, Double.NaN, Double.NaN);

  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);

  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);

  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;

  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.         }

  71.         return cmp;
  72.     };

  74.     /** X coordinate value (abscissa). */
  75.     private final double x;

  77.     /** Y coordinate value (ordinate). */
  78.     private final double y;

  80.     /** Z coordinate value (height). */
  81.     private final double z;

  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.     }

  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.     }

  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.     }

  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.     }

  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.     }

  123.     /** {@inheritDoc} */
  124.     @Override
  125.     public int getDimension() {
  126.         return 3;
  127.     }

  129.     /** {@inheritDoc} */
  130.     @Override
  131.     public boolean isNaN() {
  132.         return Double.isNaN(x) || Double.isNaN(y) || Double.isNaN(z);
  133.     }

  135.     /** {@inheritDoc} */
  136.     @Override
  137.     public boolean isInfinite() {
  138.         return !isNaN() && (Double.isInfinite(x) || Double.isInfinite(y) || Double.isInfinite(z));
  139.     }

  141.     /** {@inheritDoc} */
  142.     @Override
  143.     public boolean isFinite() {
  144.         return Double.isFinite(x) && Double.isFinite(y) && Double.isFinite(z);
  145.     }

  147.     /** {@inheritDoc} */
  148.     @Override
  149.     public Vector3D getZero() {
  150.         return ZERO;
  151.     }

  153.     /** {@inheritDoc} */
  154.     @Override
  155.     public Vector3D vectorTo(final Vector3D v) {
  156.         return v.subtract(this);
  157.     }

  159.     /** {@inheritDoc} */
  160.     @Override
  161.     public Unit directionTo(final Vector3D v) {
  162.         return vectorTo(v).normalize();
  163.     }

  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.     }

  173.     /** {@inheritDoc} */
  174.     @Override
  175.     public double norm() {
  176.         return Vectors.norm(x, y, z);
  177.     }

  179.     /** {@inheritDoc} */
  180.     @Override
  181.     public double normSq() {
  182.         return Vectors.normSq(x, y, z);
  183.     }

  185.     /** {@inheritDoc} */
  186.     @Override
  187.     public Vector3D withNorm(final double magnitude) {
  188.         final double m = magnitude / getCheckedNorm();

  190.         return new Vector3D(
  191.                     m * x,
  192.                     m * y,
  193.                     m * z
  194.                 );
  195.     }

  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.     }

  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.     }

  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.     }

  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.     }

  237.     /** {@inheritDoc} */
  238.     @Override
  239.     public Vector3D negate() {
  240.         return new Vector3D(-x, -y, -z);
  241.     }

  243.     /** {@inheritDoc} */
  244.     @Override
  245.     public Unit normalize() {
  246.         return Unit.from(x, y, z);
  247.     }

  249.     /** {@inheritDoc} */
  250.     @Override
  251.     public Unit normalizeOrNull() {
  252.         return Unit.tryCreateNormalized(x, y, z, false);
  253.     }

  255.     /** {@inheritDoc} */
  256.     @Override
  257.     public Vector3D multiply(final double a) {
  258.         return new Vector3D(a * x, a * y, a * z);
  259.     }

  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.     }

  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.     }

  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.     }

  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();

  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.         }

  319.         // the vectors are sufficiently separated to use the cosine
  320.         return Math.acos(dot / normProduct);
  321.     }

  323.     /** {@inheritDoc} */
  324.     @Override
  325.     public Vector3D project(final Vector3D base) {
  326.         return getComponent(base, false, Vector3D::new);
  327.     }

  329.     /** {@inheritDoc} */
  330.     @Override
  331.     public Vector3D reject(final Vector3D base) {
  332.         return getComponent(base, true, Vector3D::new);
  333.     }

  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();

  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.     }

  367.     /** {@inheritDoc} */
  368.     @Override
  369.     public Vector3D.Unit orthogonal(final Vector3D dir) {
  370.         return dir.getComponent(this, true, Vector3D.Unit::from);
  371.     }

  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.     }

  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.     }

  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.     }

  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.     }

  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.             }

  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.     }

  451.     /** {@inheritDoc} */
  452.     @Override
  453.     public String toString() {
  454.         return SimpleTupleFormat.getDefault().format(x, y, z);
  455.     }

  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);

  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());

  483.         final double scale = aDotB / baseMagSq;

  485.         final double projX = scale * base.x;
  486.         final double projY = scale * base.y;
  487.         final double projZ = scale * base.z;

  489.         if (reject) {
  490.             return factory.apply(x - projX, y - projY, z - projZ);
  491.         }

  493.         return factory.apply(projX, projY, projZ);
  494.     }

  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.     }

  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.     }

  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.     }

  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.     }

  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.         }

  548.         return computeMax(it.next(), it);
  549.     }

  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();

  561.         Vector3D vec;
  562.         while (more.hasNext()) {
  563.             vec = more.next();

  565.             x = Math.max(x, vec.getX());
  566.             y = Math.max(y, vec.getY());
  567.             z = Math.max(z, vec.getZ());
  568.         }

  570.         return Vector3D.of(x, y, z);
  571.     }

  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.     }

  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.         }

  593.         return computeMin(it.next(), it);
  594.     }

  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();

  606.         Vector3D vec;
  607.         while (more.hasNext()) {
  608.             vec = more.next();

  610.             x = Math.min(x, vec.getX());
  611.             y = Math.min(y, vec.getY());
  612.             z = Math.min(z, vec.getZ());
  613.         }

  615.         return Vector3D.of(x, y, z);
  616.     }

  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.     }

  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.         }

  640.         return computeCentroid(it.next(), it);
  641.     }

  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;

  652.         while (more.hasNext()) {
  653.             sum.add(more.next());
  654.             ++count;
  655.         }

  657.         return sum.get().multiply(1.0 / count);
  658.     }

  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);

  678.         /** Maximum coordinate value for computing normalized vectors
  679.          * with raw, unscaled values.
  680.          */
  681.         private static final double UNSCALED_MAX = 0x1.0p+500;

  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;

  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;

  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.         }

  703.         /** {@inheritDoc} */
  704.         @Override
  705.         public double norm() {
  706.             return 1;
  707.         }

  709.         /** {@inheritDoc} */
  710.         @Override
  711.         public double normSq() {
  712.             return 1;
  713.         }

  715.         /** {@inheritDoc} */
  716.         @Override
  717.         public Unit normalize() {
  718.             return this;
  719.         }

  721.         /** {@inheritDoc} */
  722.         @Override
  723.         public Unit normalizeOrNull() {
  724.             return this;
  725.         }

  727.         /** {@inheritDoc} */
  728.         @Override
  729.         public Vector3D withNorm(final double mag) {
  730.             return multiply(mag);
  731.         }

  733.         /** {@inheritDoc} */
  734.         @Override
  735.         public Unit negate() {
  736.             return new Unit(-getX(), -getY(), -getZ());
  737.         }

  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.         }

  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.         }

  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) {

  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.             }

  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;

  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.             }

  806.             final double scaledNormInv = 1.0 / Math.sqrt(
  807.                     (scaledX * scaledX) +
  808.                     (scaledY * scaledY) +
  809.                     (scaledZ * scaledZ));

  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.     }

  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;

  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.         }

  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.         }

  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.         }

  864.         /** {@inheritDoc} */
  865.         @Override
  866.         public Vector3D get() {
  867.             return Vector3D.of(
  868.                     xsum.getAsDouble(),
  869.                     ysum.getAsDouble(),
  870.                     zsum.getAsDouble());
  871.         }

  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.         }

  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.         }

  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. }
Created with JaCoCo 0.8.5.201910111838