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.ArrayUtils;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.WrongSizeException;
21 import com.irurueta.geometry.estimators.LockedException;
22 import com.irurueta.geometry.estimators.MetricTransformation3DEstimator;
23 import com.irurueta.geometry.estimators.NotReadyException;
24
25 import java.io.Serializable;
26 import java.util.ArrayList;
27
28 /**
29 * This class performs metric transformations on 3D space.
30 * Metric transformations include transformations related to rotations,
31 * translations and scale.
32 */
33 @SuppressWarnings("DuplicatedCode")
34 public class MetricTransformation3D extends EuclideanTransformation3D implements Serializable {
35
36 /**
37 * Default scale factor, which leaves objects with the same scale.
38 */
39 public static final double DEFAULT_SCALE = 1.0;
40
41 /**
42 * Scale factor. Negative values mean that objects get reversed. Values
43 * greater than 1.0 means that objects get enlarged and values between 0.0
44 * and 1.0 means that objects get reduced.
45 */
46 private double scale;
47
48 /**
49 * Empty constructor.
50 * Creates transformation that has no effect.
51 */
52 public MetricTransformation3D() {
53 super();
54 scale = DEFAULT_SCALE;
55 }
56
57 /**
58 * Creates transformation with provided rotation.
59 *
60 * @param rotation a 3D rotation.
61 * @throws NullPointerException raised if provided rotation is null.
62 */
63 public MetricTransformation3D(final Rotation3D rotation) {
64 super(rotation);
65 scale = DEFAULT_SCALE;
66 }
67
68 /**
69 * Creates transformation with provided 3D translation.
70 *
71 * @param translation array indicating 3D translation using inhomogeneous
72 * coordinates.
73 * @throws NullPointerException raised if provided array is null.
74 * @throws IllegalArgumentException raised if length of array is not equal
75 * to NUM_TRANSLATION_COORDS.
76 */
77 public MetricTransformation3D(final double[] translation) {
78 super(translation);
79 scale = DEFAULT_SCALE;
80 }
81
82 /**
83 * Creates transformation with provided scale value.
84 *
85 * @param scale Scale value. Values between 0.0 and 1.0 reduce objects,
86 * values greater than 1.0 enlarge objects and negative values reverse
87 * objects.
88 */
89 public MetricTransformation3D(final double scale) {
90 super();
91 this.scale = scale;
92 }
93
94 /**
95 * Creates transformation with provided rotation, translation and scale
96 * value.
97 *
98 * @param rotation a 3D rotation.
99 * @param translation array indicating 3D translation using inhomogeneous
100 * coordinates.
101 * @param scale scale value. Values between 0.0 and 1.0 reduce objects,
102 * values greater than 1.0 enlarge objects and negative values reverse
103 * objects.
104 * @throws NullPointerException raised if provided array is null or if
105 * rotation is null.
106 * @throws IllegalArgumentException raised if length of array is not equal
107 * to NUM_TRANSLATION_COORDS.
108 */
109 public MetricTransformation3D(final Rotation3D rotation, final double[] translation, final double scale) {
110 super(rotation, translation);
111 this.scale = scale;
112 }
113
114 /**
115 * Creates transformation by estimating its internal values using provided 4
116 * corresponding original and transformed points.
117 *
118 * @param inputPoint1 1st input point.
119 * @param inputPoint2 2nd input point.
120 * @param inputPoint3 3rd input point.
121 * @param inputPoint4 4th input point.
122 * @param outputPoint1 1st transformed point corresponding to 1st input
123 * point.
124 * @param outputPoint2 2nd transformed point corresponding to 2nd input
125 * point.
126 * @param outputPoint3 3rd transformed point corresponding to 3rd input
127 * point.
128 * @param outputPoint4 4th transformed point corresponding to 4th input
129 * point.
130 * @throws CoincidentPointsException raised if transformation cannot be
131 * estimated for some reason (point configuration degeneracy, duplicate
132 * points or numerical instabilities).
133 */
134 public MetricTransformation3D(
135 final Point3D inputPoint1, final Point3D inputPoint2, final Point3D inputPoint3, final Point3D inputPoint4,
136 final Point3D outputPoint1, final Point3D outputPoint2, final Point3D outputPoint3,
137 final Point3D outputPoint4) throws CoincidentPointsException {
138 internalSetMetricTransformationFromPoints(inputPoint1, inputPoint2, inputPoint3, inputPoint4, outputPoint1,
139 outputPoint2, outputPoint3, outputPoint4);
140 }
141
142 /**
143 * Returns scale of this transformation.
144 * A value between 0.0 and 1.0 indicates that objects will be reduced,
145 * a value greater than 1.0 indicates that objects will be enlarged, and a
146 * negative value indicates that objects will be reversed.
147 *
148 * @return scale.
149 */
150 public double getScale() {
151 return scale;
152 }
153
154 /**
155 * Sets scale of this transformation.
156 *
157 * @param scale Scale value to be set. A value between 0.0 and 1.0 indicates
158 * that objects will be reduced, a value greater than 1.0 indicates that
159 * objects will be enlarged, and a negative value indicates that objects
160 * will be reversed.
161 */
162 public void setScale(final double scale) {
163 this.scale = scale;
164 }
165
166 /**
167 * Represents this transformation as a 4x4 matrix and stores the result in
168 * provided instance.
169 *
170 * @param m instance where transformation matrix will be stored.
171 * @throws IllegalArgumentException Raised if provided instance is not a 4x4
172 * matrix.
173 */
174 @Override
175 public void asMatrix(final Matrix m) {
176 if (m.getRows() != HOM_COORDS || m.getColumns() != HOM_COORDS) {
177 throw new IllegalArgumentException();
178 }
179
180 // set rotation
181 final var rot = getRotation().asInhomogeneousMatrix();
182 rot.multiplyByScalar(scale);
183
184 m.setSubmatrix(0, 0, Rotation3D.INHOM_COORDS - 1,
185 Rotation3D.INHOM_COORDS - 1, rot);
186
187 final var translation = getTranslation();
188
189 // set translation
190 m.setSubmatrix(0, HOM_COORDS - 1, translation.length - 1,
191 HOM_COORDS - 1, translation);
192
193 // set last element
194 m.setElementAt(HOM_COORDS - 1, HOM_COORDS - 1, 1.0);
195 }
196
197 /**
198 * Transforms input point using this transformation and stores the result in
199 * provided output points.
200 *
201 * @param inputPoint point to be transformed.
202 * @param outputPoint instance where transformed point data will be stored.
203 */
204 @Override
205 public void transform(final Point3D inputPoint, final Point3D outputPoint) {
206 inputPoint.normalize();
207 getRotation().rotate(inputPoint, outputPoint);
208
209 final var translation = getTranslation();
210
211 outputPoint.setInhomogeneousCoordinates(scale * outputPoint.getInhomX() + translation[0],
212 scale * outputPoint.getInhomY() + translation[1],
213 scale * outputPoint.getInhomZ() + translation[2]);
214 }
215
216 /**
217 * Inverses this transformation.
218 */
219 @Override
220 public void inverse() {
221 inverse(this);
222 }
223
224 /**
225 * Computes the inverse of this transformation and returns the result as a
226 * new transformation instance.
227 *
228 * @return inverse transformation.
229 */
230 @Override
231 public Transformation3D inverseAndReturnNew() {
232 final var result = new MetricTransformation3D();
233 inverse(result);
234 return result;
235 }
236
237 /**
238 * Computes the inverse of this transformation and stores the result in
239 * provided instance.
240 *
241 * @param result instance where inverse transformation will be stored.
242 */
243 protected void inverse(final MetricTransformation3D result) {
244 // Transformation is as follows: x' = s*R* x + t
245 // Then inverse transformation is: (1/s)*R* x' = (1/s) * R' * s * R * x +
246 // (1/s) * R'*t = x + (1/s) * R'*t
247 // --> x = (1/s) * R'*x' - (1/s) * R'*t
248 super.inverse(result);
249 final var translation = result.getTranslation();
250 final var invScale = 1.0 / scale;
251 ArrayUtils.multiplyByScalar(translation, invScale, translation);
252 result.scale = invScale;
253 }
254
255 /**
256 * Converts this transformation into a metric transformation.
257 * Because this method is inherited, and this instance is already metric,
258 * this method just returns a copy of this transformation.
259 *
260 * @return this transformation converted into a metric transformation.
261 */
262 @Override
263 public MetricTransformation3D toMetric() {
264 return new MetricTransformation3D(getRotation(), getTranslation(), scale);
265 }
266
267 /**
268 * Converts this transformation into an affine transformation.
269 *
270 * @return this transformation converted into an affine transformation.
271 */
272 public AffineTransformation3D toAffine() {
273 return new AffineTransformation3D(scale, getRotation(), getTranslation());
274 }
275
276 /**
277 * Combines this transformation with provided transformation.
278 * The combination is equivalent to multiplying the matrix of this
279 * transformation with the matrix of provided transformation.
280 *
281 * @param transformation transformation to be combined with.
282 */
283 @Override
284 public void combine(final EuclideanTransformation3D transformation) {
285 combine(transformation.toMetric(), this);
286 }
287
288 /**
289 * Combines this transformation with provided transformation and returns
290 * the result as a new transformation instance.
291 * The combination is equivalent to multiplying the matrix of this
292 * transformation with the matrix of provided transformation.
293 *
294 * @param transformation transformation to be combined with.
295 * @return A new transformation resulting of the combination with this
296 * transformation and provided transformation.
297 */
298 @Override
299 public MetricTransformation3D combineAndReturnNew(final EuclideanTransformation3D transformation) {
300 final var result = new MetricTransformation3D();
301 combine(transformation.toMetric(), result);
302 return result;
303 }
304
305 /**
306 * Combines this transformation with provided transformation.
307 * The combination is equivalent to multiplying the matrix of this
308 * transformation with the matrix of provided transformation.
309 *
310 * @param transformation transformation to be combined with.
311 */
312 public void combine(final MetricTransformation3D transformation) {
313 combine(transformation, this);
314 }
315
316 /**
317 * Combines this transformation with provided transformation and returns
318 * the result as a new transformation instance.
319 * The combination is equivalent to multiplying the matrix of this
320 * transformation with the matrix of provided transformation.
321 *
322 * @param transformation transformation to be combined with.
323 * @return a new transformation resulting of the combination with this
324 * transformation and provided transformation.
325 */
326 public MetricTransformation3D combineAndReturnNew(final MetricTransformation3D transformation) {
327
328 final var result = new MetricTransformation3D();
329 combine(transformation, result);
330 return result;
331 }
332
333 /**
334 * Estimates this transformation internal parameters by using 4
335 * corresponding original and transformed points.
336 *
337 * @param inputPoint1 1st input point.
338 * @param inputPoint2 2nd input point.
339 * @param inputPoint3 3rd input point.
340 * @param inputPoint4 4th input point.
341 * @param outputPoint1 1st transformed point corresponding to 1st input
342 * point.
343 * @param outputPoint2 2nd transformed point corresponding to 2nd input
344 * point.
345 * @param outputPoint3 3rd transformed point corresponding to 3rd input
346 * point.
347 * @param outputPoint4 4th transformed point corresponding to 4th input
348 * point.
349 * @throws CoincidentPointsException raised if transformation cannot be
350 * estimated for some reason (point configuration degeneracy, duplicate
351 * points or numerical instabilities).
352 */
353 @Override
354 public void setTransformationFromPoints(
355 final Point3D inputPoint1, final Point3D inputPoint2, final Point3D inputPoint3, final Point3D inputPoint4,
356 final Point3D outputPoint1, final Point3D outputPoint2, final Point3D outputPoint3,
357 final Point3D outputPoint4) throws CoincidentPointsException {
358 internalSetMetricTransformationFromPoints(inputPoint1, inputPoint2, inputPoint3, inputPoint4, outputPoint1,
359 outputPoint2, outputPoint3, outputPoint4);
360 }
361
362 /**
363 * Combines this transformation with provided input transformation and
364 * stores the result into provided output transformation.
365 * The combination is equivalent to multiplying the matrix of this
366 * transformation with the matrix of provided input transformation.
367 *
368 * @param inputTransformation transformation to be combined with.
369 * @param outputTransformation transformation where result will be stored.
370 */
371 private void combine(final MetricTransformation3D inputTransformation,
372 final MetricTransformation3D outputTransformation) {
373 // combination in matrix representation is:
374 // [s1*R1 t1] * [s2*R2 t2] = [s1*s2*R1*R2 + t1*0T s1*R1*t2 + t1*1] = [s1*s2*R1*R2 s1*R1*t2 + t1]
375 // [0T 1 ] [0T 1 ] [0T*s2*R2 + 1*0T 0T*t2 + 1*1 ] [0T 1 ]
376
377 try {
378 // we do translation first, because this.rotation might change later
379 final var r1 = getRotation().asInhomogeneousMatrix();
380 final var t2 = Matrix.newFromArray(inputTransformation.getTranslation(), true);
381 // this is R1 * t2
382 r1.multiply(t2);
383 r1.multiplyByScalar(this.scale);
384
385 ArrayUtils.sum(r1.toArray(), this.getTranslation(), outputTransformation.getTranslation());
386
387 outputTransformation.setRotation(this.getRotation().combineAndReturnNew(inputTransformation.getRotation()));
388
389 outputTransformation.scale = this.scale * inputTransformation.scale;
390
391 } catch (final WrongSizeException ignore) {
392 // never happens
393 }
394 }
395
396 /**
397 * Estimates this transformation internal parameters by using 4
398 * corresponding original and transformed points.
399 *
400 * @param inputPoint1 1st input point.
401 * @param inputPoint2 2nd input point.
402 * @param inputPoint3 3rd input point.
403 * @param inputPoint4 4th input point.
404 * @param outputPoint1 1st transformed point corresponding to 1st input
405 * point.
406 * @param outputPoint2 2nd transformed point corresponding to 2nd input
407 * point.
408 * @param outputPoint3 3rd transformed point corresponding to 3rd input
409 * point.
410 * @param outputPoint4 4th transformed point corresponding to 4th input
411 * point.
412 * @throws CoincidentPointsException raised if transformation cannot be
413 * estimated for some reason (point configuration degeneracy, duplicate
414 * points or numerical instabilities).
415 */
416 private void internalSetMetricTransformationFromPoints(
417 final Point3D inputPoint1, final Point3D inputPoint2, final Point3D inputPoint3, final Point3D inputPoint4,
418 final Point3D outputPoint1, final Point3D outputPoint2, final Point3D outputPoint3,
419 final Point3D outputPoint4) throws CoincidentPointsException {
420 final var inputPoints = new ArrayList<Point3D>();
421 inputPoints.add(inputPoint1);
422 inputPoints.add(inputPoint2);
423 inputPoints.add(inputPoint3);
424 inputPoints.add(inputPoint4);
425
426 final var outputPoints = new ArrayList<Point3D>();
427 outputPoints.add(outputPoint1);
428 outputPoints.add(outputPoint2);
429 outputPoints.add(outputPoint3);
430 outputPoints.add(outputPoint4);
431
432 final var estimator = new MetricTransformation3DEstimator(inputPoints, outputPoints);
433
434 try {
435 estimator.estimate(this);
436 } catch (final LockedException | NotReadyException ignore) {
437 // never thrown
438 }
439 }
440 }