1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * The ASF licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.apache.commons.geometry.euclidean.threed.rotation;
18
19 import java.util.Objects;
20 import java.util.function.DoubleFunction;
21
22 import org.apache.commons.geometry.core.internal.GeometryInternalError;
23 import org.apache.commons.geometry.euclidean.internal.Vectors;
24 import org.apache.commons.geometry.euclidean.threed.AffineTransformMatrix3D;
25 import org.apache.commons.geometry.euclidean.threed.Vector3D;
26 import org.apache.commons.numbers.angle.Angle;
27 import org.apache.commons.numbers.quaternion.Quaternion;
28 import org.apache.commons.numbers.quaternion.Slerp;
29
30 /**
31 * Class using a unit-length quaternion to represent
32 * <a href="https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation">rotations</a>
33 * in 3-dimensional Euclidean space.
34 * The underlying quaternion is in <em>positive polar form</em>: It is normalized and has a
35 * non-negative scalar component ({@code w}).
36 *
37 * @see Quaternion
38 */
39 public final class QuaternionRotation implements Rotation3D {
40 /** Threshold value for the dot product of antiparallel vectors. If the dot product of two vectors is
41 * less than this value, (adjusted for the lengths of the vectors), then the vectors are considered to be
42 * antiparallel (ie, negations of each other).
43 */
44 private static final double ANTIPARALLEL_DOT_THRESHOLD = 2.0e-15 - 1.0;
45
46 /** Threshold value used to identify singularities when converting from quaternions to
47 * axis angle sequences.
48 */
49 private static final double AXIS_ANGLE_SINGULARITY_THRESHOLD = 0.9999999999;
50
51 /** Instance used to represent the identity rotation, ie a rotation with
52 * an angle of zero.
53 */
54 private static final QuaternionRotation IDENTITY_INSTANCE = of(Quaternion.ONE);
55
56 /** Unit-length quaternion instance in positive polar form. */
57 private final Quaternion quat;
58
59 /** Simple constructor. The given quaternion is converted to positive polar form.
60 * @param quat quaternion instance
61 * @throws IllegalStateException if the the norm of the given components is zero,
62 * NaN, or infinite
63 */
64 private QuaternionRotation(final Quaternion quat) {
65 this.quat = quat.positivePolarForm();
66 }
67
68 /** Get the underlying quaternion instance.
69 * @return the quaternion instance
70 */
71 public Quaternion getQuaternion() {
72 return quat;
73 }
74
75 /**
76 * Get the axis of rotation as a normalized {@link Vector3D}. The rotation axis
77 * is not well defined when the rotation is the identity rotation, ie it has a
78 * rotation angle of zero. In this case, the vector representing the positive
79 * x-axis is returned.
80 *
81 * @return the axis of rotation
82 */
83 @Override
84 public Vector3D getAxis() {
85 final Vector3D axis = Vector3D.of(quat.getX(), quat.getY(), quat.getZ())
86 .normalizeOrNull();
87 return axis != null ?
88 axis :
89 Vector3D.Unit.PLUS_X;
90 }
91
92 /**
93 * Get the angle of rotation in radians. The returned value is in the range 0
94 * through {@code pi}.
95 *
96 * @return The rotation angle in the range {@code [0, pi]}.
97 */
98 @Override
99 public double getAngle() {
100 return 2 * Math.acos(quat.getW());
101 }
102
103 /**
104 * Get the inverse of this rotation. The returned rotation has the same
105 * rotation angle but the opposite rotation axis. If {@code r.apply(u)}
106 * is equal to {@code v}, then {@code r.negate().apply(v)} is equal
107 * to {@code u}.
108 *
109 * @return the negation (inverse) of the rotation
110 */
111 @Override
112 public QuaternionRotation inverse() {
113 return new QuaternionRotation(quat.conjugate());
114 }
115
116 /**
117 * Apply this rotation to the given vector.
118 *
119 * @param v vector to rotate
120 * @return the rotated vector
121 */
122 @Override
123 public Vector3D apply(final Vector3D v) {
124 final double qw = quat.getW();
125 final double qx = quat.getX();
126 final double qy = quat.getY();
127 final double qz = quat.getZ();
128
129 final double x = v.getX();
130 final double y = v.getY();
131 final double z = v.getZ();
132
133 // calculate the Hamilton product of the quaternion and vector
134 final double iw = -(qx * x) - (qy * y) - (qz * z);
135 final double ix = (qw * x) + (qy * z) - (qz * y);
136 final double iy = (qw * y) + (qz * x) - (qx * z);
137 final double iz = (qw * z) + (qx * y) - (qy * x);
138
139 // calculate the Hamilton product of the intermediate vector and
140 // the inverse quaternion
141
142 return Vector3D.of(
143 (iw * -qx) + (ix * qw) + (iy * -qz) - (iz * -qy),
144 (iw * -qy) - (ix * -qz) + (iy * qw) + (iz * -qx),
145 (iw * -qz) + (ix * -qy) - (iy * -qx) + (iz * qw)
146 );
147 }
148
149 /** {@inheritDoc}
150 *
151 * <p>This method simply calls {@code apply(vec)} since rotations treat
152 * points and vectors similarly.</p>
153 */
154 @Override
155 public Vector3D applyVector(final Vector3D vec) {
156 return apply(vec);
157 }
158
159 /** {@inheritDoc}
160 *
161 * <p>This method simply returns true since rotations always preserve the orientation
162 * of the space.</p>
163 */
164 @Override
165 public boolean preservesOrientation() {
166 return true;
167 }
168
169 /** Return an {@link AffineTransformMatrix3D} representing the same rotation as this
170 * instance.
171 * @return a transform matrix representing the same rotation as this instance
172 */
173 public AffineTransformMatrix3D toMatrix() {
174
175 final double qw = quat.getW();
176 final double qx = quat.getX();
177 final double qy = quat.getY();
178 final double qz = quat.getZ();
179
180 // pre-calculate products that we'll need
181 final double xx = qx * qx;
182 final double xy = qx * qy;
183 final double xz = qx * qz;
184 final double xw = qx * qw;
185
186 final double yy = qy * qy;
187 final double yz = qy * qz;
188 final double yw = qy * qw;
189
190 final double zz = qz * qz;
191 final double zw = qz * qw;
192
193 final double m00 = 1.0 - (2.0 * (yy + zz));
194 final double m01 = 2.0 * (xy - zw);
195 final double m02 = 2.0 * (xz + yw);
196 final double m03 = 0.0;
197
198 final double m10 = 2.0 * (xy + zw);
199 final double m11 = 1.0 - (2.0 * (xx + zz));
200 final double m12 = 2.0 * (yz - xw);
201 final double m13 = 0.0;
202
203 final double m20 = 2.0 * (xz - yw);
204 final double m21 = 2.0 * (yz + xw);
205 final double m22 = 1.0 - (2.0 * (xx + yy));
206 final double m23 = 0.0;
207
208 return AffineTransformMatrix3D.of(
209 m00, m01, m02, m03,
210 m10, m11, m12, m13,
211 m20, m21, m22, m23
212 );
213 }
214
215 /**
216 * Multiply this instance by the given argument, returning the result as
217 * a new instance. This is equivalent to the expression {@code t * q} where
218 * {@code q} is the argument and {@code t} is this instance.
219 *
220 * <p>
221 * Multiplication of quaternions behaves similarly to transformation
222 * matrices in regard to the order that operations are performed.
223 * For example, if <code>q<sub>1</sub></code> and <code>q<sub>2</sub></code> are unit
224 * quaternions, then the quaternion <code>q<sub>r</sub> = q<sub>1</sub>*q<sub>2</sub></code>
225 * will give the effect of applying the rotation in <code>q<sub>2</sub></code> followed
226 * by the rotation in <code>q<sub>1</sub></code>. In other words, the rightmost element
227 * in the multiplication is applied first.
228 * </p>
229 *
230 * @param q quaternion to multiply with the current instance
231 * @return the result of multiplying this quaternion by the argument
232 */
233 public QuaternionRotation multiply(final QuaternionRotation q) {
234 final Quaternion product = quat.multiply(q.quat);
235 return new QuaternionRotation(product);
236 }
237
238 /** Multiply the argument by this instance, returning the result as
239 * a new instance. This is equivalent to the expression {@code q * t} where
240 * {@code q} is the argument and {@code t} is this instance.
241 *
242 * <p>
243 * Multiplication of quaternions behaves similarly to transformation
244 * matrices in regard to the order that operations are performed.
245 * For example, if <code>q<sub>1</sub></code> and <code>q<sub>2</sub></code> are unit
246 * quaternions, then the quaternion <code>q<sub>r</sub> = q<sub>1</sub>*q<sub>2</sub></code>
247 * will give the effect of applying the rotation in <code>q<sub>2</sub></code> followed
248 * by the rotation in <code>q<sub>1</sub></code>. In other words, the rightmost element
249 * in the multiplication is applied first.
250 * </p>
251 *
252 * @param q quaternion to multiply by the current instance
253 * @return the result of multiplying the argument by the current instance
254 */
255 public QuaternionRotation premultiply(final QuaternionRotation q) {
256 return q.multiply(this);
257 }
258
259 /**
260 * Creates a function that performs a
261 * <a href="https://en.wikipedia.org/wiki/Slerp">spherical
262 * linear interpolation</a> between this instance and the argument.
263 * <p>
264 * The argument to the function returned by this method is the
265 * interpolation parameter {@code t}.
266 * If {@code t = 0}, the rotation is equal to this instance.
267 * If {@code t = 1}, the rotation is equal to the {@code end} instance.
268 * All other values are interpolated (or extrapolated if {@code t} is
269 * outside of the {@code [0, 1]} range).
270 *
271 * @param end end value of the interpolation
272 * @return a function that interpolates between this instance and the
273 * argument.
274 *
275 * @see org.apache.commons.numbers.quaternion.Slerp
276 */
277 public DoubleFunction<QuaternionRotation> slerp(final QuaternionRotation end) {
278 final Slerp s = new Slerp(getQuaternion(), end.getQuaternion());
279 return t -> QuaternionRotation.of(s.apply(t));
280 }
281
282 /** Get a sequence of axis-angle rotations that produce an overall rotation equivalent to this instance.
283 *
284 * <p>
285 * In most cases, the returned rotation sequence will be unique. However, at points of singularity
286 * (second angle equal to {@code 0} or {@code -pi} for Euler angles and {@code +pi/2} or {@code -pi/2}
287 * for Tait-Bryan angles), there are an infinite number of possible sequences that produce the same result.
288 * In these cases, the result is returned that leaves the last rotation equal to 0 (in the case of a relative
289 * reference frame) or the first rotation equal to 0 (in the case of an absolute reference frame).
290 * </p>
291 *
292 * @param frame the reference frame used to interpret the positions of the rotation axes
293 * @param axes the sequence of rotation axes
294 * @return a sequence of axis-angle rotations equivalent to this rotation
295 */
296 public AxisAngleSequence toAxisAngleSequence(final AxisReferenceFrame frame, final AxisSequence axes) {
297 if (frame == null) {
298 throw new IllegalArgumentException("Axis reference frame cannot be null");
299 }
300 if (axes == null) {
301 throw new IllegalArgumentException("Axis sequence cannot be null");
302 }
303
304 final double[] angles = getAngles(frame, axes);
305
306 return new AxisAngleSequence(frame, axes, angles[0], angles[1], angles[2]);
307 }
308
309 /** Get a sequence of axis-angle rotations that produce an overall rotation equivalent to this instance.
310 * Each rotation axis is interpreted relative to the rotated coordinate frame (ie, intrinsic rotation).
311 * @param axes the sequence of rotation axes
312 * @return a sequence of relative axis-angle rotations equivalent to this rotation
313 * @see #toAxisAngleSequence(AxisReferenceFrame, AxisSequence)
314 */
315 public AxisAngleSequence toRelativeAxisAngleSequence(final AxisSequence axes) {
316 return toAxisAngleSequence(AxisReferenceFrame.RELATIVE, axes);
317 }
318
319 /** Get a sequence of axis-angle rotations that produce an overall rotation equivalent to this instance.
320 * Each rotation axis is interpreted as part of an absolute, unmoving coordinate frame (ie, extrinsic rotation).
321 * @param axes the sequence of rotation axes
322 * @return a sequence of absolute axis-angle rotations equivalent to this rotation
323 * @see #toAxisAngleSequence(AxisReferenceFrame, AxisSequence)
324 */
325 public AxisAngleSequence toAbsoluteAxisAngleSequence(final AxisSequence axes) {
326 return toAxisAngleSequence(AxisReferenceFrame.ABSOLUTE, axes);
327 }
328
329 /** {@inheritDoc} */
330 @Override
331 public int hashCode() {
332 return quat.hashCode();
333 }
334
335 /** {@inheritDoc} */
336 @Override
337 public boolean equals(final Object obj) {
338 if (this == obj) {
339 return true;
340 }
341 if (!(obj instanceof QuaternionRotation)) {
342 return false;
343 }
344
345 final QuaternionRotation other = (QuaternionRotation) obj;
346 return Objects.equals(this.quat, other.quat);
347 }
348
349 /** {@inheritDoc} */
350 @Override
351 public String toString() {
352 return quat.toString();
353 }
354
355 /** Get a sequence of angles around the given axes that produce a rotation equivalent
356 * to this instance.
357 * @param frame the reference frame used to define the positions of the axes
358 * @param axes the axis sequence
359 * @return a sequence of angles around the given axes that produce a rotation equivalent
360 * to this instance
361 */
362 private double[] getAngles(final AxisReferenceFrame frame, final AxisSequence axes) {
363
364 final AxisSequenceType sequenceType = axes.getType();
365
366 final Vector3D axis1 = axes.getAxis1();
367 final Vector3D axis2 = axes.getAxis2();
368 final Vector3D axis3 = axes.getAxis3();
369
370 if (frame == AxisReferenceFrame.RELATIVE) {
371 if (sequenceType == AxisSequenceType.TAIT_BRYAN) {
372 return getRelativeTaitBryanAngles(axis1, axis2, axis3);
373 } else if (sequenceType == AxisSequenceType.EULER) {
374 return getRelativeEulerAngles(axis1, axis2);
375 }
376 } else if (frame == AxisReferenceFrame.ABSOLUTE) {
377 if (sequenceType == AxisSequenceType.TAIT_BRYAN) {
378 return getAbsoluteTaitBryanAngles(axis1, axis2, axis3);
379 } else if (sequenceType == AxisSequenceType.EULER) {
380 return getAbsoluteEulerAngles(axis1, axis2);
381 }
382 }
383
384 // all possibilities should have been covered above
385 throw new GeometryInternalError();
386 }
387
388 /** Get a sequence of angles around the given Tait-Bryan axes that produce a rotation equivalent
389 * to this instance. The axes are interpreted as being relative to the rotated coordinate frame.
390 * @param axis1 first Tait-Bryan axis
391 * @param axis2 second Tait-Bryan axis
392 * @param axis3 third Tait-Bryan axis
393 * @return a sequence of rotation angles around the relative input axes that produce a rotation equivalent
394 * to this instance
395 */
396 private double[] getRelativeTaitBryanAngles(final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) {
397
398 // We can use geometry to get the first and second angles pretty easily here by analyzing the positions
399 // of the transformed rotation axes. The third angle is trickier but we can get it by treating it as
400 // if it were the first rotation in the inverse (which it would be).
401
402 final Vector3D vec3 = apply(axis3);
403 final Vector3D invVec1 = inverse().apply(axis1);
404
405 final double angle2Sin = vec3.dot(axis2.cross(axis3));
406
407 if (angle2Sin < -AXIS_ANGLE_SINGULARITY_THRESHOLD ||
408 angle2Sin > AXIS_ANGLE_SINGULARITY_THRESHOLD) {
409
410 final Vector3D vec2 = apply(axis2);
411
412 final double angle1TanY = vec2.dot(axis1.cross(axis2));
413 final double angle1TanX = vec2.dot(axis2);
414
415 final double angle2 = angle2Sin > AXIS_ANGLE_SINGULARITY_THRESHOLD ?
416 Angle.PI_OVER_TWO :
417 -Angle.PI_OVER_TWO;
418
419 return new double[] {
420 Math.atan2(angle1TanY, angle1TanX),
421 angle2,
422 0.0
423 };
424 }
425
426 final Vector3D crossAxis13 = axis1.cross(axis3);
427
428 final double angle1TanY = vec3.dot(crossAxis13);
429 final double angle1TanX = vec3.dot(axis3);
430
431 final double angle3TanY = invVec1.dot(crossAxis13);
432 final double angle3TanX = invVec1.dot(axis1);
433
434 return new double[] {
435 Math.atan2(angle1TanY, angle1TanX),
436 Math.asin(angle2Sin),
437 Math.atan2(angle3TanY, angle3TanX)
438 };
439 }
440
441 /** Get a sequence of angles around the given Tait-Bryan axes that produce a rotation equivalent
442 * to this instance. The axes are interpreted as being part of an absolute (unmoving) coordinate frame.
443 * @param axis1 first Tait-Bryan axis
444 * @param axis2 second Tait-Bryan axis
445 * @param axis3 third Tait-Bryan axis
446 * @return a sequence of rotation angles around the absolute input axes that produce a rotation equivalent
447 * to this instance
448 */
449 private double[] getAbsoluteTaitBryanAngles(final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) {
450 // A relative axis-angle rotation sequence is equivalent to an absolute one with the rotation
451 // sequence reversed, meaning we can reuse our relative logic here.
452 return reverseArray(getRelativeTaitBryanAngles(axis3, axis2, axis1));
453 }
454
455 /** Get a sequence of angles around the given Euler axes that produce a rotation equivalent
456 * to this instance. The axes are interpreted as being relative to the rotated coordinate frame. Only
457 * the first two axes are needed since, by definition, the first Euler angle axis is repeated as the
458 * third axis.
459 * @param axis1 first Euler axis
460 * @param axis2 second Euler axis
461 * @return a sequence of rotation angles around the relative input axes that produce a rotation equivalent
462 * to this instance
463 */
464 private double[] getRelativeEulerAngles(final Vector3D axis1, final Vector3D axis2) {
465
466 // Use the same overall approach as with the Tait-Bryan angles: get the first two angles by looking
467 // at the transformed rotation axes and the third by using the inverse.
468
469 final Vector3D crossAxis = axis1.cross(axis2);
470
471 final Vector3D vec1 = apply(axis1);
472 final Vector3D invVec1 = inverse().apply(axis1);
473
474 final double angle2Cos = vec1.dot(axis1);
475
476 if (angle2Cos < -AXIS_ANGLE_SINGULARITY_THRESHOLD ||
477 angle2Cos > AXIS_ANGLE_SINGULARITY_THRESHOLD) {
478
479 final Vector3D vec2 = apply(axis2);
480
481 final double angle1TanY = vec2.dot(crossAxis);
482 final double angle1TanX = vec2.dot(axis2);
483
484 final double angle2 = angle2Cos > AXIS_ANGLE_SINGULARITY_THRESHOLD ? 0.0 : Math.PI;
485
486 return new double[] {
487 Math.atan2(angle1TanY, angle1TanX),
488 angle2,
489 0.0
490 };
491 }
492
493 final double angle1TanY = vec1.dot(axis2);
494 final double angle1TanX = -vec1.dot(crossAxis);
495
496 final double angle3TanY = invVec1.dot(axis2);
497 final double angle3TanX = invVec1.dot(crossAxis);
498
499 return new double[] {
500 Math.atan2(angle1TanY, angle1TanX),
501 Math.acos(angle2Cos),
502 Math.atan2(angle3TanY, angle3TanX)
503 };
504 }
505
506 /** Get a sequence of angles around the given Euler axes that produce a rotation equivalent
507 * to this instance. The axes are interpreted as being part of an absolute (unmoving) coordinate frame.
508 * Only the first two axes are needed since, by definition, the first Euler angle axis is repeated as
509 * the third axis.
510 * @param axis1 first Euler axis
511 * @param axis2 second Euler axis
512 * @return a sequence of rotation angles around the absolute input axes that produce a rotation equivalent
513 * to this instance
514 */
515 private double[] getAbsoluteEulerAngles(final Vector3D axis1, final Vector3D axis2) {
516 // A relative axis-angle rotation sequence is equivalent to an absolute one with the rotation
517 // sequence reversed, meaning we can reuse our relative logic here.
518 return reverseArray(getRelativeEulerAngles(axis1, axis2));
519 }
520
521 /** Create a new instance from the given quaternion. The quaternion is normalized and
522 * converted to positive polar form (ie, with w >= 0).
523 *
524 * @param quat the quaternion to use for the rotation
525 * @return a new instance built from the given quaternion.
526 * @throws IllegalStateException if the the norm of the given components is zero,
527 * NaN, or infinite
528 * @see Quaternion#normalize()
529 * @see Quaternion#positivePolarForm()
530 */
531 public static QuaternionRotation of(final Quaternion quat) {
532 return new QuaternionRotation(quat);
533 }
534
535 /**
536 * Create a new instance from the given quaternion values. The inputs are
537 * normalized and converted to positive polar form (ie, with w >= 0).
538 *
539 * @param w quaternion scalar component
540 * @param x first quaternion vectorial component
541 * @param y second quaternion vectorial component
542 * @param z third quaternion vectorial component
543 * @return a new instance containing the normalized quaterion components
544 * @throws IllegalStateException if the the norm of the given components is zero,
545 * NaN, or infinite
546 * @see Quaternion#normalize()
547 * @see Quaternion#positivePolarForm()
548 */
549 public static QuaternionRotation of(final double w,
550 final double x,
551 final double y,
552 final double z) {
553 return of(Quaternion.of(w, x, y, z));
554 }
555
556 /** Return an instance representing a rotation of zero.
557 * @return instance representing a rotation of zero.
558 */
559 public static QuaternionRotation identity() {
560 return IDENTITY_INSTANCE;
561 }
562
563 /** Create a new instance representing a rotation of {@code angle} radians around
564 * {@code axis}.
565 *
566 * <p>
567 * Rotation direction follows the right-hand rule, meaning that if one
568 * places their right hand such that the thumb points in the direction of the vector,
569 * the curl of the fingers indicates the direction of rotation.
570 * </p>
571 *
572 * <p>
573 * Note that the returned quaternion will represent the defined rotation but the values
574 * returned by {@link #getAxis()} and {@link #getAngle()} may not match the ones given here.
575 * This is because the axis and angle are normalized such that the axis has unit length,
576 * and the angle lies in the range {@code [0, pi]}. Depending on the inputs, the axis may
577 * need to be inverted in order for the angle to lie in this range.
578 * </p>
579 *
580 * @param axis the axis of rotation
581 * @param angle angle of rotation in radians
582 * @return a new instance representing the defined rotation
583 *
584 * @throws IllegalArgumentException if the given axis cannot be normalized or the angle is NaN or infinite
585 */
586 public static QuaternionRotation fromAxisAngle(final Vector3D axis, final double angle) {
587 // reference formula:
588 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm
589 final Vector3D normAxis = axis.normalize();
590
591 if (!Double.isFinite(angle)) {
592 throw new IllegalArgumentException("Invalid angle: " + angle);
593 }
594
595 final double halfAngle = 0.5 * angle;
596 final double sinHalfAngle = Math.sin(halfAngle);
597
598 final double w = Math.cos(halfAngle);
599 final double x = sinHalfAngle * normAxis.getX();
600 final double y = sinHalfAngle * normAxis.getY();
601 final double z = sinHalfAngle * normAxis.getZ();
602
603 return of(w, x, y, z);
604 }
605
606 /** Return an instance that rotates the first vector to the second.
607 *
608 * <p>Except for a possible scale factor, if the returned instance is
609 * applied to vector {@code u}, it will produce the vector {@code v}. There are an
610 * infinite number of such rotations; this method chooses the one with the smallest
611 * associated angle, meaning the one whose axis is orthogonal to the {@code (u, v)}
612 * plane. If {@code u} and {@code v} are collinear, an arbitrary rotation axis is
613 * chosen.</p>
614 *
615 * @param u origin vector
616 * @param v target vector
617 * @return a new instance that rotates {@code u} to point in the direction of {@code v}
618 * @throws IllegalArgumentException if either vector has a norm of zero, NaN, or infinity
619 */
620 public static QuaternionRotation createVectorRotation(final Vector3D u, final Vector3D v) {
621
622 final double normProduct = Vectors.checkedNorm(u) * Vectors.checkedNorm(v);
623 final double dot = u.dot(v);
624
625 if (dot < ANTIPARALLEL_DOT_THRESHOLD * normProduct) {
626 // Special case where u1 = -u2:
627 // create a pi angle rotation around
628 // an arbitrary unit vector orthogonal to u1
629 final Vector3D axis = u.orthogonal();
630
631 return of(0,
632 axis.getX(),
633 axis.getY(),
634 axis.getZ());
635 }
636
637 // General case:
638 // (u1, u2) defines a plane so rotate around the normal of the plane
639
640 // w must equal cos(theta/2); we can calculate this directly using values
641 // we already have with the identity cos(theta/2) = sqrt((1 + cos(theta)) / 2)
642 // and the fact that dot = norm(u1) * norm(u2) * cos(theta).
643 final double w = Math.sqrt(0.5 * (1.0 + (dot / normProduct)));
644
645 // The cross product u1 x u2 must be normalized and then multiplied by
646 // sin(theta/2) in order to set the vectorial part of the quaternion. To
647 // accomplish this, we'll use the following:
648 //
649 // 1) norm(a x b) = norm(a) * norm(b) * sin(theta)
650 // 2) sin(theta/2) = sqrt((1 - cos(theta)) / 2)
651 //
652 // Our full, combined normalization and sine half angle term factor then becomes:
653 //
654 // sqrt((1 - cos(theta)) / 2) / (norm(u1) * norm(u2) * sin(theta))
655 //
656 // This can be simplified to the expression below.
657 final double vectorialScaleFactor = 1.0 / (2.0 * w * normProduct);
658 final Vector3D axis = u.cross(v);
659
660 return of(w,
661 vectorialScaleFactor * axis.getX(),
662 vectorialScaleFactor * axis.getY(),
663 vectorialScaleFactor * axis.getZ());
664 }
665
666 /** Return an instance that rotates the basis defined by the first two vectors into the basis
667 * defined by the second two.
668 *
669 * <p>
670 * The given basis vectors do not have to be directly orthogonal. A right-handed orthonormal
671 * basis is created from each pair by normalizing the first vector, making the second vector
672 * orthogonal to the first, and then taking the cross product. A rotation is then calculated
673 * that rotates the first to the second.
674 * </p>
675 *
676 * @param u1 first vector of the source basis
677 * @param u2 second vector of the source basis
678 * @param v1 first vector of the target basis
679 * @param v2 second vector of the target basis
680 * @return an instance that rotates the source basis to the target basis
681 * @throws IllegalArgumentException if any of the input vectors cannot be normalized
682 * or the vectors defining either basis are collinear
683 */
684 public static QuaternionRotation createBasisRotation(final Vector3D u1, final Vector3D u2,
685 final Vector3D v1, final Vector3D v2) {
686
687 // calculate orthonormalized bases
688 final Vector3D a = u1.normalize();
689 final Vector3D b = a.orthogonal(u2);
690 final Vector3D c = a.cross(b);
691
692 final Vector3D d = v1.normalize();
693 final Vector3D e = d.orthogonal(v2);
694 final Vector3D f = d.cross(e);
695
696 // create an orthogonal rotation matrix representing the change of basis; this matrix will
697 // be the multiplication of the matrix composed of the column vectors d, e, f and the
698 // inverse of the matrix composed of the column vectors a, b, c (which is simply the transpose since
699 // it's orthogonal).
700 final double m00 = Vectors.linearCombination(d.getX(), a.getX(), e.getX(), b.getX(), f.getX(), c.getX());
701 final double m01 = Vectors.linearCombination(d.getX(), a.getY(), e.getX(), b.getY(), f.getX(), c.getY());
702 final double m02 = Vectors.linearCombination(d.getX(), a.getZ(), e.getX(), b.getZ(), f.getX(), c.getZ());
703
704 final double m10 = Vectors.linearCombination(d.getY(), a.getX(), e.getY(), b.getX(), f.getY(), c.getX());
705 final double m11 = Vectors.linearCombination(d.getY(), a.getY(), e.getY(), b.getY(), f.getY(), c.getY());
706 final double m12 = Vectors.linearCombination(d.getY(), a.getZ(), e.getY(), b.getZ(), f.getY(), c.getZ());
707
708 final double m20 = Vectors.linearCombination(d.getZ(), a.getX(), e.getZ(), b.getX(), f.getZ(), c.getX());
709 final double m21 = Vectors.linearCombination(d.getZ(), a.getY(), e.getZ(), b.getY(), f.getZ(), c.getY());
710 final double m22 = Vectors.linearCombination(d.getZ(), a.getZ(), e.getZ(), b.getZ(), f.getZ(), c.getZ());
711
712
713 return orthogonalRotationMatrixToQuaternion(
714 m00, m01, m02,
715 m10, m11, m12,
716 m20, m21, m22
717 );
718 }
719
720 /** Create a new instance equivalent to the given sequence of axis-angle rotations.
721 * @param sequence the axis-angle rotation sequence to convert to a quaternion rotation
722 * @return instance representing a rotation equivalent to the given axis-angle sequence
723 */
724 public static QuaternionRotation fromAxisAngleSequence(final AxisAngleSequence sequence) {
725 final AxisSequence axes = sequence.getAxisSequence();
726
727 final QuaternionRotation q1 = fromAxisAngle(axes.getAxis1(), sequence.getAngle1());
728 final QuaternionRotation q2 = fromAxisAngle(axes.getAxis2(), sequence.getAngle2());
729 final QuaternionRotation q3 = fromAxisAngle(axes.getAxis3(), sequence.getAngle3());
730
731 if (sequence.getReferenceFrame() == AxisReferenceFrame.ABSOLUTE) {
732 return q3.multiply(q2).multiply(q1);
733 }
734
735 return q1.multiply(q2).multiply(q3);
736 }
737
738 /** Create an instance from an orthogonal rotation matrix.
739 *
740 * @param m00 matrix entry <code>m<sub>0,0</sub></code>
741 * @param m01 matrix entry <code>m<sub>0,1</sub></code>
742 * @param m02 matrix entry <code>m<sub>0,2</sub></code>
743 * @param m10 matrix entry <code>m<sub>1,0</sub></code>
744 * @param m11 matrix entry <code>m<sub>1,1</sub></code>
745 * @param m12 matrix entry <code>m<sub>1,2</sub></code>
746 * @param m20 matrix entry <code>m<sub>2,0</sub></code>
747 * @param m21 matrix entry <code>m<sub>2,1</sub></code>
748 * @param m22 matrix entry <code>m<sub>2,2</sub></code>
749 * @return an instance representing the same 3D rotation as the given matrix
750 */
751 private static QuaternionRotation orthogonalRotationMatrixToQuaternion(
752 final double m00, final double m01, final double m02,
753 final double m10, final double m11, final double m12,
754 final double m20, final double m21, final double m22) {
755
756 // reference formula:
757 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
758
759 // The overall approach here is to take the equations for converting a quaternion to
760 // a matrix along with the fact that 1 = x^2 + y^2 + z^2 + w^2 for a normalized quaternion
761 // and solve for the various terms. This can theoretically be done using just the diagonal
762 // terms from the matrix. However, there are a few issues with this:
763 // 1) The term that we end up taking the square root of may be negative.
764 // 2) It's ambiguous as to whether we should use a plus or minus for the value of the
765 // square root.
766 // We'll address these concerns by only calculating a single term from one of the diagonal
767 // elements and then calculate the rest from the non-diagonals, which do not involve
768 // a square root. This solves the first issue since we can make sure to choose a diagonal
769 // element that will not cause us to take a square root of a negative number. The second
770 // issue is solved since only the relative signs between the quaternion terms are important
771 // (q and -q represent the same 3D rotation). It therefore doesn't matter whether we choose
772 // a plus or minus for our initial square root solution.
773
774 final double trace = m00 + m11 + m22;
775
776 final double w;
777 final double x;
778 final double y;
779 final double z;
780
781 if (trace > 0) {
782 // let s = 4*w
783 final double s = 2.0 * Math.sqrt(1.0 + trace);
784 final double sinv = 1.0 / s;
785
786 x = (m21 - m12) * sinv;
787 y = (m02 - m20) * sinv;
788 z = (m10 - m01) * sinv;
789 w = 0.25 * s;
790 } else if ((m00 > m11) && (m00 > m22)) {
791 // let s = 4*x
792 final double s = 2.0 * Math.sqrt(1.0 + m00 - m11 - m22);
793 final double sinv = 1.0 / s;
794
795 x = 0.25 * s;
796 y = (m01 + m10) * sinv;
797 z = (m02 + m20) * sinv;
798 w = (m21 - m12) * sinv;
799 } else if (m11 > m22) {
800 // let s = 4*y
801 final double s = 2.0 * Math.sqrt(1.0 + m11 - m00 - m22);
802 final double sinv = 1.0 / s;
803
804 x = (m01 + m10) * sinv;
805 y = 0.25 * s;
806 z = (m21 + m12) * sinv;
807 w = (m02 - m20) * sinv;
808 } else {
809 // let s = 4*z
810 final double s = 2.0 * Math.sqrt(1.0 + m22 - m00 - m11);
811 final double sinv = 1.0 / s;
812
813 x = (m02 + m20) * sinv;
814 y = (m21 + m12) * sinv;
815 z = 0.25 * s;
816 w = (m10 - m01) * sinv;
817 }
818
819 return of(w, x, y, z);
820 }
821
822 /** Reverse the elements in {@code arr}. The array is returned.
823 * @param arr the array to reverse
824 * @return the input array with the elements reversed
825 */
826 private static double[] reverseArray(final double[] arr) {
827
828 final int len = arr.length;
829 double temp;
830
831 int i;
832 int j;
833
834 for (i = 0, j = len - 1; i < len / 2; ++i, --j) {
835 temp = arr[i];
836 arr[i] = arr[j];
837 arr[j] = temp;
838 }
839
840 return arr;
841 }
842 }