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.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20
21 import java.io.Serializable;
22
23 /**
24 * This class defines the amount of rotation for 3D points or planes.
25 * Rotation is defined internally as axis coordinates and rotation angle,
26 * following Rodrigues formulas.
27 * This class is based in:
28 * <a href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/sfrotation_java.htm">
29 * http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/sfrotation_java.htm
30 * </a>
31 */
32 public class AxisRotation3D extends Rotation3D implements Serializable {
33
34 /**
35 * Number of parameters defining a rotation axis.
36 */
37 public static final int AXIS_PARAMS = 3;
38
39 /**
40 * Constant defining machine precision.
41 */
42 public static final double EPS = 1e-12;
43
44 /**
45 * x element of axis angle.
46 */
47 private double axisX = 0.0;
48
49 /**
50 * y element of axis angle.
51 */
52 private double axisY = 0.0;
53
54 /**
55 * z element of axis angle.
56 */
57 private double axisZ = 1.0;
58
59 /**
60 * angle element of axis angle.
61 */
62 private double theta = 0.0;
63
64 /**
65 * Constructor which allows initial value to be supplied as axis and angle.
66 * For better accuracy, axis values should be normalized.
67 *
68 * @param axisX x dimension of normalized axis.
69 * @param axisY y dimension of normalized axis.
70 * @param axisZ z dimension of normalized axis.
71 * @param theta angle in radians.
72 */
73 public AxisRotation3D(final double axisX, final double axisY, final double axisZ, final double theta) {
74 setAxisAndRotation(axisX, axisY, axisZ, theta);
75 }
76
77 /**
78 * Constructor where array of axis values and rotation angle are provided
79 * For better accuracy, axis values should be normalized.
80 *
81 * @param axis Array containing x,y and z values of axis.
82 * @param theta rotation angle in radians.
83 * @throws IllegalArgumentException if provided axis length is not 3.
84 */
85 public AxisRotation3D(final double[] axis, final double theta) {
86 setAxisAndRotation(axis, theta);
87 }
88
89 /**
90 * Copy constructor.
91 *
92 * @param rotation instance to copy.
93 */
94 public AxisRotation3D(final AxisRotation3D rotation) {
95 fromRotation(rotation);
96 }
97
98 /**
99 * Copy constructor.
100 *
101 * @param rot Converts and copies provided rotation instance.
102 */
103 public AxisRotation3D(final Rotation3D rot) {
104 try {
105 fromMatrix(rot.asInhomogeneousMatrix());
106 } catch (final InvalidRotationMatrixException ignore) {
107 // never happens
108 }
109 }
110
111 /**
112 * Empty constructor.
113 */
114 public AxisRotation3D() {
115 }
116
117 /**
118 * Returns type of this rotation.
119 *
120 * @return Type of this rotation.
121 */
122 @Override
123 public Rotation3DType getType() {
124 return Rotation3DType.AXIS_ROTATION3D;
125 }
126
127 /**
128 * Sets rotation of this instance by copying provided rotation.
129 *
130 * @param rot Rotation to be copied.
131 */
132 @Override
133 public final void fromRotation(final AxisRotation3D rot) {
134 if (rot != null) {
135 axisX = rot.axisX;
136 axisY = rot.axisY;
137 axisZ = rot.axisZ;
138 theta = rot.theta;
139 } else {
140 axisX = 0.0;
141 axisY = 0.0;
142 axisZ = 1.0;
143 theta = 0.0;
144 }
145 }
146
147 /**
148 * Sets rotation axis of this instance while preserving the rotation angle.
149 * Once set, points will rotate around provided axis.
150 *
151 * @param axisX X coordinate of rotation axis.
152 * @param axisY Y coordinate of rotation axis.
153 * @param axisZ Z coordinate of rotation axis.
154 */
155 public void setAxis(final double axisX, final double axisY, final double axisZ) {
156 theta = Math.sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);
157 if (theta == 0.0) {
158 this.axisX = 1;
159 this.axisY = this.axisZ = 0.0;
160 return;
161 }
162 this.axisX = axisX / theta;
163 this.axisY = axisY / theta;
164 this.axisZ = axisZ / theta;
165 }
166
167 /**
168 * Sets the axis and rotation of this instance.
169 * Once set, points will rotate around provided axis an amount equal to
170 * provided rotation angle in radians.
171 * Note: to avoid numerical instabilities and improve accuracy, axis
172 * coordinates should be normalized (e.g. norm equal to 1).
173 *
174 * @param axisX X coordinate of rotation axis.
175 * @param axisY Y coordinate of rotation axis.
176 * @param axisZ Z coordinate of rotation axis.
177 * @param theta Amount of rotation in radians.
178 */
179 @Override
180 public final void setAxisAndRotation(
181 final double axisX, final double axisY, final double axisZ, final double theta) {
182 this.axisX = axisX;
183 this.axisY = axisY;
184 this.axisZ = axisZ;
185 this.theta = theta;
186 }
187
188 /**
189 * Returns X coordinate of rotation axis.
190 *
191 * @return X coordinate of rotation axis.
192 */
193 public double getAxisX() {
194 return axisX;
195 }
196
197 /**
198 * Returns Y coordinate of rotation axis.
199 *
200 * @return Y coordinate of rotation axis.
201 */
202 public double getAxisY() {
203 return axisY;
204 }
205
206 /**
207 * Returns Z coordinate of rotation axis.
208 *
209 * @return Z coordinate of rotation axis.
210 */
211 public double getAxisZ() {
212 return axisZ;
213 }
214
215 /**
216 * Returns rotation axis corresponding to this instance.
217 * Result is stored in provided axis array, which must have length 3
218 *
219 * @param axis Array where axis coordinates will be stored.
220 * @throws IllegalArgumentException Raised if provided array does not have
221 * length 3.
222 */
223 @Override
224 public void rotationAxis(final double[] axis) {
225 if (axis.length != INHOM_COORDS) {
226 throw new IllegalArgumentException();
227 }
228
229 axis[0] = axisX;
230 axis[1] = axisY;
231 axis[2] = axisZ;
232 }
233
234 /**
235 * Returns rotation amount or angle in radians around the rotation axis
236 * associated to this instance.
237 *
238 * @return Rotation angle in radians.
239 */
240 @Override
241 public double getRotationAngle() {
242 return theta;
243 }
244
245 /**
246 * Returns a 3D rotation which is inverse to this instance.
247 * In other words, the combination of this rotation with its inverse
248 * produces no change.
249 *
250 * @return Inverse 3D rotation.
251 */
252 @Override
253 public AxisRotation3D inverseRotationAndReturnNew() {
254 final var rot = new AxisRotation3D();
255 inverseRotation(rot);
256 return rot;
257 }
258
259 /**
260 * Sets into provided MatrixRotation3D instance a rotation inverse to this
261 * instance.
262 * The combination of this rotation with its inverse produces no change.
263 *
264 * @param result Instance where inverse rotation will be set.
265 */
266 public void inverseRotation(final AxisRotation3D result) {
267 // copy this rotation into result
268 result.fromRotation(this);
269 // reverse angle
270 result.theta = -theta;
271 }
272
273 /**
274 * Sets into provided MatrixRotation3D instance a rotation inverse to this
275 * instance.
276 * The combination of this rotation with its inverse produces no change.
277 *
278 * @param result Instance where inverse rotation will be set.
279 */
280 @Override
281 public void inverseRotation(final Rotation3D result) {
282 if (result instanceof AxisRotation3D axixResult) {
283 inverseRotation(axixResult);
284
285 } else if (result instanceof MatrixRotation3D) {
286 final var rot = new AxisRotation3D();
287 inverseRotation(rot);
288 try {
289 result.fromMatrix(rot.asInhomogeneousMatrix());
290 } catch (final InvalidRotationMatrixException ignore) {
291 // never happens
292 }
293 }
294 }
295
296 /**
297 * Reverses the rotation of this instance.
298 */
299 @Override
300 public void inverseRotation() {
301 inverseRotation(this);
302 }
303
304 /**
305 * Returns this 3D rotation instance expressed as a 3x3 inhomogeneous
306 * matrix.
307 * This is equivalent to call getInternalMatrix().
308 *
309 * @return Rotation matrix expressed in inhomogeneous coordinates.
310 */
311 @Override
312 public Matrix asInhomogeneousMatrix() {
313 Matrix result = null;
314 try {
315 result = new Matrix(INHOM_COORDS, INHOM_COORDS);
316 asInhomogeneousMatrix(result);
317 } catch (final WrongSizeException ignore) {
318 // never happens
319 }
320
321 return result;
322 }
323
324 /**
325 * Sets into provided Matrix instance this 3D rotation expressed as a
326 * 3x3 inhomogeneous matrix.
327 *
328 * @param result Matrix where rotation will be set.
329 * @throws IllegalArgumentException Raised if provided instance does not
330 * have size 3x3.
331 */
332 @Override
333 public void asInhomogeneousMatrix(final Matrix result) {
334 if (result.getRows() != INHOM_COORDS || result.getColumns() != INHOM_COORDS) {
335 throw new IllegalArgumentException();
336 }
337
338 final var c = Math.cos(theta);
339 final var s = Math.sin(theta);
340 final var t = 1.0 - c;
341 // normalize axis
342 final var magnitude = Math.sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);
343 if (magnitude > EPS) {
344 // normalize only if axis norm is large enough to avoid numerical
345 // instability
346 axisX /= magnitude;
347 axisY /= magnitude;
348 axisZ /= magnitude;
349 }
350
351 result.setElementAt(0, 0, c + axisX * axisX * t);
352 result.setElementAt(1, 1, c + axisY * axisY * t);
353 result.setElementAt(2, 2, c + axisZ * axisZ * t);
354
355
356 var tmp1 = axisX * axisY * t;
357 var tmp2 = axisZ * s;
358 result.setElementAt(1, 0, tmp1 + tmp2);
359 result.setElementAt(0, 1, tmp1 - tmp2);
360 tmp1 = axisX * axisZ * t;
361 tmp2 = axisY * s;
362 result.setElementAt(2, 0, tmp1 - tmp2);
363 result.setElementAt(0, 2, tmp1 + tmp2);
364 tmp1 = axisY * axisZ * t;
365 tmp2 = axisX * s;
366 result.setElementAt(2, 1, tmp1 + tmp2);
367 result.setElementAt(1, 2, tmp1 - tmp2);
368 }
369
370 /**
371 * Returns this 3D rotation instance expressed as a 4x4 homogeneous matrix.
372 *
373 * @return Rotation matrix expressed in homogeneous coordinates.
374 */
375 @Override
376 public Matrix asHomogeneousMatrix() {
377 Matrix result = null;
378 try {
379 result = Matrix.identity(HOM_COORDS, HOM_COORDS);
380 // sets into 3x3 top-left sub-matrix the internal matrix of this
381 // instance, the remaining part will continue to be the identity
382 result.setSubmatrix(0, 0, INHOM_COORDS - 1,
383 INHOM_COORDS - 1, asInhomogeneousMatrix());
384 } catch (final WrongSizeException ignore) {
385 // never happens
386 }
387 return result;
388 }
389
390 /**
391 * Sets into provided Matrix instance this 3D rotation expressed as a
392 * 4x4 homogeneous matrix.
393 *
394 * @param result Matrix where rotation will be set.
395 * @throws IllegalArgumentException Raised if provided instance does not
396 * have size 4x4.
397 */
398 @Override
399 public void asHomogeneousMatrix(final Matrix result) {
400 if (result.getRows() != HOM_COORDS || result.getColumns() != HOM_COORDS) {
401 throw new IllegalArgumentException();
402 }
403
404 result.initialize(0.0);
405 // sets into 3x3 top-left sub-matrix the internal matrix of this instance
406 result.setSubmatrix(0, 0, INHOM_COORDS - 1,
407 INHOM_COORDS - 1, asInhomogeneousMatrix());
408 // set las element to 1.0 (to be like the identity
409 result.setElementAt(HOM_COORDS - 1, HOM_COORDS - 1, 1.0);
410 }
411
412 /**
413 * Sets amount of rotation from provided inhomogeneous rotation matrix.
414 * Provided matrix must be orthogonal (i.e. squared, non-singular, it's
415 * transpose must be its inverse) and must have determinant equal to 1.
416 * Provided matrix must also have size 3x3.
417 *
418 * @param m Provided rotation matrix.
419 * @param threshold Threshold to determine whether matrix is orthonormal.
420 * @throws InvalidRotationMatrixException Raised if provided matrix is not
421 * valid (has wrong size, or it is not orthonormal).
422 * @throws IllegalArgumentException Raised if provided threshold is negative
423 * {@link #isValidRotationMatrix(Matrix)}
424 */
425 @Override
426 public void fromInhomogeneousMatrix(final Matrix m, final double threshold) throws InvalidRotationMatrixException {
427
428 if (m.getRows() != INHOM_COORDS || m.getColumns() != INHOM_COORDS) {
429 throw new InvalidRotationMatrixException();
430 }
431 if (!Rotation3D.isValidRotationMatrix(m, threshold)) {
432 throw new InvalidRotationMatrixException();
433 }
434
435 double angle;
436 double x;
437 double y;
438 // variables for result
439 double z;
440 // margin to allow for rounding errors
441 var epsilon = 0.01;
442 // margin to distinguish between 0 and 180 degrees
443 var epsilon2 = 0.1;
444
445 if ((Math.abs(m.getElementAt(0, 1) - m.getElementAt(1, 0)) < epsilon) &&
446 (Math.abs(m.getElementAt(0, 2) - m.getElementAt(2, 0)) < epsilon) &&
447 (Math.abs(m.getElementAt(1, 2) - m.getElementAt(2, 1)) < epsilon)) {
448 // singularity found
449 // first check for identity matrix which must have +1 for all terms
450 // in leading diagonal and zero in other terms
451 if ((Math.abs(m.getElementAt(0, 1) + m.getElementAt(1, 0)) < epsilon2)
452 && (Math.abs(m.getElementAt(0, 2) + m.getElementAt(2, 0)) < epsilon2)
453 && (Math.abs(m.getElementAt(1, 2) + m.getElementAt(2, 1)) < epsilon2)
454 && (Math.abs(m.getElementAt(0, 0) + m.getElementAt(1, 1)
455 + m.getElementAt(2, 2) - 3.0) < epsilon2)) {
456 // this singularity is identity matrix so angle = 0
457 setAxisAndRotation(0.0, 0.0, 1.0, 0.0); // zero angle,
458 // arbitrary axis
459 return;
460 }
461 // otherwise this singularity is angle = 180
462 angle = Math.PI;
463 final var xx = (m.getElementAt(0, 0) + 1.0) / 2.0;
464 final var yy = (m.getElementAt(1, 1) + 1.0) / 2.0;
465 final var zz = (m.getElementAt(2, 2) + 1.0) / 2.0;
466 final var xy = (m.getElementAt(0, 1) + m.getElementAt(1, 0)) / 4.0;
467 final var xz = (m.getElementAt(0, 2) + m.getElementAt(2, 0)) / 4.0;
468 final var yz = (m.getElementAt(1, 2) + m.getElementAt(2, 1)) / 4.0;
469 if ((xx > yy) && (xx > zz)) {
470 // m.getElementAt(0, 0) is the
471 // largest diagonal term
472 if (xx < epsilon) {
473 x = 0.0;
474 y = Math.sqrt(2.0) / 2.0;
475 z = Math.sqrt(2.0) / 2.0;
476 } else {
477 x = Math.sqrt(xx);
478 y = xy / x;
479 z = xz / x;
480 }
481 } else if (yy > zz) {
482 // m.getElementAt(1, 1) is the largest
483 // diagonal term
484 if (yy < epsilon) {
485 x = Math.sqrt(2.0) / 2.0;
486 y = 0.0;
487 z = Math.sqrt(2.0) / 2.0;
488 } else {
489 y = Math.sqrt(yy);
490 x = xy / y;
491 z = yz / y;
492 }
493 } else {
494 // m.getElementAt(2, 2) is the largest diagonal term so
495 // base result on this
496 if (zz < epsilon) {
497 x = Math.sqrt(2.0) / 2.0;
498 y = Math.sqrt(2.0) / 2.0;
499 z = 0.0;
500 } else {
501 z = Math.sqrt(zz);
502 x = xz / z;
503 y = yz / z;
504 }
505 }
506 setAxisAndRotation(x, y, z, angle);
507 // return 180 deg rotation
508 return;
509 }
510
511 // as we have reached here there are no singularities, so we can handle
512 // normally
513 var s = Math.sqrt((m.getElementAt(2, 1) - m.getElementAt(1, 2))
514 * (m.getElementAt(2, 1) - m.getElementAt(1, 2))
515 + (m.getElementAt(0, 2) - m.getElementAt(2, 0))
516 * (m.getElementAt(0, 2) - m.getElementAt(2, 0))
517 + (m.getElementAt(1, 0) - m.getElementAt(0, 1))
518 * (m.getElementAt(1, 0) - m.getElementAt(0, 1))); // used to
519 // normalise
520 if (Math.abs(s) < 0.001) {
521 s = 1.0;
522 }
523 // prevent divide by zero, should not happen if matrix is orthogonal and
524 // should be caught by singularity test above, but I've left it in just
525 // in case
526 theta = Math.acos((m.getElementAt(0, 0) + m.getElementAt(1, 1)
527 + m.getElementAt(2, 2) - 1.0) / 2.0);
528 axisX = (m.getElementAt(2, 1) - m.getElementAt(1, 2)) / s;
529 axisY = (m.getElementAt(0, 2) - m.getElementAt(2, 0)) / s;
530 axisZ = (m.getElementAt(1, 0) - m.getElementAt(0, 1)) / s;
531 }
532
533 /**
534 * Sets amount of rotation from provided homogeneous rotation matrix.
535 * Provided matrix must be orthogonal (i.e. squared, non-singular, it's
536 * transpose must be its inverse) and must have determinant equal to 1.
537 * Provided matrix must also have size 4x4, and its last row and column must
538 * be zero, except for element in last row and column which must be 1.
539 *
540 * @param m Provided rotation matrix.
541 * @param threshold Threshold to determine whether matrix is orthonormal.
542 * @throws InvalidRotationMatrixException Raised if provided matrix is not
543 * valid (has wrong size, or it is not orthonormal).
544 * @throws IllegalArgumentException Raised if provided threshold is
545 * negative.
546 * {@link #isValidRotationMatrix(Matrix)}.
547 */
548 @SuppressWarnings("DuplicatedCode")
549 @Override
550 public void fromHomogeneousMatrix(final Matrix m, final double threshold) throws InvalidRotationMatrixException {
551 if (m.getRows() != HOM_COORDS || m.getColumns() != HOM_COORDS) {
552 throw new InvalidRotationMatrixException();
553 }
554 if (!Rotation3D.isValidRotationMatrix(m, threshold)) {
555 throw new InvalidRotationMatrixException();
556 }
557 if (Math.abs(m.getElementAt(3, 0)) > threshold
558 || Math.abs(m.getElementAt(3, 1)) > threshold
559 || Math.abs(m.getElementAt(3, 2)) > threshold
560 || Math.abs(m.getElementAt(0, 3)) > threshold
561 || Math.abs(m.getElementAt(1, 3)) > threshold
562 || Math.abs(m.getElementAt(2, 3)) > threshold
563 || Math.abs(m.getElementAt(3, 3) - 1.0) > threshold) {
564 throw new InvalidRotationMatrixException();
565 }
566
567 fromInhomogeneousMatrix(m.getSubmatrix(0, 0, INHOM_COORDS - 1,
568 INHOM_COORDS - 1), threshold);
569 }
570
571 /**
572 * Rotates a 3D point using the origin of coordinates as the axis of
573 * rotation.
574 * Point will be rotated by the amount of rotation contained in this
575 * instance.
576 *
577 * @param inputPoint Input point to be rotated.
578 * @param resultPoint Rotated point.
579 */
580 @Override
581 public void rotate(final Point3D inputPoint, final Point3D resultPoint) {
582 final var s = Math.sin(theta / 2.0);
583 final var xh = axisX * s;
584 final var yh = axisY * s;
585 final var zh = axisZ * s;
586 final var wh = Math.cos(theta / 2.0);
587
588 final var inhomX = inputPoint.getInhomX();
589 final var inhomY = inputPoint.getInhomY();
590 final var inhomZ = inputPoint.getInhomZ();
591
592 final var resultX = wh * wh * inhomX + 2.0 * yh * wh * inhomZ - 2.0 * zh * wh * inhomY + xh * xh * inhomX
593 + 2.0 * yh * xh * inhomY + 2 * zh * xh * inhomZ - zh * zh * inhomX - yh * yh * inhomX;
594 final var resultY = 2.0 * xh * yh * inhomX + yh * yh * inhomY + 2.0 * zh * yh * inhomZ
595 + 2.0 * wh * zh * inhomX - zh * zh * inhomY + wh * wh * inhomY - 2 * xh * wh * inhomZ
596 - xh * xh * inhomY;
597 final var resultZ = 2.0 * xh * zh * inhomX + 2.0 * yh * zh * inhomY + zh * zh * inhomZ
598 - 2.0 * wh * yh * inhomX - yh * yh * inhomZ + 2.0 * wh * xh * inhomY - xh * xh * inhomZ
599 + wh * wh * inhomZ;
600 resultPoint.setInhomogeneousCoordinates(resultX, resultY, resultZ);
601 }
602
603 /**
604 * Combines provided rotation with this rotation and returns the result as
605 * a new MatrixRotation3D instance.
606 *
607 * @param rotation Input rotation to be combined.
608 * @return Combined rotation, which is equal to the multiplication of the
609 * internal matrix of provided rotation with the internal matrix of this
610 * instance.
611 */
612 public AxisRotation3D combineAndReturnNew(final AxisRotation3D rotation) {
613 final var result = new AxisRotation3D();
614 combine(this, rotation, result);
615 return result;
616 }
617
618 /**
619 * Combines provided rotation with this rotation and returns the result as
620 * a new MatrixRotation3D instance.
621 *
622 * @param rotation Input rotation to be combined.
623 * @return Combined rotation, which is equal to the multiplication of the
624 * internal matrix of provided rotation with the internal matrix of this
625 * instance.
626 */
627 @Override
628 public Rotation3D combineAndReturnNew(final Rotation3D rotation) {
629 if (rotation instanceof AxisRotation3D axisRotation) {
630 return combineAndReturnNew(axisRotation);
631 } else {
632 return combineAndReturnNew(new AxisRotation3D(rotation));
633 }
634 }
635
636 /**
637 * Combines provided rotation into this rotation resulting in the
638 * multiplication of the internal matrices of both rotations.
639 *
640 * @param rotation Input rotation to be combined.
641 */
642 public void combine(final AxisRotation3D rotation) {
643 combine(this, rotation, this);
644 }
645
646 /**
647 * Combines provided rotation into this rotation resulting in the
648 * multiplication of the internal matrices of both rotations.
649 *
650 * @param rotation Input rotation to be combined.
651 */
652 @Override
653 public void combine(final Rotation3D rotation) {
654 if (rotation instanceof AxisRotation3D axisRotation) {
655 combine(axisRotation);
656 } else {
657 combine(new AxisRotation3D(rotation));
658 }
659 }
660
661 /**
662 * Combines the rotation of instances rot1 and rot1 into provided result
663 * instance.
664 *
665 * @param rot1 1st input rotation.
666 * @param rot2 2nd input rotation.
667 * @param result Combined rotation, which is equal to the multiplication of
668 * the internal matrix of provided rotation with the internal matrix of this
669 * instance.
670 */
671 public static void combine(final AxisRotation3D rot1, final AxisRotation3D rot2, final AxisRotation3D result) {
672
673 final var m1 = rot1.asInhomogeneousMatrix();
674 final var m2 = rot2.asInhomogeneousMatrix();
675 try {
676 m1.multiply(m2);
677 result.fromMatrix(m1);
678 } catch (final InvalidRotationMatrixException | WrongSizeException ignore) {
679 // never happens
680 }
681 }
682
683 /**
684 * Sets values of this rotation from a quaternion.
685 *
686 * @param q a quaternion to set values from.
687 */
688 @Override
689 public void fromRotation(final Quaternion q) {
690 q.toAxisRotation(this);
691 }
692
693 /**
694 * Converts this 3D rotation into a matrix rotation storing the result
695 * into provided instance.
696 *
697 * @param result instance where result wil be stored.
698 */
699 @Override
700 public void toMatrixRotation(final MatrixRotation3D result) {
701 result.setAxisAndRotation(new double[]{axisX, axisY, axisZ}, theta);
702 }
703
704 /**
705 * Converts this 3D rotation into an axis rotation storing the result into
706 * provided instance.
707 *
708 * @param result instance where result will be stored.
709 */
710 @Override
711 public void toAxisRotation(final AxisRotation3D result) {
712 result.fromRotation(this);
713 }
714
715 /**
716 * Converts this 3D rotation into a quaternion storing the result into
717 * provided instance.
718 *
719 * @param result instance where result will be stored.
720 */
721 @Override
722 public void toQuaternion(final Quaternion result) {
723 result.setFromAxisAndRotation(axisX, axisY, axisZ, theta);
724 }
725 }