AffineTransformMatrix3D.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.function.UnaryOperator;

  21. import org.apache.commons.geometry.core.internal.DoubleFunction3N;
  22. import org.apache.commons.geometry.euclidean.AbstractAffineTransformMatrix;
  23. import org.apache.commons.geometry.euclidean.internal.Matrices;
  24. import org.apache.commons.geometry.euclidean.internal.Vectors;
  25. import org.apache.commons.geometry.euclidean.threed.rotation.QuaternionRotation;

  27. /** Class using a matrix to represent affine transformations in 3 dimensional Euclidean space.
  28.  *
  29.  * <p>Instances of this class use a 4x4 matrix for all transform operations.
  30.  * The last row of this matrix is always set to the values <code>[0 0 0 1]</code> and so
  31.  * is not stored. Hence, the methods in this class that accept or return arrays always
  32.  * use arrays containing 12 elements, instead of 16.
  33.  * </p>
  34.  */
  35. public final class AffineTransformMatrix3D extends AbstractAffineTransformMatrix<Vector3D, AffineTransformMatrix3D> {
  36.     /** The number of internal matrix elements. */
  37.     private static final int NUM_ELEMENTS = 12;

  39.     /** String used to start the transform matrix string representation. */
  40.     private static final String MATRIX_START = "[ ";

  42.     /** String used to end the transform matrix string representation. */
  43.     private static final String MATRIX_END = " ]";

  45.     /** String used to separate elements in the matrix string representation. */
  46.     private static final String ELEMENT_SEPARATOR = ", ";

  48.     /** String used to separate rows in the matrix string representation. */
  49.     private static final String ROW_SEPARATOR = "; ";

  51.     /** Shared transform set to the identity matrix. */
  52.     private static final AffineTransformMatrix3D IDENTITY_INSTANCE = new AffineTransformMatrix3D(
  53.                 1, 0, 0, 0,
  54.                 0, 1, 0, 0,
  55.                 0, 0, 1, 0
  56.             );

  58.     /** Transform matrix entry <code>m<sub>0,0</sub></code>. */
  59.     private final double m00;
  60.     /** Transform matrix entry <code>m<sub>0,1</sub></code>. */
  61.     private final double m01;
  62.     /** Transform matrix entry <code>m<sub>0,2</sub></code>. */
  63.     private final double m02;
  64.     /** Transform matrix entry <code>m<sub>0,3</sub></code>. */
  65.     private final double m03;

  67.     /** Transform matrix entry <code>m<sub>1,0</sub></code>. */
  68.     private final double m10;
  69.     /** Transform matrix entry <code>m<sub>1,1</sub></code>. */
  70.     private final double m11;
  71.     /** Transform matrix entry <code>m<sub>1,2</sub></code>. */
  72.     private final double m12;
  73.     /** Transform matrix entry <code>m<sub>1,3</sub></code>. */
  74.     private final double m13;

  76.     /** Transform matrix entry <code>m<sub>2,0</sub></code>. */
  77.     private final double m20;
  78.     /** Transform matrix entry <code>m<sub>2,1</sub></code>. */
  79.     private final double m21;
  80.     /** Transform matrix entry <code>m<sub>2,2</sub></code>. */
  81.     private final double m22;
  82.     /** Transform matrix entry <code>m<sub>2,3</sub></code>. */
  83.     private final double m23;

  85.     /**
  86.      * Package-private constructor; sets all internal matrix elements.
  87.      * @param m00 matrix entry <code>m<sub>0,0</sub></code>
  88.      * @param m01 matrix entry <code>m<sub>0,1</sub></code>
  89.      * @param m02 matrix entry <code>m<sub>0,2</sub></code>
  90.      * @param m03 matrix entry <code>m<sub>0,3</sub></code>
  91.      * @param m10 matrix entry <code>m<sub>1,0</sub></code>
  92.      * @param m11 matrix entry <code>m<sub>1,1</sub></code>
  93.      * @param m12 matrix entry <code>m<sub>1,2</sub></code>
  94.      * @param m13 matrix entry <code>m<sub>1,3</sub></code>
  95.      * @param m20 matrix entry <code>m<sub>2,0</sub></code>
  96.      * @param m21 matrix entry <code>m<sub>2,1</sub></code>
  97.      * @param m22 matrix entry <code>m<sub>2,2</sub></code>
  98.      * @param m23 matrix entry <code>m<sub>2,3</sub></code>
  99.      */
  100.     private AffineTransformMatrix3D(
  101.             final double m00, final double m01, final double m02, final double m03,
  102.             final double m10, final double m11, final double m12, final double m13,
  103.             final double m20, final double m21, final double m22, final double m23) {

  105.         this.m00 = m00;
  106.         this.m01 = m01;
  107.         this.m02 = m02;
  108.         this.m03 = m03;

  110.         this.m10 = m10;
  111.         this.m11 = m11;
  112.         this.m12 = m12;
  113.         this.m13 = m13;

  115.         this.m20 = m20;
  116.         this.m21 = m21;
  117.         this.m22 = m22;
  118.         this.m23 = m23;
  119.     }

  121.     /** Return a 12 element array containing the variable elements from the
  122.      * internal transformation matrix. The elements are in row-major order.
  123.      * The array indices map to the internal matrix as follows:
  124.      * <pre>
  125.      *      [
  126.      *          arr[0],   arr[1],   arr[2],   arr[3]
  127.      *          arr[4],   arr[5],   arr[6],   arr[7],
  128.      *          arr[8],   arr[9],   arr[10],  arr[11],
  129.      *          0         0         0         1
  130.      *      ]
  131.      * </pre>
  132.      * @return 12 element array containing the variable elements from the
  133.      *      internal transformation matrix
  134.      */
  135.     public double[] toArray() {
  136.         return new double[] {
  137.             m00, m01, m02, m03,
  138.             m10, m11, m12, m13,
  139.             m20, m21, m22, m23
  140.         };
  141.     }

  143.     /** Apply this transform to the given point, returning the result as a new instance.
  144.      *
  145.      * <p>The transformed point is computed by creating a 4-element column vector from the
  146.      * coordinates in the input and setting the last element to 1. This is then multiplied with the
  147.      * 4x4 transform matrix to produce the transformed point. The {@code 1} in the last position
  148.      * is ignored.
  149.      * <pre>
  150.      *      [ m00  m01  m02  m03 ]     [ x ]     [ x']
  151.      *      [ m10  m11  m12  m13 ]  *  [ y ]  =  [ y']
  152.      *      [ m20  m21  m22  m23 ]     [ z ]     [ z']
  153.      *      [ 0    0    0    1   ]     [ 1 ]     [ 1 ]
  154.      * </pre>
  155.      */
  156.     @Override
  157.     public Vector3D apply(final Vector3D pt) {
  158.         final double x = pt.getX();
  159.         final double y = pt.getY();
  160.         final double z = pt.getZ();

  162.         return Vector3D.of(
  163.                 applyX(x, y, z),
  164.                 applyY(x, y, z),
  165.                 applyZ(x, y, z));
  166.     }

  168.     /** Apply this transform to the given point coordinates and return the transformed
  169.      * x value. The return value is equal to
  170.      * <code>(x * m<sub>00</sub>) + (y * m<sub>01</sub>) + (z * m<sub>02</sub>) + m<sub>03</sub></code>.
  171.      * @param x x coordinate value
  172.      * @param y y coordinate value
  173.      * @param z z coordinate value
  174.      * @return transformed x coordinate value
  175.      * @see #apply(Vector3D)
  176.      */
  177.     public double applyX(final double x, final double y, final double z) {
  178.         return applyVectorX(x, y, z) + m03;
  179.     }

  181.     /** Apply this transform to the given point coordinates and return the transformed
  182.      * y value. The return value is equal to
  183.      * <code>(x * m<sub>10</sub>) + (y * m<sub>11</sub>) + (z * m<sub>12</sub>) + m<sub>13</sub></code>.
  184.      * @param x x coordinate value
  185.      * @param y y coordinate value
  186.      * @param z z coordinate value
  187.      * @return transformed y coordinate value
  188.      * @see #apply(Vector3D)
  189.      */
  190.     public double applyY(final double x, final double y, final double z) {
  191.         return applyVectorY(x, y, z) + m13;
  192.     }

  194.     /** Apply this transform to the given point coordinates and return the transformed
  195.      * z value. The return value is equal to
  196.      * <code>(x * m<sub>20</sub>) + (y * m<sub>21</sub>) + (z * m<sub>22</sub>) + m<sub>23</sub></code>.
  197.      * @param x x coordinate value
  198.      * @param y y coordinate value
  199.      * @param z z coordinate value
  200.      * @return transformed z coordinate value
  201.      * @see #apply(Vector3D)
  202.      */
  203.     public double applyZ(final double x, final double y, final double z) {
  204.         return applyVectorZ(x, y, z) + m23;
  205.     }

  207.     /** {@inheritDoc}
  208.      *
  209.      *  <p>The transformed vector is computed by creating a 4-element column vector from the
  210.      * coordinates in the input and setting the last element to 0. This is then multiplied with the
  211.      * 4x4 transform matrix to produce the transformed vector. The {@code 0} in the last position
  212.      * is ignored.
  213.      * <pre>
  214.      *      [ m00  m01  m02  m03 ]     [ x ]     [ x']
  215.      *      [ m10  m11  m12  m13 ]  *  [ y ]  =  [ y']
  216.      *      [ m20  m21  m22  m23 ]     [ z ]     [ z']
  217.      *      [ 0    0    0    1   ]     [ 0 ]     [ 0 ]
  218.      * </pre>
  219.      *
  220.      * @see #applyDirection(Vector3D)
  221.      */
  222.     @Override
  223.     public Vector3D applyVector(final Vector3D vec) {
  224.         return applyVector(vec, Vector3D::of);
  225.     }

  227.     /** Apply this transform to the given vector coordinates, ignoring translations, and
  228.      * return the transformed x value. The return value is equal to
  229.      * <code>(x * m<sub>00</sub>) + (y * m<sub>01</sub>) + (z * m<sub>02</sub>)</code>.
  230.      * @param x x coordinate value
  231.      * @param y y coordinate value
  232.      * @param z z coordinate value
  233.      * @return transformed x coordinate value
  234.      * @see #applyVector(Vector3D)
  235.      */
  236.     public double applyVectorX(final double x, final double y, final double z) {
  237.         return Vectors.linearCombination(m00, x, m01, y, m02, z);
  238.     }

  240.     /** Apply this transform to the given vector coordinates, ignoring translations, and
  241.      * return the transformed y value. The return value is equal to
  242.      * <code>(x * m<sub>10</sub>) + (y * m<sub>11</sub>) + (z * m<sub>12</sub>)</code>.
  243.      * @param x x coordinate value
  244.      * @param y y coordinate value
  245.      * @param z z coordinate value
  246.      * @return transformed y coordinate value
  247.      * @see #applyVector(Vector3D)
  248.      */
  249.     public double applyVectorY(final double x, final double y, final double z) {
  250.         return Vectors.linearCombination(m10, x, m11, y, m12, z);
  251.     }

  253.     /** Apply this transform to the given vector coordinates, ignoring translations, and
  254.      * return the transformed z value. The return value is equal to
  255.      * <code>(x * m<sub>20</sub>) + (y * m<sub>21</sub>) + (z * m<sub>22</sub>)</code>.
  256.      * @param x x coordinate value
  257.      * @param y y coordinate value
  258.      * @param z z coordinate value
  259.      * @return transformed z coordinate value
  260.      * @see #applyVector(Vector3D)
  261.      */
  262.     public double applyVectorZ(final double x, final double y, final double z) {
  263.         return Vectors.linearCombination(m20, x, m21, y, m22, z);
  264.     }

  266.     /** {@inheritDoc}
  267.      * @see #applyVector(Vector3D)
  268.      */
  269.     @Override
  270.     public Vector3D.Unit applyDirection(final Vector3D vec) {
  271.         return applyVector(vec, Vector3D.Unit::from);
  272.     }

  274.     /** {@inheritDoc} */
  275.     @Override
  276.     public double determinant() {
  277.         return Matrices.determinant(
  278.                 m00, m01, m02,
  279.                 m10, m11, m12,
  280.                 m20, m21, m22
  281.             );
  282.     }

  284.     /** {@inheritDoc}
  285.      *
  286.      * <p><strong>Example</strong>
  287.      * <pre>
  288.      *      [ a, b, c, d ]   [ a, b, c, 0 ]
  289.      *      [ e, f, g, h ]   [ e, f, g, 0 ]
  290.      *      [ i, j, k, l ] &rarr; [ i, j, k, 0 ]
  291.      *      [ 0, 0, 0, 1 ]   [ 0, 0, 0, 1 ]
  292.      * </pre>
  293.      */
  294.     @Override
  295.     public AffineTransformMatrix3D linear() {
  296.         return new AffineTransformMatrix3D(
  297.                 m00, m01, m02, 0.0,
  298.                 m10, m11, m12, 0.0,
  299.                 m20, m21, m22, 0.0);
  300.     }

  302.     /** {@inheritDoc}
  303.      *
  304.      * <p><strong>Example</strong>
  305.      * <pre>
  306.      *      [ a, b, c, d ]   [ a, e, i, 0 ]
  307.      *      [ e, f, g, h ]   [ b, f, j, 0 ]
  308.      *      [ i, j, k, l ] &rarr; [ c, g, k, 0 ]
  309.      *      [ 0, 0, 0, 1 ]   [ 0, 0, 0, 1 ]
  310.      * </pre>
  311.      */
  312.     @Override
  313.     public AffineTransformMatrix3D linearTranspose() {
  314.         return new AffineTransformMatrix3D(
  315.                 m00, m10, m20, 0.0,
  316.                 m01, m11, m21, 0.0,
  317.                 m02, m12, m22, 0.0);
  318.     }

  320.     /** Apply a translation to the current instance, returning the result as a new transform.
  321.      * @param translation vector containing the translation values for each axis
  322.      * @return a new transform containing the result of applying a translation to
  323.      *      the current instance
  324.      */
  325.     public AffineTransformMatrix3D translate(final Vector3D translation) {
  326.         return translate(translation.getX(), translation.getY(), translation.getZ());
  327.     }

  329.     /** Apply a translation to the current instance, returning the result as a new transform.
  330.      * @param x translation in the x direction
  331.      * @param y translation in the y direction
  332.      * @param z translation in the z direction
  333.      * @return a new transform containing the result of applying a translation to
  334.      *      the current instance
  335.      */
  336.     public AffineTransformMatrix3D translate(final double x, final double y, final double z) {
  337.         return new AffineTransformMatrix3D(
  338.                     m00, m01, m02, m03 + x,
  339.                     m10, m11, m12, m13 + y,
  340.                     m20, m21, m22, m23 + z
  341.                 );
  342.     }

  344.     /** Apply a scale operation to the current instance, returning the result as a new transform.
  345.      * @param factor the scale factor to apply to all axes
  346.      * @return a new transform containing the result of applying a scale operation to
  347.      *      the current instance
  348.      */
  349.     public AffineTransformMatrix3D scale(final double factor) {
  350.         return scale(factor, factor, factor);
  351.     }

  353.     /** Apply a scale operation to the current instance, returning the result as a new transform.
  354.      * @param scaleFactors vector containing scale factors for each axis
  355.      * @return a new transform containing the result of applying a scale operation to
  356.      *      the current instance
  357.      */
  358.     public AffineTransformMatrix3D scale(final Vector3D scaleFactors) {
  359.         return scale(scaleFactors.getX(), scaleFactors.getY(), scaleFactors.getZ());
  360.     }

  362.     /** Apply a scale operation to the current instance, returning the result as a new transform.
  363.      * @param x scale factor for the x axis
  364.      * @param y scale factor for the y axis
  365.      * @param z scale factor for the z axis
  366.      * @return a new transform containing the result of applying a scale operation to
  367.      *      the current instance
  368.      */
  369.     public AffineTransformMatrix3D scale(final double x, final double y, final double z) {
  370.         return new AffineTransformMatrix3D(
  371.                     m00 * x, m01 * x, m02 * x, m03 * x,
  372.                     m10 * y, m11 * y, m12 * y, m13 * y,
  373.                     m20 * z, m21 * z, m22 * z, m23 * z
  374.                 );
  375.     }

  377.     /** Apply a rotation to the current instance, returning the result as a new transform.
  378.      * @param rotation the rotation to apply
  379.      * @return a new transform containing the result of applying a rotation to the
  380.      *      current instance
  381.      * @see QuaternionRotation#toMatrix()
  382.      */
  383.     public AffineTransformMatrix3D rotate(final QuaternionRotation rotation) {
  384.         return multiply(rotation.toMatrix(), this);
  385.     }

  387.     /** Apply a rotation around the given center point to the current instance, returning the result
  388.      * as a new transform. This is achieved by translating the center point to the origin, applying
  389.      * the rotation, and then translating back.
  390.      * @param center the center of rotation
  391.      * @param rotation the rotation to apply
  392.      * @return a new transform containing the result of applying a rotation about the given center
  393.      *      point to the current instance
  394.      * @see QuaternionRotation#toMatrix()
  395.      */
  396.     public AffineTransformMatrix3D rotate(final Vector3D center, final QuaternionRotation rotation) {
  397.         return multiply(createRotation(center, rotation), this);
  398.     }

  400.     /** Get a new transform created by multiplying this instance by the argument.
  401.      * This is equivalent to the expression {@code A * M} where {@code A} is the
  402.      * current transform matrix and {@code M} is the given transform matrix. In
  403.      * terms of transformations, applying the returned matrix is equivalent to
  404.      * applying {@code M} and <em>then</em> applying {@code A}. In other words,
  405.      * the rightmost transform is applied first.
  406.      *
  407.      * @param m the transform to multiply with
  408.      * @return the result of multiplying the current instance by the given
  409.      *      transform matrix
  410.      */
  411.     public AffineTransformMatrix3D multiply(final AffineTransformMatrix3D m) {
  412.         return multiply(this, m);
  413.     }

  415.     /** Get a new transform created by multiplying the argument by this instance.
  416.      * This is equivalent to the expression {@code M * A} where {@code A} is the
  417.      * current transform matrix and {@code M} is the given transform matrix. In
  418.      * terms of transformations, applying the returned matrix is equivalent to
  419.      * applying {@code A} and <em>then</em> applying {@code M}. In other words,
  420.      * the rightmost transform is applied first.
  421.      *
  422.      * @param m the transform to multiply with
  423.      * @return the result of multiplying the given transform matrix by the current
  424.      *      instance
  425.      */
  426.     public AffineTransformMatrix3D premultiply(final AffineTransformMatrix3D m) {
  427.         return multiply(m, this);
  428.     }

  430.     /** {@inheritDoc}
  431.     *
  432.     * @throws IllegalStateException if the matrix cannot be inverted
  433.     */
  434.     @Override
  435.     public AffineTransformMatrix3D inverse() {

  437.         // Our full matrix is 4x4 but we can significantly reduce the amount of computations
  438.         // needed here since we know that our last row is [0 0 0 1].

  440.         final double det = Matrices.checkDeterminantForInverse(determinant());

  442.         // validate the remaining matrix elements that were not part of the determinant
  443.         Matrices.checkElementForInverse(m03);
  444.         Matrices.checkElementForInverse(m13);
  445.         Matrices.checkElementForInverse(m23);

  447.         // compute the necessary elements of the cofactor matrix
  448.         // (we need all but the last column)

  450.         final double invDet = 1.0 / det;

  452.         final double c00 = invDet * Matrices.determinant(m11, m12, m21, m22);
  453.         final double c01 = -invDet * Matrices.determinant(m10, m12, m20, m22);
  454.         final double c02 = invDet * Matrices.determinant(m10, m11, m20, m21);

  456.         final double c10 = -invDet * Matrices.determinant(m01, m02, m21, m22);
  457.         final double c11 = invDet * Matrices.determinant(m00, m02, m20, m22);
  458.         final double c12 = -invDet * Matrices.determinant(m00, m01, m20, m21);

  460.         final double c20 = invDet * Matrices.determinant(m01, m02, m11, m12);
  461.         final double c21 = -invDet * Matrices.determinant(m00, m02, m10, m12);
  462.         final double c22 = invDet * Matrices.determinant(m00, m01, m10, m11);

  464.         final double c30 = -invDet * Matrices.determinant(
  465.                     m01, m02, m03,
  466.                     m11, m12, m13,
  467.                     m21, m22, m23
  468.                 );
  469.         final double c31 = invDet * Matrices.determinant(
  470.                     m00, m02, m03,
  471.                     m10, m12, m13,
  472.                     m20, m22, m23
  473.                 );
  474.         final double c32 = -invDet * Matrices.determinant(
  475.                     m00, m01, m03,
  476.                     m10, m11, m13,
  477.                     m20, m21, m23
  478.                 );

  480.         return new AffineTransformMatrix3D(
  481.                     c00, c10, c20, c30,
  482.                     c01, c11, c21, c31,
  483.                     c02, c12, c22, c32
  484.                 );
  485.     }

  487.     /** {@inheritDoc} */
  488.     @Override
  489.     public int hashCode() {
  490.         final int prime = 31;
  491.         int result = 1;

  493.         result = (result * prime) + (Double.hashCode(m00) - Double.hashCode(m01) +
  494.                 Double.hashCode(m02) - Double.hashCode(m03));
  495.         result = (result * prime) + (Double.hashCode(m10) - Double.hashCode(m11) +
  496.                 Double.hashCode(m12) - Double.hashCode(m13));
  497.         result = (result * prime) + (Double.hashCode(m20) - Double.hashCode(m21) +
  498.                 Double.hashCode(m22) - Double.hashCode(m23));

  500.         return result;
  501.     }

  503.     /**
  504.      * Return true if the given object is an instance of {@link AffineTransformMatrix3D}
  505.      * and all matrix element values are exactly equal.
  506.      * @param obj object to test for equality with the current instance
  507.      * @return true if all transform matrix elements are exactly equal; otherwise false
  508.      */
  509.     @Override
  510.     public boolean equals(final Object obj) {
  511.         if (this == obj) {
  512.             return true;
  513.         }
  514.         if (!(obj instanceof AffineTransformMatrix3D)) {
  515.             return false;
  516.         }

  518.         final AffineTransformMatrix3D other = (AffineTransformMatrix3D) obj;

  520.         return Double.compare(this.m00, other.m00) == 0 &&
  521.                 Double.compare(this.m01, other.m01) == 0 &&
  522.                 Double.compare(this.m02, other.m02) == 0 &&
  523.                 Double.compare(this.m03, other.m03) == 0 &&

  525.                 Double.compare(this.m10, other.m10) == 0 &&
  526.                 Double.compare(this.m11, other.m11) == 0 &&
  527.                 Double.compare(this.m12, other.m12) == 0 &&
  528.                 Double.compare(this.m13, other.m13) == 0 &&

  530.                 Double.compare(this.m20, other.m20) == 0 &&
  531.                 Double.compare(this.m21, other.m21) == 0 &&
  532.                 Double.compare(this.m22, other.m22) == 0 &&
  533.                 Double.compare(this.m23, other.m23) == 0;
  534.     }

  536.     /** {@inheritDoc} */
  537.     @Override
  538.     public String toString() {
  539.         final StringBuilder sb = new StringBuilder();

  541.         sb.append(MATRIX_START)

  543.             .append(m00)
  544.             .append(ELEMENT_SEPARATOR)
  545.             .append(m01)
  546.             .append(ELEMENT_SEPARATOR)
  547.             .append(m02)
  548.             .append(ELEMENT_SEPARATOR)
  549.             .append(m03)
  550.             .append(ROW_SEPARATOR)

  552.             .append(m10)
  553.             .append(ELEMENT_SEPARATOR)
  554.             .append(m11)
  555.             .append(ELEMENT_SEPARATOR)
  556.             .append(m12)
  557.             .append(ELEMENT_SEPARATOR)
  558.             .append(m13)
  559.             .append(ROW_SEPARATOR)

  561.             .append(m20)
  562.             .append(ELEMENT_SEPARATOR)
  563.             .append(m21)
  564.             .append(ELEMENT_SEPARATOR)
  565.             .append(m22)
  566.             .append(ELEMENT_SEPARATOR)
  567.             .append(m23)

  569.             .append(MATRIX_END);

  571.         return sb.toString();
  572.     }

  574.     /** Multiplies the given vector by the 3x3 linear transformation matrix contained in the
  575.      * upper-right corner of the affine transformation matrix. This applies all transformation
  576.      * operations except for translations. The computed coordinates are passed to the given
  577.      * factory function.
  578.      * @param <T> factory output type
  579.      * @param vec the vector to transform
  580.      * @param factory the factory instance that will be passed the transformed coordinates
  581.      * @return the factory return value
  582.      */
  583.     private <T> T applyVector(final Vector3D vec, final DoubleFunction3N<T> factory) {
  584.         final double x = vec.getX();
  585.         final double y = vec.getY();
  586.         final double z = vec.getZ();

  588.         return factory.apply(
  589.                 applyVectorX(x, y, z),
  590.                 applyVectorY(x, y, z),
  591.                 applyVectorZ(x, y, z));
  592.     }

  594.     /** Get a new transform with the given matrix elements. The array must contain 12 elements.
  595.      * @param arr 12-element array containing values for the variable entries in the
  596.      *      transform matrix
  597.      * @return a new transform initialized with the given matrix values
  598.      * @throws IllegalArgumentException if the array does not have 12 elements
  599.      */
  600.     public static AffineTransformMatrix3D of(final double... arr) {
  601.         if (arr.length != NUM_ELEMENTS) {
  602.             throw new IllegalArgumentException("Dimension mismatch: " + arr.length + " != " + NUM_ELEMENTS);
  603.         }

  605.         return new AffineTransformMatrix3D(
  606.                     arr[0], arr[1], arr[2], arr[3],
  607.                     arr[4], arr[5], arr[6], arr[7],
  608.                     arr[8], arr[9], arr[10], arr[11]
  609.                 );
  610.     }

  612.     /** Construct a new transform representing the given function. The function is sampled at
  613.      * the origin and along each axis and a matrix is created to perform the transformation.
  614.      * @param fn function to create a transform matrix from
  615.      * @return a transform matrix representing the given function
  616.      * @throws IllegalArgumentException if the given function does not represent a valid
  617.      *      affine transform
  618.      */
  619.     public static AffineTransformMatrix3D from(final UnaryOperator<Vector3D> fn) {
  620.         final Vector3D tPlusX = fn.apply(Vector3D.Unit.PLUS_X);
  621.         final Vector3D tPlusY = fn.apply(Vector3D.Unit.PLUS_Y);
  622.         final Vector3D tPlusZ = fn.apply(Vector3D.Unit.PLUS_Z);
  623.         final Vector3D tZero = fn.apply(Vector3D.ZERO);

  625.         final Vector3D u = tPlusX.subtract(tZero);
  626.         final Vector3D v = tPlusY.subtract(tZero);
  627.         final Vector3D w = tPlusZ.subtract(tZero);

  629.         final AffineTransformMatrix3D mat =  AffineTransformMatrix3D.fromColumnVectors(u, v, w, tZero);

  631.         final double det = mat.determinant();
  632.         if (!Vectors.isRealNonZero(det)) {
  633.             throw new IllegalArgumentException("Transform function is invalid: matrix determinant is " + det);
  634.         }

  636.         return mat;
  637.     }

  639.     /** Get a new transform create from the given column vectors. The returned transform
  640.      * does not include any translation component.
  641.      * @param u first column vector; this corresponds to the first basis vector
  642.      *      in the coordinate frame
  643.      * @param v second column vector; this corresponds to the second basis vector
  644.      *      in the coordinate frame
  645.      * @param w third column vector; this corresponds to the third basis vector
  646.      *      in the coordinate frame
  647.      * @return a new transform with the given column vectors
  648.      */
  649.     public static AffineTransformMatrix3D fromColumnVectors(final Vector3D u, final Vector3D v, final Vector3D w) {
  650.         return fromColumnVectors(u, v, w, Vector3D.ZERO);
  651.     }

  653.     /** Get a new transform created from the given column vectors.
  654.      * @param u first column vector; this corresponds to the first basis vector
  655.      *      in the coordinate frame
  656.      * @param v second column vector; this corresponds to the second basis vector
  657.      *      in the coordinate frame
  658.      * @param w third column vector; this corresponds to the third basis vector
  659.      *      in the coordinate frame
  660.      * @param t fourth column vector; this corresponds to the translation of the transform
  661.      * @return a new transform with the given column vectors
  662.      */
  663.     public static AffineTransformMatrix3D fromColumnVectors(final Vector3D u, final Vector3D v,
  664.             final Vector3D w, final Vector3D t) {

  666.         return new AffineTransformMatrix3D(
  667.                     u.getX(), v.getX(), w.getX(), t.getX(),
  668.                     u.getY(), v.getY(), w.getY(), t.getY(),
  669.                     u.getZ(), v.getZ(), w.getZ(), t.getZ()
  670.                 );
  671.     }

  673.     /** Get the transform representing the identity matrix. This transform does not
  674.      * modify point or vector values when applied.
  675.      * @return transform representing the identity matrix
  676.      */
  677.     public static AffineTransformMatrix3D identity() {
  678.         return IDENTITY_INSTANCE;
  679.     }

  681.     /** Create a transform representing the given translation.
  682.      * @param translation vector containing translation values for each axis
  683.      * @return a new transform representing the given translation
  684.      */
  685.     public static AffineTransformMatrix3D createTranslation(final Vector3D translation) {
  686.         return createTranslation(translation.getX(), translation.getY(), translation.getZ());
  687.     }

  689.     /** Create a transform representing the given translation.
  690.      * @param x translation in the x direction
  691.      * @param y translation in the y direction
  692.      * @param z translation in the z direction
  693.      * @return a new transform representing the given translation
  694.      */
  695.     public static AffineTransformMatrix3D createTranslation(final double x, final double y, final double z) {
  696.         return new AffineTransformMatrix3D(
  697.                     1, 0, 0, x,
  698.                     0, 1, 0, y,
  699.                     0, 0, 1, z
  700.                 );
  701.     }

  703.     /** Create a transform representing a scale operation with the given scale factor applied to all axes.
  704.      * @param factor scale factor to apply to all axes
  705.      * @return a new transform representing a uniform scaling in all axes
  706.      */
  707.     public static AffineTransformMatrix3D createScale(final double factor) {
  708.         return createScale(factor, factor, factor);
  709.     }

  711.     /** Create a transform representing a scale operation.
  712.      * @param factors vector containing scale factors for each axis
  713.      * @return a new transform representing a scale operation
  714.      */
  715.     public static AffineTransformMatrix3D createScale(final Vector3D factors) {
  716.         return createScale(factors.getX(), factors.getY(), factors.getZ());
  717.     }

  719.     /** Create a transform representing a scale operation.
  720.      * @param x scale factor for the x axis
  721.      * @param y scale factor for the y axis
  722.      * @param z scale factor for the z axis
  723.      * @return a new transform representing a scale operation
  724.      */
  725.     public static AffineTransformMatrix3D createScale(final double x, final double y, final double z) {
  726.         return new AffineTransformMatrix3D(
  727.                     x, 0, 0, 0,
  728.                     0, y, 0, 0,
  729.                     0, 0, z, 0
  730.                 );
  731.     }

  733.     /** Create a transform representing a rotation about the given center point. This is achieved by translating
  734.      * the center to the origin, applying the rotation, and then translating back.
  735.      * @param center the center of rotation
  736.      * @param rotation the rotation to apply
  737.      * @return a new transform representing a rotation about the given center point
  738.      * @see QuaternionRotation#toMatrix()
  739.      */
  740.     public static AffineTransformMatrix3D createRotation(final Vector3D center, final QuaternionRotation rotation) {
  741.         return createTranslation(center.negate())
  742.                 .rotate(rotation)
  743.                 .translate(center);
  744.     }

  746.     /** Multiply two transform matrices together and return the result as a new transform instance.
  747.      * @param a first transform
  748.      * @param b second transform
  749.      * @return the transform computed as {@code a x b}
  750.      */
  751.     private static AffineTransformMatrix3D multiply(final AffineTransformMatrix3D a,
  752.             final AffineTransformMatrix3D b) {

  754.         // calculate the matrix elements
  755.         final double c00 = Vectors.linearCombination(a.m00, b.m00, a.m01, b.m10, a.m02, b.m20);
  756.         final double c01 = Vectors.linearCombination(a.m00, b.m01, a.m01, b.m11, a.m02, b.m21);
  757.         final double c02 = Vectors.linearCombination(a.m00, b.m02, a.m01, b.m12, a.m02, b.m22);
  758.         final double c03 = Vectors.linearCombination(a.m00, b.m03, a.m01, b.m13, a.m02, b.m23) + a.m03;

  760.         final double c10 = Vectors.linearCombination(a.m10, b.m00, a.m11, b.m10, a.m12, b.m20);
  761.         final double c11 = Vectors.linearCombination(a.m10, b.m01, a.m11, b.m11, a.m12, b.m21);
  762.         final double c12 = Vectors.linearCombination(a.m10, b.m02, a.m11, b.m12, a.m12, b.m22);
  763.         final double c13 = Vectors.linearCombination(a.m10, b.m03, a.m11, b.m13, a.m12, b.m23) + a.m13;

  765.         final double c20 = Vectors.linearCombination(a.m20, b.m00, a.m21, b.m10, a.m22, b.m20);
  766.         final double c21 = Vectors.linearCombination(a.m20, b.m01, a.m21, b.m11, a.m22, b.m21);
  767.         final double c22 = Vectors.linearCombination(a.m20, b.m02, a.m21, b.m12, a.m22, b.m22);
  768.         final double c23 = Vectors.linearCombination(a.m20, b.m03, a.m21, b.m13, a.m22, b.m23) + a.m23;

  770.         return new AffineTransformMatrix3D(
  771.                     c00, c01, c02, c03,
  772.                     c10, c11, c12, c13,
  773.                     c20, c21, c22, c23
  774.                 );
  775.     }
  776. }
Created with JaCoCo 0.8.5.201910111838