1 /*
2 * Copyright (C) 2012 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.geometry;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.ArrayUtils;
20 import com.irurueta.algebra.Matrix;
21 import com.irurueta.algebra.RQDecomposer;
22 import com.irurueta.algebra.SingularValueDecomposer;
23 import com.irurueta.algebra.Utils;
24 import com.irurueta.algebra.WrongSizeException;
25
26 import java.io.Serializable;
27 import java.util.Arrays;
28
29 /**
30 * This class performs affine transformations on 3D space.
31 * Affine transformations include transformations related to rotations,
32 * translations, independently scaling horizontal or vertical coordinates
33 * or skewing the coordinates axis.
34 * This class is not intended to be used on points located at infinity or
35 * at very large coordinates, since numerical instabilities may occur. For
36 * those cases use a ProjectiveTransformation3D instead.
37 */
38 @SuppressWarnings("DuplicatedCode")
39 public class AffineTransformation3D extends Transformation3D implements Serializable {
40
41 /**
42 * Constant indicating number of coordinates required in translation arrays.
43 */
44 public static final int NUM_TRANSLATION_COORDS = 3;
45
46
47 /**
48 * Constant defining number of inhomogeneous coordinates in 3D space.
49 */
50 public static final int INHOM_COORDS = 3;
51
52 /**
53 * Constant defining number of homogeneous coordinates in 3D space.
54 */
55 public static final int HOM_COORDS = 4;
56
57 /**
58 * Linear mapping.
59 */
60 private Matrix a;
61
62 /**
63 * 2D translation to be performed on geometric objects.
64 * Translation is specified using inhomogeneous coordinates.
65 */
66 private double[] translation;
67
68 /**
69 * Empty constructor.
70 * Creates transformation that has no effect.
71 */
72 public AffineTransformation3D() {
73 super();
74 try {
75 a = Matrix.identity(INHOM_COORDS, INHOM_COORDS);
76 } catch (final WrongSizeException ignore) {
77 // never happens
78 }
79 translation = new double[NUM_TRANSLATION_COORDS];
80 }
81
82 /**
83 * Creates transformation with provided rotation.
84 *
85 * @param a linear mapping.
86 * @throws NullPointerException raised if provided rotation is null.
87 * @throws IllegalArgumentException raised if provided matrix does not have
88 * size 3x3.
89 */
90 public AffineTransformation3D(final Matrix a) {
91 setA(a);
92 translation = new double[NUM_TRANSLATION_COORDS];
93 }
94
95 /**
96 * Creates transformation with provided scale value.
97 *
98 * @param scale scale value. Values between 0.0 and 1.0 reduce objects,
99 * values greater than 1.0 enlarge objects and negative values reverse
100 * objects.
101 */
102 public AffineTransformation3D(final double scale) {
103 final var diag = new double[INHOM_COORDS];
104 Arrays.fill(diag, scale);
105 a = Matrix.diagonal(diag);
106 translation = new double[NUM_TRANSLATION_COORDS];
107 }
108
109 /**
110 * Creates transformation with provided rotation.
111 *
112 * @param rotation a 3D rotation.
113 * @throws NullPointerException raised if provided rotation is null.
114 */
115 public AffineTransformation3D(final Rotation3D rotation) {
116 a = rotation.asInhomogeneousMatrix();
117 translation = new double[NUM_TRANSLATION_COORDS];
118 }
119
120 /**
121 * Creates transformation with provided scale and rotation.
122 *
123 * @param scale scale value. Values between 0.0 and 1.0 reduce objects,
124 * values greater than 1.0 enlarge objects and negative values reverse
125 * objects.
126 * @param rotation a 3D rotation.
127 * @throws NullPointerException raised if provided rotation is null.
128 */
129 public AffineTransformation3D(final double scale, final Rotation3D rotation) {
130 final var diag = new double[INHOM_COORDS];
131 Arrays.fill(diag, scale);
132 a = Matrix.diagonal(diag);
133 try {
134 a.multiply(rotation.asInhomogeneousMatrix());
135 } catch (final WrongSizeException ignore) {
136 // never happens
137 }
138 translation = new double[NUM_TRANSLATION_COORDS];
139 }
140
141 /**
142 * Creates transformation with provided affine parameters and rotation.
143 *
144 * @param params Affine parameters including horizontal scaling, vertical
145 * scaling and skewness.
146 * @param rotation a 3D rotation.
147 * @throws NullPointerException raised if provided parameters are null or
148 * if provided rotation is null.
149 */
150 public AffineTransformation3D(final AffineParameters3D params, final Rotation3D rotation) {
151 a = params.asMatrix();
152 try {
153 a.multiply(rotation.asInhomogeneousMatrix());
154 } catch (final WrongSizeException ignore) {
155 // never happens
156 }
157 translation = new double[NUM_TRANSLATION_COORDS];
158 }
159
160 /**
161 * Creates transformation with provided 3D translation.
162 *
163 * @param translation array indicating 3D translation using inhomogeneous
164 * coordinates.
165 * @throws NullPointerException raised if provided array is null.
166 * @throws IllegalArgumentException raised if length of array is not equal
167 * to NUM_TRANSLATION_COORDS.
168 */
169 public AffineTransformation3D(final double[] translation) {
170 if (translation.length != NUM_TRANSLATION_COORDS) {
171 throw new IllegalArgumentException();
172 }
173
174 try {
175 a = Matrix.identity(INHOM_COORDS, INHOM_COORDS);
176 } catch (final WrongSizeException ignore) {
177 // never happens
178 }
179 this.translation = translation;
180 }
181
182 /**
183 * Creates transformation with provided rotation, translation and scale
184 * value.
185 *
186 * @param a linear mapping.
187 * @param translation array indicating 3D translation using inhomogeneous
188 * coordinates.
189 * @throws NullPointerException raised if provided array is null or if
190 * linear mapping is null.
191 * @throws IllegalArgumentException Raised if length of array is not equal
192 * to NUM_TRANSLATION_COORDS.
193 */
194 public AffineTransformation3D(final Matrix a, final double[] translation) {
195 if (translation.length != NUM_TRANSLATION_COORDS) {
196 throw new IllegalArgumentException();
197 }
198 this.translation = translation;
199
200 setA(a);
201 }
202
203 /**
204 * Creates transformation with provided scale and translation.
205 *
206 * @param scale Scale value. Values between 0.0 and 1.0 reduce objects,
207 * values greater than 1.0 enlarge objects and negative values reverse
208 * objects.
209 * @param translation Array indicating 3D translation using inhomogeneous
210 * coordinates.
211 * @throws NullPointerException raised if provided translation is null.
212 * @throws IllegalArgumentException Raised if provided translation does not
213 * have length 3.
214 */
215 public AffineTransformation3D(final double scale, final double[] translation) {
216 if (translation.length != NUM_TRANSLATION_COORDS) {
217 throw new IllegalArgumentException();
218 }
219
220 final var diag = new double[INHOM_COORDS];
221 Arrays.fill(diag, scale);
222 a = Matrix.diagonal(diag);
223
224 this.translation = translation;
225 }
226
227 /**
228 * Creates transformation with provided rotation and translation.
229 *
230 * @param rotation a 3D rotation.
231 * @param translation array indicating 3D translation using inhomogeneous
232 * coordinates.
233 * @throws NullPointerException raised if provided rotation or translation
234 * is null.
235 * @throws IllegalArgumentException raised if provided translation does not
236 * have length 3.
237 */
238 public AffineTransformation3D(final Rotation3D rotation, final double[] translation) {
239 if (translation.length != NUM_TRANSLATION_COORDS) {
240 throw new IllegalArgumentException();
241 }
242
243 a = rotation.asInhomogeneousMatrix();
244 this.translation = translation;
245 }
246
247 /**
248 * Creates transformation with provided scale, rotation and translation.
249 *
250 * @param scale Scale value. Values between 0.0 and 1.0 reduce objects,
251 * values greater than 1.0 enlarge objects and negative values reverse
252 * objects.
253 * @param rotation a 3D rotation.
254 * @param translation array indicating 3D translation using inhomogeneous
255 * coordinates.
256 * @throws NullPointerException raised if provided rotation or translation
257 * is null.
258 * @throws IllegalArgumentException raised if provided translation does not
259 * have length 3.
260 */
261 public AffineTransformation3D(final double scale, final Rotation3D rotation, final double[] translation) {
262 if (translation.length != NUM_TRANSLATION_COORDS) {
263 throw new IllegalArgumentException();
264 }
265
266 final var diag = new double[INHOM_COORDS];
267 Arrays.fill(diag, scale);
268 a = Matrix.diagonal(diag);
269 try {
270 a.multiply(rotation.asInhomogeneousMatrix());
271 } catch (final WrongSizeException ignore) {
272 // never happens
273 }
274
275 this.translation = translation;
276 }
277
278 /**
279 * Creates transformation with provided parameters, rotation and
280 * translation.
281 *
282 * @param params affine parameters including horizontal scaling, vertical
283 * scaling and skewness.
284 * @param rotation a 3D rotation.
285 * @param translation array indicating 3D translation using inhomogeneous
286 * coordinates.
287 * @throws NullPointerException raised if provided parameters, rotation or
288 * translation is null.
289 * @throws IllegalArgumentException raised if provided translation does not
290 * have length 3.
291 */
292 public AffineTransformation3D(final AffineParameters3D params, final Rotation3D rotation,
293 final double[] translation) {
294 if (translation.length != NUM_TRANSLATION_COORDS) {
295 throw new IllegalArgumentException();
296 }
297
298 a = params.asMatrix();
299 try {
300 a.multiply(rotation.asInhomogeneousMatrix());
301 } catch (final WrongSizeException ignore) {
302 // never happens
303 }
304 this.translation = translation;
305 }
306
307 /**
308 * Creates transformation by estimating its internal values using provided 4
309 * corresponding original and transformed points.
310 *
311 * @param inputPoint1 1st input point.
312 * @param inputPoint2 2nd input point.
313 * @param inputPoint3 3rd input point.
314 * @param inputPoint4 4th input point.
315 * @param outputPoint1 1st transformed point corresponding to 1st input
316 * point.
317 * @param outputPoint2 2nd transformed point corresponding to 2nd input
318 * point.
319 * @param outputPoint3 3rd transformed point corresponding to 3rd input
320 * point.
321 * @param outputPoint4 4th transformed point corresponding to 4th input
322 * point.
323 * @throws CoincidentPointsException raised if transformation cannot be
324 * estimated for some reason (point configuration degeneracy, duplicate
325 * points or numerical instabilities).
326 */
327 public AffineTransformation3D(
328 final Point3D inputPoint1, final Point3D inputPoint2, final Point3D inputPoint3, final Point3D inputPoint4,
329 final Point3D outputPoint1, final Point3D outputPoint2, final Point3D outputPoint3,
330 final Point3D outputPoint4) throws CoincidentPointsException {
331 try {
332 a = new Matrix(INHOM_COORDS, INHOM_COORDS);
333 } catch (final WrongSizeException ignore) {
334 // never happens
335 }
336 translation = new double[NUM_TRANSLATION_COORDS];
337 setTransformationFromPoints(inputPoint1, inputPoint2, inputPoint3, inputPoint4, outputPoint1, outputPoint2,
338 outputPoint3, outputPoint4);
339 }
340
341 /**
342 * Creates transformation by estimating its internal values using provided 4
343 * corresponding original and transformed planes.
344 *
345 * @param inputPlane1 1st input plane.
346 * @param inputPlane2 2nd input plane.
347 * @param inputPlane3 3rd input plane.
348 * @param inputPlane4 4th input plane.
349 * @param outputPlane1 1st transformed plane corresponding to 1st input
350 * plane.
351 * @param outputPlane2 2nd transformed plane corresponding to 2nd input
352 * plane.
353 * @param outputPlane3 3rd transformed plane corresponding to 3rd input
354 * plane.
355 * @param outputPlane4 4th transformed plane corresponding to 4th input
356 * plane.
357 * @throws CoincidentPlanesException raised if transformation cannot be
358 * estimated for some reason (plane configuration degeneracy, duplicate
359 * planes or numerical instabilities).
360 */
361 public AffineTransformation3D(
362 final Plane inputPlane1, final Plane inputPlane2, final Plane inputPlane3, final Plane inputPlane4,
363 final Plane outputPlane1, final Plane outputPlane2, final Plane outputPlane3, final Plane outputPlane4)
364 throws CoincidentPlanesException {
365 setTransformationFromPlanes(inputPlane1, inputPlane2, inputPlane3, inputPlane4, outputPlane1, outputPlane2,
366 outputPlane3, outputPlane4);
367 }
368
369 /**
370 * Creates transformation by estimating internal parameters using provided 2
371 * corresponding original and transformed lines.
372 *
373 * @param inputLine1 1st input line.
374 * @param inputLine2 2nd input line.
375 * @param outputLine1 1st transformed line corresponding to 1st input line.
376 * @param outputLine2 2nd transformed line corresponding to 2nd input line.
377 * @throws CoincidentLinesException raised if transformation cannot be
378 * estimated for some reason (line configuration degeneracy, duplicate lines
379 * or numerical instabilities).
380 */
381 public AffineTransformation3D(
382 final Line3D inputLine1, final Line3D inputLine2, final Line3D outputLine1, final Line3D outputLine2)
383 throws CoincidentLinesException {
384 setTransformationFromLines(inputLine1, inputLine2, outputLine1, outputLine2);
385 }
386
387 /**
388 * Returns linear mapping matrix to perform affine transformation.
389 * Point transformation is computed as a * x + t, where x is a point and t
390 * is the amount of translation.
391 *
392 * @return linear mapping matrix.
393 */
394 public Matrix getA() {
395 return a;
396 }
397
398 /**
399 * Sets linear mapping matrix to perform affine transformation.
400 *
401 * @param a linear mapping matrix.
402 * @throws NullPointerException raised if provided matrix is null.
403 * @throws IllegalArgumentException raised if provided matrix does not have
404 * size 3x3.
405 */
406 public final void setA(final Matrix a) {
407 if (a == null) {
408 throw new NullPointerException();
409 }
410 if (a.getRows() != INHOM_COORDS || a.getColumns() != INHOM_COORDS) {
411 throw new IllegalArgumentException();
412 }
413
414 this.a = a;
415 }
416
417 /**
418 * Returns 3D rotation assigned to this transformation.
419 * Note: if this rotation instance is modified, its changes won't be
420 * reflected on this instance until rotation is set again.
421 *
422 * @return 3D rotation.
423 * @throws AlgebraException if for some reason rotation cannot
424 * be estimated (usually because of numerical instability).
425 */
426 public Rotation3D getRotation() throws AlgebraException {
427 // Use QR decomposition to retrieve rotation
428 final var decomposer = new RQDecomposer(a);
429 try {
430 decomposer.decompose();
431 return new MatrixRotation3D(decomposer.getQ());
432 } catch (final InvalidRotationMatrixException ignore) {
433 return null;
434 }
435 }
436
437 /**
438 * Sets 3D rotation for this transformation.
439 *
440 * @param rotation a 3D rotation.
441 * @throws NullPointerException raised if provided rotation is null.
442 * @throws AlgebraException raised if for numerical reasons rotation cannot
443 * be set (usually because of numerical instability in parameters of this
444 * transformation).
445 */
446 public void setRotation(final Rotation3D rotation) throws AlgebraException {
447 final var rotMatrix = rotation.asInhomogeneousMatrix();
448
449 // Use QR decomposition to retrieve parameters matrix
450 final var decomposer = new RQDecomposer(a);
451 decomposer.decompose();
452 // retrieves params matrix
453 final var localA = decomposer.getR();
454 localA.multiply(rotMatrix);
455 this.a = localA;
456 }
457
458 /**
459 * Adds provided rotation to current rotation assigned to this
460 * transformation.
461 *
462 * @param rotation 3D rotation to be added.
463 * @throws AlgebraException raised if for numerical reasons rotation cannot
464 * be set (usually because of numerical instability in parameters of this
465 * transformation).
466 */
467 public void addRotation(final Rotation3D rotation) throws AlgebraException {
468 final var localRotation = getRotation();
469 localRotation.combine(rotation);
470 setRotation(localRotation);
471 }
472
473 /**
474 * Sets scale of this transformation.
475 *
476 * @param scale scale value to be set. a value between 0.0 and 1.0 indicates
477 * that objects will be reduced, a value greater than 1.0 indicates that
478 * objects will be enlarged, and a negative value indicates that objects
479 * will be reversed.
480 * @throws AlgebraException raised if for numerical reasons scale cannot
481 * be set (usually because of numerical instability in parameters of this
482 * transformation).
483 */
484 public void setScale(final double scale) throws AlgebraException {
485 final var decomposer = new RQDecomposer(a);
486 decomposer.decompose();
487 // params
488 final var localA = decomposer.getR();
489 localA.setElementAt(0, 0, scale);
490 localA.setElementAt(1, 1, scale);
491 // multiply by rotation
492 localA.multiply(decomposer.getQ());
493 this.a = localA;
494 }
495
496 /**
497 * Gets affine parameters of this instance.
498 * Affine parameters contain horizontal scale, vertical scale and skewness
499 * of axes.
500 *
501 * @return affine parameters.
502 * @throws AlgebraException raised if for numerical reasons affine
503 * parameters cannot be retrieved (usually because of numerical instability
504 * in matrix a).
505 */
506 public AffineParameters3D getParameters() throws AlgebraException {
507 final var parameters = new AffineParameters3D();
508 getParameters(parameters);
509 return parameters;
510 }
511
512 /**
513 * Computes affine parameters of this instance and stores the result in
514 * provided instance.
515 * Affine parameters contain horizontal scale, vertical scale and skewness
516 * of axes.
517 *
518 * @param result instance where affine parameters will be stored.
519 * @throws AlgebraException raised if for numerical reasons affine
520 * parameters cannot be retrieved (usually because of numerical instability
521 * in matrix a).
522 */
523 public void getParameters(final AffineParameters3D result) throws AlgebraException {
524 final var decomposer = new RQDecomposer(a);
525 decomposer.decompose();
526 final var params = decomposer.getR();
527 result.fromMatrix(params);
528 }
529
530 /**
531 * Sets affine parameters of this instance.
532 * Affine parameters contain horizontal scale, vertical scale and skewness
533 * of axes.
534 *
535 * @param parameters affine parameters to be set.
536 * @throws AlgebraException raised if for numerical reasons affine
537 * parameters cannot be set (usually because of numerical instability in
538 * current matrix a).
539 */
540 public void setParameters(final AffineParameters3D parameters) throws AlgebraException {
541 final var decomposer = new RQDecomposer(a);
542 decomposer.decompose();
543 final var params = parameters.asMatrix();
544 final var rotation = decomposer.getQ();
545
546 params.multiply(rotation);
547 a = params;
548 }
549
550 /**
551 * Returns 3D translation assigned to this transformation as an array
552 * expressed in inhomogeneous coordinates.
553 *
554 * @return 3D translation array.
555 */
556 public double[] getTranslation() {
557 return translation;
558 }
559
560 /**
561 * Sets 3D translation assigned to this transformation as an array expressed
562 * in inhomogeneous coordinates.
563 *
564 * @param translation 3D translation array.
565 * @throws IllegalArgumentException Raised if provided array does not have
566 * length equal to NUM_TRANSLATION_COORDS.
567 */
568 public void setTranslation(final double[] translation) {
569 if (translation.length != NUM_TRANSLATION_COORDS) {
570 throw new IllegalArgumentException();
571 }
572
573 this.translation = translation;
574 }
575
576 /**
577 * Adds provided translation to current translation on this transformation.
578 * Provided translation must be expressed as an array of inhomogeneous
579 * coordinates.
580 *
581 * @param translation 3D translation array.
582 * @throws IllegalArgumentException Raised if provided array does not have
583 * length equal to NUM_TRANSLATION_COORDS.
584 */
585 public void addTranslation(final double[] translation) {
586 ArrayUtils.sum(this.translation, translation, this.translation);
587 }
588
589 /**
590 * Returns current x coordinate translation assigned to this transformation.
591 *
592 * @return X coordinate translation.
593 */
594 public double getTranslationX() {
595 return translation[0];
596 }
597
598 /**
599 * Sets x coordinate translation to be made by this transformation.
600 *
601 * @param translationX X coordinate translation to be set.
602 */
603 public void setTranslationX(final double translationX) {
604 translation[0] = translationX;
605 }
606
607 /**
608 * Returns current y coordinate translation assigned to this transformation.
609 *
610 * @return Y coordinate translation.
611 */
612 public double getTranslationY() {
613 return translation[1];
614 }
615
616 /**
617 * Sets y coordinate translation to be made by this transformation.
618 *
619 * @param translationY Y coordinate translation to be set.
620 */
621 public void setTranslationY(final double translationY) {
622 translation[1] = translationY;
623 }
624
625 /**
626 * Returns current z coordinate translation assigned to this transformation.
627 *
628 * @return Z coordinate translation.
629 */
630 public double getTranslationZ() {
631 return translation[2];
632 }
633
634 /**
635 * Sets z coordinate translation to be made by this transformation.
636 *
637 * @param translationZ z coordinate translation to be set.
638 */
639 public void setTranslationZ(final double translationZ) {
640 translation[2] = translationZ;
641 }
642
643 /**
644 * Sets x, y, z coordinates of translation to be made by this
645 * transformation.
646 *
647 * @param translationX translation x coordinate to be set.
648 * @param translationY translation y coordinate to be set.
649 * @param translationZ translation z coordinate to be set.
650 */
651 public void setTranslation(final double translationX, final double translationY, final double translationZ) {
652 translation[0] = translationX;
653 translation[1] = translationY;
654 translation[2] = translationZ;
655 }
656
657 /**
658 * Sets x, y, z coordinates of translation to be made by this
659 * transformation.
660 *
661 * @param translation translation to be set.
662 */
663 public void setTranslation(final Point3D translation) {
664 setTranslation(translation.getInhomX(), translation.getInhomY(), translation.getInhomZ());
665 }
666
667 /**
668 * Gets x, y, z coordinates of translation to be made by this transformation
669 * as a new point.
670 *
671 * @return a new point containing translation coordinates.
672 */
673 public Point3D getTranslationPoint() {
674 final var out = Point3D.create();
675 getTranslationPoint(out);
676 return out;
677 }
678
679 /**
680 * Gets x, y, z coordinates of translation to be made by this transformation
681 * and stores them into provided point.
682 *
683 * @param out point where translation coordinates will be stored.
684 */
685 public void getTranslationPoint(final Point3D out) {
686 out.setInhomogeneousCoordinates(translation[0], translation[1], translation[2]);
687 }
688
689 /**
690 * Adds provided x coordinate to current translation assigned to this
691 * transformation.
692 *
693 * @param translationX X coordinate to be added to current translation.
694 */
695 public void addTranslationX(final double translationX) {
696 translation[0] += translationX;
697 }
698
699 /**
700 * Adds provided y coordinate to current translation assigned to this
701 * transformation.
702 *
703 * @param translationY Y coordinate to be added to current translation.
704 */
705 public void addTranslationY(final double translationY) {
706 translation[1] += translationY;
707 }
708
709 /**
710 * Adds provided z coordinate to current translation assigned to this
711 * transformation.
712 *
713 * @param translationZ Z coordinate to be added to current translation.
714 */
715 public void addTranslationZ(final double translationZ) {
716 translation[2] += translationZ;
717 }
718
719 /**
720 * Adds provided coordinates to current translation assigned to this
721 * transformation.
722 *
723 * @param translationX x coordinate to be added to current translation.
724 * @param translationY y coordinate to be added to current translation.
725 * @param translationZ z coordinate to be added to current translation.
726 */
727 public void addTranslation(final double translationX, final double translationY, final double translationZ) {
728 translation[0] += translationX;
729 translation[1] += translationY;
730 translation[2] += translationZ;
731 }
732
733 /**
734 * Adds provided coordinates to current translation assigned to this
735 * transformation.
736 *
737 * @param translation x, y, z coordinates to be added to current
738 * translation.
739 */
740 public void addTranslation(final Point3D translation) {
741 addTranslation(translation.getInhomX(), translation.getInhomY(), translation.getInhomZ());
742 }
743
744 /**
745 * Represents this transformation as a 4x4 matrix.
746 * a point can be transformed as T * p, where T is the transformation matrix
747 * and p is a point expressed as an homogeneous vector.
748 *
749 * @return This transformation in matrix form.
750 */
751 @Override
752 public Matrix asMatrix() {
753 Matrix m = null;
754 try {
755 m = new Matrix(HOM_COORDS, HOM_COORDS);
756 asMatrix(m);
757 } catch (final WrongSizeException ignore) {
758 // never happens
759 }
760 return m;
761 }
762
763 /**
764 * Represents this transformation as a 4x4 matrix and stores the result in
765 * provided instance.
766 *
767 * @param m instance where transformation matrix will be stored.
768 * @throws IllegalArgumentException raised if provided instance is not a 4x4
769 * matrix.
770 */
771 @Override
772 public void asMatrix(final Matrix m) {
773 if (m.getRows() != HOM_COORDS || m.getColumns() != HOM_COORDS) {
774 throw new IllegalArgumentException();
775 }
776
777 // set rotation
778 m.setSubmatrix(0, 0, 2, 2, a);
779
780 // set translation
781 m.setSubmatrix(0, 3, 2, 3, translation);
782
783 // set last element
784 m.setElementAt(3, 0, 0.0);
785 m.setElementAt(3, 1, 0.0);
786 m.setElementAt(3, 2, 0.0);
787 m.setElementAt(3, 3, 1.0);
788 }
789
790 /**
791 * Transforms input point using this transformation and stores the result in
792 * provided output points.
793 *
794 * @param inputPoint point to be transformed.
795 * @param outputPoint instance where transformed point data will be stored.
796 */
797 @Override
798 public void transform(final Point3D inputPoint, final Point3D outputPoint) {
799 try {
800 final var coords = new double[Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH];
801 coords[0] = inputPoint.getInhomX();
802 coords[1] = inputPoint.getInhomY();
803 coords[2] = inputPoint.getInhomZ();
804
805 final var p = a.multiplyAndReturnNew(Matrix.newFromArray(coords, true));
806
807 outputPoint.setInhomogeneousCoordinates(p.getElementAtIndex(0) + translation[0],
808 p.getElementAtIndex(1) + translation[1], p.getElementAtIndex(2) + translation[2]);
809 } catch (final WrongSizeException ignore) {
810 // this exception will never be raised
811 }
812 }
813
814 /**
815 * Transforms a quadric using this transformation and stores the result into
816 * provided output quadric.
817 *
818 * @param inputQuadric quadric to be transformed.
819 * @param outputQuadric instance where data of transformed quadric will be
820 * stored.
821 * @throws NonSymmetricMatrixException raised if due to numerical precision
822 * the resulting output conic matrix is not considered to be symmetric.
823 * @throws AlgebraException raised if transform cannot be computed because of
824 * numerical instabilities.
825 */
826 @Override
827 public void transform(final Quadric inputQuadric, final Quadric outputQuadric) throws NonSymmetricMatrixException,
828 AlgebraException {
829 // point' * quadric * point = 0
830 // point' * T' * transformedQuadric * T * point = 0
831 // where:
832 // - transformedPoint = T * point
833
834 // Hence:
835 // transformedQuadric = T^-1' * quadric * T^-1
836
837 inputQuadric.normalize();
838
839 final var q = inputQuadric.asMatrix();
840 final var invT = inverseAndReturnNew().asMatrix();
841 // normalize transformation matrix invT to increase accuracy
842 var norm = Utils.normF(invT);
843 invT.multiplyByScalar(1.0 / norm);
844
845 final var m = invT.transposeAndReturnNew();
846 try {
847 m.multiply(q);
848 m.multiply(invT);
849 } catch (final WrongSizeException ignore) {
850 // never happens
851 }
852
853 // normalize resulting m matrix to increase accuracy so that it can be
854 // considered symmetric
855 norm = Utils.normF(m);
856 m.multiplyByScalar(1.0 / norm);
857
858 outputQuadric.setParameters(m);
859 }
860
861 /**
862 * Transforms a dual quadric using this transformation and stores the result
863 * into provided output dual quadric.
864 *
865 * @param inputDualQuadric dual quadric to be transformed.
866 * @param outputDualQuadric instance where data of transformed dual quadric
867 * will be stored.
868 * @throws NonSymmetricMatrixException raised if due to numerical precision
869 * the resulting output dual conic matrix is not considered to be symmetric.
870 */
871 @Override
872 public void transform(final DualQuadric inputDualQuadric, final DualQuadric outputDualQuadric)
873 throws NonSymmetricMatrixException {
874 // plane' * dualQuadric * plane = 0
875 // plane' * T^-1 * T * dualQuadric * T' * T^-1'*plane
876
877 // Hence:
878 // transformed plane: T^-1'*plane
879 // transformed dual quadric: T * dualQuadric * T'
880
881 inputDualQuadric.normalize();
882
883 final var dualQ = inputDualQuadric.asMatrix();
884 final var t = asMatrix();
885 // normalize transformation matrix T to increase accuracy
886 var norm = Utils.normF(t);
887 t.multiplyByScalar(1.0 / norm);
888
889 final var transT = t.transposeAndReturnNew();
890 try {
891 t.multiply(dualQ);
892 t.multiply(transT);
893 } catch (final WrongSizeException ignore) {
894 // never happens
895 }
896
897 // normalize resulting m matrix to increase accuracy so that it can be
898 // considered symmetric
899 norm = Utils.normF(t);
900 t.multiplyByScalar(1.0 / norm);
901
902 outputDualQuadric.setParameters(t);
903 }
904
905 /**
906 * Transforms provided input plane using this transformation and stores the
907 * result into provided output plane instance.
908 *
909 * @param inputPlane plane to be transformed.
910 * @param outputPlane instance where data of transformed plane will be
911 * stored.
912 * @throws AlgebraException raised if transformAndReturnNew cannot be
913 * computed because of numerical instabilities.
914 */
915 @Override
916 public void transform(final Plane inputPlane, final Plane outputPlane) throws AlgebraException {
917 // plane' * point = 0 --> plane' * T^-1 * T * point
918 // (plane' * T^-1)*(T*point) = (T^-1'*plane)'*(T*point)
919 // where:
920 // - transformedPlane = T^-1'*plane
921 // - transformedPoint = T*point
922
923 inputPlane.normalize();
924
925 final var invT = inverseAndReturnNew().asMatrix();
926 final var p = Matrix.newFromArray(inputPlane.asArray());
927
928 // normalize transformation matrix T to increase accuracy
929 final var norm = Utils.normF(invT);
930 invT.multiplyByScalar(1.0 / norm);
931
932 invT.transpose();
933 invT.multiply(p);
934
935 outputPlane.setParameters(invT.toArray());
936 }
937
938 /**
939 * Transforms a camera using this transformation and stores the result into
940 * provided output camera.
941 *
942 * @param inputCamera camera to be transformed.
943 * @param outputCamera instance where data of transformed camera will be
944 * stored.
945 * @throws AlgebraException raised if transform cannot be computed because
946 * of numerical instabilities.
947 */
948 @Override
949 public void transform(final PinholeCamera inputCamera, final PinholeCamera outputCamera) throws AlgebraException {
950
951 inputCamera.normalize();
952
953 final var invT = inverseAndReturnNew().asMatrix();
954 final var c = inputCamera.getInternalMatrix();
955 c.multiply(invT);
956 outputCamera.setInternalMatrix(c);
957 }
958
959 /**
960 * Inverses this transformation.
961 *
962 * @throws AlgebraException if inverse transform cannot be computed because
963 * of numerical instabilities.
964 */
965 public void inverse() throws AlgebraException {
966 inverse(this);
967 }
968
969 /**
970 * Computes the inverse of this transformation and returns the result as a
971 * new transformation instance.
972 *
973 * @return inverse transformation.
974 * @throws AlgebraException if inverse transform cannot be computed because
975 * of numerical instabilities.
976 */
977 public Transformation3D inverseAndReturnNew() throws AlgebraException {
978 final var result = new AffineTransformation3D();
979 inverse(result);
980 return result;
981 }
982
983 /**
984 * Computes the inverse of this transformation and stores the result in
985 * provided instance.
986 *
987 * @param result instance where inverse transformation will be stored.
988 * @throws AlgebraException if inverse transform cannot be computed because
989 * of numerical instabilities.
990 */
991 public void inverse(final AffineTransformation3D result) throws AlgebraException {
992
993 // x' = a * x + t -->
994 // a^-1 * x' = a^-1 * a * x + a^-1 * t = x + a^-1 * t -->
995 // x = a^-1 * x' - a^-1 * t
996
997 try {
998 // reverse rotation
999 final var invA = Utils.inverse(a);
1000 result.a = invA;
1001
1002 // reverse translation
1003 final var t = Matrix.newFromArray(translation, true);
1004 t.multiplyByScalar(-1.0);
1005
1006 final var resultT = invA.multiplyAndReturnNew(t);
1007 result.translation = resultT.toArray();
1008 } catch (final WrongSizeException ignore) {
1009 // never happens
1010 }
1011 }
1012
1013 /**
1014 * Converts this transformation into a metric transformation.
1015 *
1016 * @return This transformation converted into a projective transformation.
1017 */
1018 public ProjectiveTransformation3D toProjective() {
1019 return new ProjectiveTransformation3D(a, translation);
1020 }
1021
1022 /**
1023 * Combines this transformation with provided transformation.
1024 * The combination is equivalent to multiplying the matrix of this
1025 * transformation with the matrix of provided transformation.
1026 *
1027 * @param transformation Transformation to be combined with.
1028 */
1029 public void combine(final AffineTransformation3D transformation) {
1030 combine(transformation, this);
1031 }
1032
1033 /**
1034 * Combines this transformation with provided transformation and returns
1035 * the result as a new transformation instance.
1036 * The combination is equivalent to multiplying the matrix of this
1037 * transformation with the matrix of provided transformation.
1038 *
1039 * @param transformation Transformation to be combined with.
1040 * @return a new transformation resulting of the combination with this
1041 * transformation and provided transformation.
1042 */
1043 public AffineTransformation3D combineAndReturnNew(final AffineTransformation3D transformation) {
1044
1045 final var result = new AffineTransformation3D();
1046 combine(transformation, result);
1047 return result;
1048 }
1049
1050 /**
1051 * Combines this transformation with provided input transformation and
1052 * stores the result into provided output transformation.
1053 * The combination is equivalent to multiplying the matrix of this
1054 * transformation with the matrix of provided input transformation.
1055 *
1056 * @param inputTransformation transformation to be combined with.
1057 * @param outputTransformation transformation where result will be stored.
1058 */
1059 private void combine(final AffineTransformation3D inputTransformation,
1060 final AffineTransformation3D outputTransformation) {
1061 // combination in matrix representation is:
1062 // [A1 t1] * [A2 t2] = [A1*A2 + t1*0T A1*t2 + t1*1] = [A1*A2 A1*t2 + t1]
1063 // [0T 1 ] [0T 1 ] [0T*A2 + 1*0T 0T*t2 + 1*1 ] [0T 1 ]
1064
1065 try {
1066 // we do translation first, because this.rotation might change later
1067 final var a1 = new Matrix(this.a);
1068 final var t2 = Matrix.newFromArray(inputTransformation.translation, true);
1069 // this is R1 * t2
1070 a1.multiply(t2);
1071
1072 ArrayUtils.sum(a1.toArray(), this.translation, outputTransformation.translation);
1073
1074 outputTransformation.a = this.a.multiplyAndReturnNew(inputTransformation.a);
1075
1076 } catch (final WrongSizeException ignore) {
1077 // never happens
1078 }
1079 }
1080
1081 /**
1082 * Estimates this transformation internal parameters by using 4
1083 * corresponding original and transformed points.
1084 *
1085 * @param inputPoint1 1st input point.
1086 * @param inputPoint2 2nd input point.
1087 * @param inputPoint3 3rd input point.
1088 * @param inputPoint4 4th input point.
1089 * @param outputPoint1 1st transformed point corresponding to 1st input
1090 * point.
1091 * @param outputPoint2 2nd transformed point corresponding to 2nd input
1092 * point.
1093 * @param outputPoint3 3rd transformed point corresponding to 3rd input
1094 * point.
1095 * @param outputPoint4 4th transformed point corresponding to 4th input
1096 * point.
1097 * @throws CoincidentPointsException raised if transformation cannot be
1098 * estimated for some reason (point configuration degeneracy, duplicate
1099 * points or numerical instabilities).
1100 */
1101 public final void setTransformationFromPoints(
1102 final Point3D inputPoint1, final Point3D inputPoint2, final Point3D inputPoint3, final Point3D inputPoint4,
1103 final Point3D outputPoint1, final Point3D outputPoint2, final Point3D outputPoint3,
1104 final Point3D outputPoint4) throws CoincidentPointsException {
1105
1106 // normalize points to increase accuracy
1107 inputPoint1.normalize();
1108 inputPoint2.normalize();
1109 inputPoint3.normalize();
1110 inputPoint4.normalize();
1111
1112 outputPoint1.normalize();
1113 outputPoint2.normalize();
1114 outputPoint3.normalize();
1115 outputPoint4.normalize();
1116
1117 // matrix of homogeneous linear system of equations.
1118 // There are 13 unknowns and 12 equations (3 for each pair of
1119 // corresponding points)
1120 Matrix m = null;
1121 try {
1122 // build matrix initialized to zero
1123 m = new Matrix(12, 13);
1124
1125 // 1st pair of points
1126 var iX = inputPoint1.getHomX();
1127 var iY = inputPoint1.getHomY();
1128 var iZ = inputPoint1.getHomZ();
1129 var iW = inputPoint1.getHomW();
1130
1131 var oX = outputPoint1.getHomX();
1132 var oY = outputPoint1.getHomY();
1133 var oZ = outputPoint1.getHomZ();
1134 var oW = outputPoint1.getHomW();
1135
1136 var oWiX = oW * iX;
1137 var oWiY = oW * iY;
1138 var oWiZ = oW * iZ;
1139 var oWiW = oW * iW;
1140
1141 var oXiW = oX * iW;
1142 var oYiW = oY * iW;
1143 var oZiW = oZ * iW;
1144
1145 var tmp = oWiX * oWiX + oWiY * oWiY + oWiZ * oWiZ + oWiW * oWiW;
1146 var norm = Math.sqrt(tmp + oXiW * oXiW);
1147
1148 m.setElementAt(0, 0, oWiX / norm);
1149 m.setElementAt(0, 1, oWiY / norm);
1150 m.setElementAt(0, 2, oWiZ / norm);
1151 m.setElementAt(0, 9, oWiW / norm);
1152 m.setElementAt(0, 12, -oXiW / norm);
1153
1154 norm = Math.sqrt(tmp + oYiW * oYiW);
1155
1156 m.setElementAt(1, 3, oWiX / norm);
1157 m.setElementAt(1, 4, oWiY / norm);
1158 m.setElementAt(1, 5, oWiZ / norm);
1159 m.setElementAt(1, 10, oWiW / norm);
1160 m.setElementAt(1, 12, -oYiW / norm);
1161
1162 norm = Math.sqrt(tmp + oZiW * oZiW);
1163
1164 m.setElementAt(2, 6, oWiX / norm);
1165 m.setElementAt(2, 7, oWiY / norm);
1166 m.setElementAt(2, 8, oWiZ / norm);
1167 m.setElementAt(2, 11, oWiW / norm);
1168 m.setElementAt(2, 12, -oZiW / norm);
1169
1170 // 2nd pair of points
1171 iX = inputPoint2.getHomX();
1172 iY = inputPoint2.getHomY();
1173 iZ = inputPoint2.getHomZ();
1174 iW = inputPoint2.getHomW();
1175
1176 oX = outputPoint2.getHomX();
1177 oY = outputPoint2.getHomY();
1178 oZ = outputPoint2.getHomZ();
1179 oW = outputPoint2.getHomW();
1180
1181 oWiX = oW * iX;
1182 oWiY = oW * iY;
1183 oWiZ = oW * iZ;
1184 oWiW = oW * iW;
1185
1186 oXiW = oX * iW;
1187 oYiW = oY * iW;
1188 oZiW = oZ * iW;
1189
1190 tmp = oWiX * oWiX + oWiY * oWiY + oWiZ * oWiZ + oWiW * oWiW;
1191 norm = Math.sqrt(tmp + oXiW * oXiW);
1192
1193 m.setElementAt(3, 0, oWiX / norm);
1194 m.setElementAt(3, 1, oWiY / norm);
1195 m.setElementAt(3, 2, oWiZ / norm);
1196 m.setElementAt(3, 9, oWiW / norm);
1197 m.setElementAt(3, 12, -oXiW / norm);
1198
1199 norm = Math.sqrt(tmp + oYiW * oYiW);
1200
1201 m.setElementAt(4, 3, oWiX / norm);
1202 m.setElementAt(4, 4, oWiY / norm);
1203 m.setElementAt(4, 5, oWiZ / norm);
1204 m.setElementAt(4, 10, oWiW / norm);
1205 m.setElementAt(4, 12, -oYiW / norm);
1206
1207 norm = Math.sqrt(tmp + oZiW * oZiW);
1208
1209 m.setElementAt(5, 6, oWiX / norm);
1210 m.setElementAt(5, 7, oWiY / norm);
1211 m.setElementAt(5, 8, oWiZ / norm);
1212 m.setElementAt(5, 11, oWiW / norm);
1213 m.setElementAt(5, 12, -oZiW / norm);
1214
1215 // 3rd pair of points
1216 iX = inputPoint3.getHomX();
1217 iY = inputPoint3.getHomY();
1218 iZ = inputPoint3.getHomZ();
1219 iW = inputPoint3.getHomW();
1220
1221 oX = outputPoint3.getHomX();
1222 oY = outputPoint3.getHomY();
1223 oZ = outputPoint3.getHomZ();
1224 oW = outputPoint3.getHomW();
1225
1226 oWiX = oW * iX;
1227 oWiY = oW * iY;
1228 oWiZ = oW * iZ;
1229 oWiW = oW * iW;
1230
1231 oXiW = oX * iW;
1232 oYiW = oY * iW;
1233 oZiW = oZ * iW;
1234
1235 tmp = oWiX * oWiX + oWiY * oWiY + oWiZ * oWiZ + oWiW * oWiW;
1236 norm = Math.sqrt(tmp + oXiW * oXiW);
1237
1238 m.setElementAt(6, 0, oWiX / norm);
1239 m.setElementAt(6, 1, oWiY / norm);
1240 m.setElementAt(6, 2, oWiZ / norm);
1241 m.setElementAt(6, 9, oWiW / norm);
1242 m.setElementAt(6, 12, -oXiW / norm);
1243
1244 norm = Math.sqrt(tmp + oYiW * oYiW);
1245
1246 m.setElementAt(7, 3, oWiX / norm);
1247 m.setElementAt(7, 4, oWiY / norm);
1248 m.setElementAt(7, 5, oWiZ / norm);
1249 m.setElementAt(7, 10, oWiW / norm);
1250 m.setElementAt(7, 12, -oYiW / norm);
1251
1252 norm = Math.sqrt(tmp + oZiW * oZiW);
1253
1254 m.setElementAt(8, 6, oWiX / norm);
1255 m.setElementAt(8, 7, oWiY / norm);
1256 m.setElementAt(8, 8, oWiZ / norm);
1257 m.setElementAt(8, 11, oWiW / norm);
1258 m.setElementAt(8, 12, -oZiW / norm);
1259
1260 // 4th pair of points
1261 iX = inputPoint4.getHomX();
1262 iY = inputPoint4.getHomY();
1263 iZ = inputPoint4.getHomZ();
1264 iW = inputPoint4.getHomW();
1265
1266 oX = outputPoint4.getHomX();
1267 oY = outputPoint4.getHomY();
1268 oZ = outputPoint4.getHomZ();
1269 oW = outputPoint4.getHomW();
1270
1271 oWiX = oW * iX;
1272 oWiY = oW * iY;
1273 oWiZ = oW * iZ;
1274 oWiW = oW * iW;
1275
1276 oXiW = oX * iW;
1277 oYiW = oY * iW;
1278 oZiW = oZ * iW;
1279
1280 tmp = oWiX * oWiX + oWiY * oWiY + oWiZ * oWiZ + oWiW * oWiW;
1281 norm = Math.sqrt(tmp + oXiW * oXiW);
1282
1283 m.setElementAt(9, 0, oWiX / norm);
1284 m.setElementAt(9, 1, oWiY / norm);
1285 m.setElementAt(9, 2, oWiZ / norm);
1286 m.setElementAt(9, 9, oWiW / norm);
1287 m.setElementAt(9, 12, -oXiW / norm);
1288
1289 norm = Math.sqrt(tmp + oYiW * oYiW);
1290
1291 m.setElementAt(10, 3, oWiX / norm);
1292 m.setElementAt(10, 4, oWiY / norm);
1293 m.setElementAt(10, 5, oWiZ / norm);
1294 m.setElementAt(10, 10, oWiW / norm);
1295 m.setElementAt(10, 12, -oYiW / norm);
1296
1297 norm = Math.sqrt(tmp + oZiW * oZiW);
1298
1299 m.setElementAt(11, 6, oWiX / norm);
1300 m.setElementAt(11, 7, oWiY / norm);
1301 m.setElementAt(11, 8, oWiZ / norm);
1302 m.setElementAt(11, 11, oWiW / norm);
1303 m.setElementAt(11, 12, -oZiW / norm);
1304 } catch (final WrongSizeException ignore) {
1305 // never happens
1306 }
1307
1308 // use SVD to decompose matrix m
1309 Matrix v;
1310 try {
1311 final var decomposer = new SingularValueDecomposer(m);
1312 decomposer.decompose();
1313
1314 // ensure that matrix m has enough rank and there is a unique
1315 // solution (up to scale)
1316 if (decomposer.getRank() < 12) {
1317 throw new CoincidentPointsException();
1318 }
1319 //V is 13x13
1320 v = decomposer.getV();
1321
1322 // last column of V will contain parameters of transformation
1323 final var value = v.getElementAt(12, 12);
1324 a.setElementAt(0, 0, v.getElementAt(0, 12) / value);
1325 a.setElementAt(0, 1, v.getElementAt(1, 12) / value);
1326 a.setElementAt(0, 2, v.getElementAt(2, 12) / value);
1327 a.setElementAt(1, 0, v.getElementAt(3, 12) / value);
1328 a.setElementAt(1, 1, v.getElementAt(4, 12) / value);
1329 a.setElementAt(1, 2, v.getElementAt(5, 12) / value);
1330 a.setElementAt(2, 0, v.getElementAt(6, 12) / value);
1331 a.setElementAt(2, 1, v.getElementAt(7, 12) / value);
1332 a.setElementAt(2, 2, v.getElementAt(8, 12) / value);
1333
1334 translation[0] = v.getElementAt(9, 12) / value;
1335 translation[1] = v.getElementAt(10, 12) / value;
1336 translation[2] = v.getElementAt(11, 12) / value;
1337
1338 } catch (final AlgebraException e) {
1339 throw new CoincidentPointsException(e);
1340 }
1341 }
1342
1343 /**
1344 * Estimates this transformation internal parameters by using 4
1345 * corresponding original and transformed planes.
1346 *
1347 * @param inputPlane1 1st input plane.
1348 * @param inputPlane2 2nd input plane.
1349 * @param inputPlane3 3rd input plane.
1350 * @param inputPlane4 4th input plane.
1351 * @param outputPlane1 1st transformed plane corresponding to 1st input
1352 * plane.
1353 * @param outputPlane2 2nd transformed plane corresponding to 2nd input
1354 * plane.
1355 * @param outputPlane3 3rd transformed plane corresponding to 3rd input
1356 * plane.
1357 * @param outputPlane4 4th transformed plane corresponding to 4th input
1358 * plane.
1359 * @throws CoincidentPlanesException raised if transformation cannot be
1360 * estimated for some reason (plane configuration degeneracy, duplicate
1361 * points or numerical instabilities).
1362 */
1363 public final void setTransformationFromPlanes(
1364 final Plane inputPlane1, final Plane inputPlane2, final Plane inputPlane3, final Plane inputPlane4,
1365 final Plane outputPlane1, final Plane outputPlane2, final Plane outputPlane3, final Plane outputPlane4)
1366 throws CoincidentPlanesException {
1367
1368 // normalize points to increase accuracy
1369 inputPlane1.normalize();
1370 inputPlane2.normalize();
1371 inputPlane3.normalize();
1372 inputPlane4.normalize();
1373
1374 outputPlane1.normalize();
1375 outputPlane2.normalize();
1376 outputPlane3.normalize();
1377 outputPlane4.normalize();
1378
1379 // matrix of homogeneous linear system of equations.
1380 // There are 13 unknowns and 12 equations (3 for each pair of
1381 // corresponding points)
1382 Matrix m = null;
1383 try {
1384 // build matrix initialized to zero
1385 m = new Matrix(12, 13);
1386
1387 // 1st pair of planes
1388 var iA = inputPlane1.getA();
1389 var iB = inputPlane1.getB();
1390 var iC = inputPlane1.getC();
1391 var iD = inputPlane1.getD();
1392
1393 var oA = outputPlane1.getA();
1394 var oB = outputPlane1.getB();
1395 var oC = outputPlane1.getC();
1396 var oD = outputPlane1.getD();
1397
1398 var oDiA = oD * iA;
1399 var oDiB = oD * iB;
1400 var oDiC = oD * iC;
1401
1402 var oAiA = oA * iA;
1403 var oAiB = oA * iB;
1404 var oAiC = oA * iC;
1405 var oAiD = oA * iD;
1406
1407 var oBiA = oB * iA;
1408 var oBiB = oB * iB;
1409 var oBiC = oB * iC;
1410 var oBiD = oB * iD;
1411
1412 var oCiA = oC * iA;
1413 var oCiB = oC * iB;
1414 var oCiC = oC * iC;
1415 var oCiD = oC * iD;
1416
1417 var tmp = oDiA * oDiA + oDiB * oDiB + oDiC * oDiC;
1418 var norm = Math.sqrt(tmp + oAiA * oAiA + oAiB * oAiB + oAiC * oAiC + oAiD * oAiD);
1419
1420 m.setElementAt(0, 0, oDiA / norm);
1421 m.setElementAt(0, 1, oDiB / norm);
1422 m.setElementAt(0, 2, oDiC / norm);
1423 m.setElementAt(0, 9, -oAiA / norm);
1424 m.setElementAt(0, 10, -oAiB / norm);
1425 m.setElementAt(0, 11, -oAiC / norm);
1426 m.setElementAt(0, 12, -oAiD / norm);
1427
1428 norm = Math.sqrt(tmp + oBiA * oBiA + oBiB * oBiB + oBiC * oBiC + oBiD * oBiD);
1429
1430 m.setElementAt(1, 3, oDiA / norm);
1431 m.setElementAt(1, 4, oDiB / norm);
1432 m.setElementAt(1, 5, oDiC / norm);
1433 m.setElementAt(1, 9, -oBiA / norm);
1434 m.setElementAt(1, 10, -oBiB / norm);
1435 m.setElementAt(1, 11, -oBiC / norm);
1436 m.setElementAt(1, 12, -oBiD / norm);
1437
1438 norm = Math.sqrt(tmp + oCiA * oCiA + oCiB * oCiB + oCiC * oCiC + oCiD * oCiD);
1439
1440 m.setElementAt(2, 6, oDiA / norm);
1441 m.setElementAt(2, 7, oDiB / norm);
1442 m.setElementAt(2, 8, oDiC / norm);
1443 m.setElementAt(2, 9, -oCiA / norm);
1444 m.setElementAt(2, 10, -oCiB / norm);
1445 m.setElementAt(2, 11, -oCiC / norm);
1446 m.setElementAt(2, 12, -oCiD / norm);
1447
1448 // 2nd pair of planes
1449 iA = inputPlane2.getA();
1450 iB = inputPlane2.getB();
1451 iC = inputPlane2.getC();
1452 iD = inputPlane2.getD();
1453
1454 oA = outputPlane2.getA();
1455 oB = outputPlane2.getB();
1456 oC = outputPlane2.getC();
1457 oD = outputPlane2.getD();
1458
1459 oDiA = oD * iA;
1460 oDiB = oD * iB;
1461 oDiC = oD * iC;
1462
1463 oAiA = oA * iA;
1464 oAiB = oA * iB;
1465 oAiC = oA * iC;
1466 oAiD = oA * iD;
1467
1468 oBiA = oB * iA;
1469 oBiB = oB * iB;
1470 oBiC = oB * iC;
1471 oBiD = oB * iD;
1472
1473 oCiA = oC * iA;
1474 oCiB = oC * iB;
1475 oCiC = oC * iC;
1476 oCiD = oC * iD;
1477
1478 tmp = oDiA * oDiA + oDiB * oDiB + oDiC * oDiC;
1479 norm = Math.sqrt(tmp + oAiA * oAiA + oAiB * oAiB + oAiC * oAiC + oAiD * oAiD);
1480
1481 m.setElementAt(3, 0, oDiA / norm);
1482 m.setElementAt(3, 1, oDiB / norm);
1483 m.setElementAt(3, 2, oDiC / norm);
1484 m.setElementAt(3, 9, -oAiA / norm);
1485 m.setElementAt(3, 10, -oAiB / norm);
1486 m.setElementAt(3, 11, -oAiC / norm);
1487 m.setElementAt(3, 12, -oAiD / norm);
1488
1489 norm = Math.sqrt(tmp + oBiA * oBiA + oBiB * oBiB + oBiC * oBiC + oBiD * oBiD);
1490
1491 m.setElementAt(4, 3, oDiA / norm);
1492 m.setElementAt(4, 4, oDiB / norm);
1493 m.setElementAt(4, 5, oDiC / norm);
1494 m.setElementAt(4, 9, -oBiA / norm);
1495 m.setElementAt(4, 10, -oBiB / norm);
1496 m.setElementAt(4, 11, -oBiC / norm);
1497 m.setElementAt(4, 12, -oBiD / norm);
1498
1499 norm = Math.sqrt(tmp + oCiA * oCiA + oCiB * oCiB + oCiC * oCiC + oCiD * oCiD);
1500
1501 m.setElementAt(5, 6, oDiA / norm);
1502 m.setElementAt(5, 7, oDiB / norm);
1503 m.setElementAt(5, 8, oDiC / norm);
1504 m.setElementAt(5, 9, -oCiA / norm);
1505 m.setElementAt(5, 10, -oCiB / norm);
1506 m.setElementAt(5, 11, -oCiC / norm);
1507 m.setElementAt(5, 12, -oCiD / norm);
1508
1509 // 3rd pair of planes
1510 iA = inputPlane3.getA();
1511 iB = inputPlane3.getB();
1512 iC = inputPlane3.getC();
1513 iD = inputPlane3.getD();
1514
1515 oA = outputPlane3.getA();
1516 oB = outputPlane3.getB();
1517 oC = outputPlane3.getC();
1518 oD = outputPlane3.getD();
1519
1520 oDiA = oD * iA;
1521 oDiB = oD * iB;
1522 oDiC = oD * iC;
1523
1524 oAiA = oA * iA;
1525 oAiB = oA * iB;
1526 oAiC = oA * iC;
1527 oAiD = oA * iD;
1528
1529 oBiA = oB * iA;
1530 oBiB = oB * iB;
1531 oBiC = oB * iC;
1532 oBiD = oB * iD;
1533
1534 oCiA = oC * iA;
1535 oCiB = oC * iB;
1536 oCiC = oC * iC;
1537 oCiD = oC * iD;
1538
1539 tmp = oDiA * oDiA + oDiB * oDiB + oDiC * oDiC;
1540 norm = Math.sqrt(tmp + oAiA * oAiA + oAiB * oAiB + oAiC * oAiC + oAiD * oAiD);
1541
1542 m.setElementAt(6, 0, oDiA / norm);
1543 m.setElementAt(6, 1, oDiB / norm);
1544 m.setElementAt(6, 2, oDiC / norm);
1545 m.setElementAt(6, 9, -oAiA / norm);
1546 m.setElementAt(6, 10, -oAiB / norm);
1547 m.setElementAt(6, 11, -oAiC / norm);
1548 m.setElementAt(6, 12, -oAiD / norm);
1549
1550 norm = Math.sqrt(tmp + oBiA * oBiA + oBiB * oBiB + oBiC * oBiC + oBiD * oBiD);
1551
1552 m.setElementAt(7, 3, oDiA / norm);
1553 m.setElementAt(7, 4, oDiB / norm);
1554 m.setElementAt(7, 5, oDiC / norm);
1555 m.setElementAt(7, 9, -oBiA / norm);
1556 m.setElementAt(7, 10, -oBiB / norm);
1557 m.setElementAt(7, 11, -oBiC / norm);
1558 m.setElementAt(7, 12, -oBiD / norm);
1559
1560 norm = Math.sqrt(tmp + oCiA * oCiA + oCiB * oCiB + oCiC * oCiC + oCiD * oCiD);
1561
1562 m.setElementAt(8, 6, oDiA / norm);
1563 m.setElementAt(8, 7, oDiB / norm);
1564 m.setElementAt(8, 8, oDiC / norm);
1565 m.setElementAt(8, 9, -oCiA / norm);
1566 m.setElementAt(8, 10, -oCiB / norm);
1567 m.setElementAt(8, 11, -oCiC / norm);
1568 m.setElementAt(8, 12, -oCiD / norm);
1569
1570 // 4th pair of planes
1571 iA = inputPlane4.getA();
1572 iB = inputPlane4.getB();
1573 iC = inputPlane4.getC();
1574 iD = inputPlane4.getD();
1575
1576 oA = outputPlane4.getA();
1577 oB = outputPlane4.getB();
1578 oC = outputPlane4.getC();
1579 oD = outputPlane4.getD();
1580
1581 oDiA = oD * iA;
1582 oDiB = oD * iB;
1583 oDiC = oD * iC;
1584
1585 oAiA = oA * iA;
1586 oAiB = oA * iB;
1587 oAiC = oA * iC;
1588 oAiD = oA * iD;
1589
1590 oBiA = oB * iA;
1591 oBiB = oB * iB;
1592 oBiC = oB * iC;
1593 oBiD = oB * iD;
1594
1595 oCiA = oC * iA;
1596 oCiB = oC * iB;
1597 oCiC = oC * iC;
1598 oCiD = oC * iD;
1599
1600 tmp = oDiA * oDiA + oDiB * oDiB + oDiC * oDiC;
1601 norm = Math.sqrt(tmp + oAiA * oAiA + oAiB * oAiB + oAiC * oAiC + oAiD * oAiD);
1602
1603 m.setElementAt(9, 0, oDiA / norm);
1604 m.setElementAt(9, 1, oDiB / norm);
1605 m.setElementAt(9, 2, oDiC / norm);
1606 m.setElementAt(9, 9, -oAiA / norm);
1607 m.setElementAt(9, 10, -oAiB / norm);
1608 m.setElementAt(9, 11, -oAiC / norm);
1609 m.setElementAt(9, 12, -oAiD / norm);
1610
1611 norm = Math.sqrt(tmp + oBiA * oBiA + oBiB * oBiB + oBiC * oBiC + oBiD * oBiD);
1612
1613 m.setElementAt(10, 3, oDiA / norm);
1614 m.setElementAt(10, 4, oDiB / norm);
1615 m.setElementAt(10, 5, oDiC / norm);
1616 m.setElementAt(10, 9, -oBiA / norm);
1617 m.setElementAt(10, 10, -oBiB / norm);
1618 m.setElementAt(10, 11, -oBiC / norm);
1619 m.setElementAt(10, 12, -oBiD / norm);
1620
1621 norm = Math.sqrt(tmp + oCiA * oCiA + oCiB * oCiB + oCiC * oCiC + oCiD * oCiD);
1622
1623 m.setElementAt(11, 6, oDiA / norm);
1624 m.setElementAt(11, 7, oDiB / norm);
1625 m.setElementAt(11, 8, oDiC / norm);
1626 m.setElementAt(11, 9, -oCiA / norm);
1627 m.setElementAt(11, 10, -oCiB / norm);
1628 m.setElementAt(11, 11, -oCiC / norm);
1629 m.setElementAt(11, 12, -oCiD / norm);
1630 } catch (final WrongSizeException ignore) {
1631 // never happens
1632 }
1633
1634 // use SVD to decompose matrix m
1635 Matrix v;
1636 try {
1637 final var decomposer = new SingularValueDecomposer(m);
1638 decomposer.decompose();
1639
1640 // ensure that matrix m has enough rank and there is a unique
1641 // solution (up to scale)
1642 if (decomposer.getRank() < 12) {
1643 throw new CoincidentPlanesException();
1644 }
1645 // V is 13x13
1646 v = decomposer.getV();
1647
1648 // last column of V will contain parameters of transformation
1649 final var value = v.getElementAt(12, 12);
1650
1651 final var invTransA = new Matrix(AffineParameters3D.INHOM_COORDS, AffineParameters3D.INHOM_COORDS);
1652 // copy former 9 elements of 13th column of V into "a" in row order
1653 invTransA.setSubmatrix(0, 0, 2, 2,
1654 v.getSubmatrixAsArray(0, 12, 8, 12),
1655 false);
1656 // normalize by scale value
1657 invTransA.multiplyByScalar(1.0 / value);
1658
1659 // initially "a contains the inverse of its transpose, so to obtain "a", we need
1660 // to transpose it and invert it
1661 invTransA.transpose();
1662 final var a1 = Utils.inverse(invTransA);
1663
1664 final var invt = new Matrix(1, 3);
1665 invt.setSubmatrix(0, 0, 0, 2,
1666 v.getSubmatrixAsArray(9, 12, 11, 12),
1667 false);
1668 // normalize by scale value (we need to change sign as well)
1669 invt.multiplyByScalar(-1.0 / value);
1670 invt.transpose();
1671
1672 final var t = a1.multiplyAndReturnNew(invt);
1673
1674 this.a = a1;
1675 this.translation = t.getBuffer();
1676 } catch (final AlgebraException e) {
1677 throw new CoincidentPlanesException(e);
1678 }
1679 }
1680
1681 /**
1682 * Estimates this transformation internal parameters by using provided 2
1683 * corresponding original and transformed lines.
1684 *
1685 * @param inputLine1 1st input line.
1686 * @param inputLine2 2nd input line.
1687 * @param outputLine1 1st transformed line corresponding to 1st input line.
1688 * @param outputLine2 2nd transformed line corresponding to 2nd input line.
1689 * @throws CoincidentLinesException Raised if transformation cannot be
1690 * estimated for some reason (line configuration degeneracy, duplicate lines
1691 * or numerical instabilities).
1692 */
1693 public final void setTransformationFromLines(
1694 final Line3D inputLine1, final Line3D inputLine2, final Line3D outputLine1, final Line3D outputLine2)
1695 throws CoincidentLinesException {
1696 try {
1697 setTransformationFromPlanes(inputLine1.getPlane1(), inputLine1.getPlane2(), inputLine2.getPlane1(),
1698 inputLine2.getPlane2(), outputLine1.getPlane1(), outputLine1.getPlane2(), outputLine2.getPlane1(),
1699 outputLine2.getPlane2());
1700 } catch (final CoincidentPlanesException e) {
1701 throw new CoincidentLinesException(e);
1702 }
1703 }
1704 }