001/* 002 * Licensed to the Apache Software Foundation (ASF) under one or more 003 * contributor license agreements. See the NOTICE file distributed with 004 * this work for additional information regarding copyright ownership. 005 * The ASF licenses this file to You under the Apache License, Version 2.0 006 * (the "License"); you may not use this file except in compliance with 007 * the License. You may obtain a copy of the License at 008 * 009 * http://www.apache.org/licenses/LICENSE-2.0 010 * 011 * Unless required by applicable law or agreed to in writing, software 012 * distributed under the License is distributed on an "AS IS" BASIS, 013 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 014 * See the License for the specific language governing permissions and 015 * limitations under the License. 016 */ 017package org.apache.commons.geometry.euclidean.threed; 018 019import java.util.function.UnaryOperator; 020 021import org.apache.commons.geometry.core.internal.DoubleFunction3N; 022import org.apache.commons.geometry.euclidean.AbstractAffineTransformMatrix; 023import org.apache.commons.geometry.euclidean.internal.Matrices; 024import org.apache.commons.geometry.euclidean.internal.Vectors; 025import org.apache.commons.geometry.euclidean.threed.rotation.QuaternionRotation; 026 027/** Class using a matrix to represent affine transformations in 3 dimensional Euclidean space. 028 * 029 * <p>Instances of this class use a 4x4 matrix for all transform operations. 030 * The last row of this matrix is always set to the values <code>[0 0 0 1]</code> and so 031 * is not stored. Hence, the methods in this class that accept or return arrays always 032 * use arrays containing 12 elements, instead of 16. 033 * </p> 034 */ 035public final class AffineTransformMatrix3D extends AbstractAffineTransformMatrix<Vector3D, AffineTransformMatrix3D> { 036 /** The number of internal matrix elements. */ 037 private static final int NUM_ELEMENTS = 12; 038 039 /** String used to start the transform matrix string representation. */ 040 private static final String MATRIX_START = "[ "; 041 042 /** String used to end the transform matrix string representation. */ 043 private static final String MATRIX_END = " ]"; 044 045 /** String used to separate elements in the matrix string representation. */ 046 private static final String ELEMENT_SEPARATOR = ", "; 047 048 /** String used to separate rows in the matrix string representation. */ 049 private static final String ROW_SEPARATOR = "; "; 050 051 /** Shared transform set to the identity matrix. */ 052 private static final AffineTransformMatrix3D IDENTITY_INSTANCE = new AffineTransformMatrix3D( 053 1, 0, 0, 0, 054 0, 1, 0, 0, 055 0, 0, 1, 0 056 ); 057 058 /** Transform matrix entry <code>m<sub>0,0</sub></code>. */ 059 private final double m00; 060 /** Transform matrix entry <code>m<sub>0,1</sub></code>. */ 061 private final double m01; 062 /** Transform matrix entry <code>m<sub>0,2</sub></code>. */ 063 private final double m02; 064 /** Transform matrix entry <code>m<sub>0,3</sub></code>. */ 065 private final double m03; 066 067 /** Transform matrix entry <code>m<sub>1,0</sub></code>. */ 068 private final double m10; 069 /** Transform matrix entry <code>m<sub>1,1</sub></code>. */ 070 private final double m11; 071 /** Transform matrix entry <code>m<sub>1,2</sub></code>. */ 072 private final double m12; 073 /** Transform matrix entry <code>m<sub>1,3</sub></code>. */ 074 private final double m13; 075 076 /** Transform matrix entry <code>m<sub>2,0</sub></code>. */ 077 private final double m20; 078 /** Transform matrix entry <code>m<sub>2,1</sub></code>. */ 079 private final double m21; 080 /** Transform matrix entry <code>m<sub>2,2</sub></code>. */ 081 private final double m22; 082 /** Transform matrix entry <code>m<sub>2,3</sub></code>. */ 083 private final double m23; 084 085 /** 086 * Package-private constructor; sets all internal matrix elements. 087 * @param m00 matrix entry <code>m<sub>0,0</sub></code> 088 * @param m01 matrix entry <code>m<sub>0,1</sub></code> 089 * @param m02 matrix entry <code>m<sub>0,2</sub></code> 090 * @param m03 matrix entry <code>m<sub>0,3</sub></code> 091 * @param m10 matrix entry <code>m<sub>1,0</sub></code> 092 * @param m11 matrix entry <code>m<sub>1,1</sub></code> 093 * @param m12 matrix entry <code>m<sub>1,2</sub></code> 094 * @param m13 matrix entry <code>m<sub>1,3</sub></code> 095 * @param m20 matrix entry <code>m<sub>2,0</sub></code> 096 * @param m21 matrix entry <code>m<sub>2,1</sub></code> 097 * @param m22 matrix entry <code>m<sub>2,2</sub></code> 098 * @param m23 matrix entry <code>m<sub>2,3</sub></code> 099 */ 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) { 104 105 this.m00 = m00; 106 this.m01 = m01; 107 this.m02 = m02; 108 this.m03 = m03; 109 110 this.m10 = m10; 111 this.m11 = m11; 112 this.m12 = m12; 113 this.m13 = m13; 114 115 this.m20 = m20; 116 this.m21 = m21; 117 this.m22 = m22; 118 this.m23 = m23; 119 } 120 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 } 142 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(); 161 162 return Vector3D.of( 163 applyX(x, y, z), 164 applyY(x, y, z), 165 applyZ(x, y, z)); 166 } 167 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 } 180 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 } 193 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 } 206 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 } 226 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 } 239 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 } 252 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 } 265 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 } 273 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 } 283 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 ] → [ 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 } 301 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 ] → [ 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 } 319 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 } 328 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 } 343 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 } 352 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 } 361 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 } 376 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 } 386 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 } 399 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 } 414 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 } 429 430 /** {@inheritDoc} 431 * 432 * @throws IllegalStateException if the matrix cannot be inverted 433 */ 434 @Override 435 public AffineTransformMatrix3D inverse() { 436 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]. 439 440 final double det = Matrices.checkDeterminantForInverse(determinant()); 441 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); 446 447 // compute the necessary elements of the cofactor matrix 448 // (we need all but the last column) 449 450 final double invDet = 1.0 / det; 451 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); 455 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); 459 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); 463 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 ); 479 480 return new AffineTransformMatrix3D( 481 c00, c10, c20, c30, 482 c01, c11, c21, c31, 483 c02, c12, c22, c32 484 ); 485 } 486 487 /** {@inheritDoc} */ 488 @Override 489 public int hashCode() { 490 final int prime = 31; 491 int result = 1; 492 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)); 499 500 return result; 501 } 502 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 } 517 518 final AffineTransformMatrix3D other = (AffineTransformMatrix3D) obj; 519 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 && 524 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 && 529 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 } 535 536 /** {@inheritDoc} */ 537 @Override 538 public String toString() { 539 final StringBuilder sb = new StringBuilder(); 540 541 sb.append(MATRIX_START) 542 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) 551 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) 560 561 .append(m20) 562 .append(ELEMENT_SEPARATOR) 563 .append(m21) 564 .append(ELEMENT_SEPARATOR) 565 .append(m22) 566 .append(ELEMENT_SEPARATOR) 567 .append(m23) 568 569 .append(MATRIX_END); 570 571 return sb.toString(); 572 } 573 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(); 587 588 return factory.apply( 589 applyVectorX(x, y, z), 590 applyVectorY(x, y, z), 591 applyVectorZ(x, y, z)); 592 } 593 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 } 604 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 } 611 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); 624 625 final Vector3D u = tPlusX.subtract(tZero); 626 final Vector3D v = tPlusY.subtract(tZero); 627 final Vector3D w = tPlusZ.subtract(tZero); 628 629 final AffineTransformMatrix3D mat = AffineTransformMatrix3D.fromColumnVectors(u, v, w, tZero); 630 631 final double det = mat.determinant(); 632 if (!Vectors.isRealNonZero(det)) { 633 throw new IllegalArgumentException("Transform function is invalid: matrix determinant is " + det); 634 } 635 636 return mat; 637 } 638 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 } 652 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) { 665 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 } 672 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 } 680 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 } 688 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 } 702 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 } 710 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 } 718 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 } 732 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 } 745 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) { 753 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; 759 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; 764 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; 769 770 return new AffineTransformMatrix3D( 771 c00, c01, c02, c03, 772 c10, c11, c12, c13, 773 c20, c21, c22, c23 774 ); 775 } 776}