1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * The ASF licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.apache.commons.geometry.euclidean.threed;
18
19 import java.util.function.UnaryOperator;
20
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;
26
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;
38
39 /** String used to start the transform matrix string representation. */
40 private static final String MATRIX_START = "[ ";
41
42 /** String used to end the transform matrix string representation. */
43 private static final String MATRIX_END = " ]";
44
45 /** String used to separate elements in the matrix string representation. */
46 private static final String ELEMENT_SEPARATOR = ", ";
47
48 /** String used to separate rows in the matrix string representation. */
49 private static final String ROW_SEPARATOR = "; ";
50
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 );
57
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;
66
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;
75
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;
84
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) {
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 }