| /* |
| * Licensed to the Apache Software Foundation (ASF) under one or more |
| * contributor license agreements. See the NOTICE file distributed with |
| * this work for additional information regarding copyright ownership. |
| * The ASF licenses this file to You under the Apache License, Version 2.0 |
| * (the "License"); you may not use this file except in compliance with |
| * the License. You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| */ |
| package org.apache.commons.geometry.euclidean.threed.rotation; |
| |
| import java.util.Objects; |
| |
| import org.apache.commons.geometry.core.Geometry; |
| import org.apache.commons.geometry.core.exception.GeometryValueException; |
| import org.apache.commons.geometry.core.exception.IllegalNormException; |
| import org.apache.commons.geometry.core.internal.GeometryInternalError; |
| import org.apache.commons.geometry.euclidean.internal.Vectors; |
| import org.apache.commons.geometry.euclidean.threed.AffineTransformMatrix3D; |
| import org.apache.commons.geometry.euclidean.threed.Vector3D; |
| import org.apache.commons.numbers.arrays.LinearCombination; |
| import org.apache.commons.numbers.quaternion.Quaternion; |
| import org.apache.commons.numbers.quaternion.Slerp; |
| |
| /** |
| * Class using a unit-length quaternion to represent |
| * <a href="https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation">rotations</a> |
| * in 3-dimensional Euclidean space. |
| * The underlying quaternion is in <em>positive polar form</em>: It is normalized and has a |
| * non-negative scalar component ({@code w}). |
| * |
| * @see Quaternion |
| */ |
| public final class QuaternionRotation implements Rotation3D { |
| /** Threshold value for the dot product of antiparallel vectors. If the dot product of two vectors is |
| * less than this value, (adjusted for the lengths of the vectors), then the vectors are considered to be |
| * antiparallel (ie, negations of each other). |
| */ |
| private static final double ANTIPARALLEL_DOT_THRESHOLD = 2.0e-15 - 1.0; |
| |
| /** Threshold value used to identify singularities when converting from quaternions to |
| * axis angle sequences. |
| */ |
| private static final double AXIS_ANGLE_SINGULARITY_THRESHOLD = 0.9999999999; |
| |
| /** Instance used to represent the identity rotation, ie a rotation with |
| * an angle of zero. |
| */ |
| private static final QuaternionRotation IDENTITY_INSTANCE = of(Quaternion.ONE); |
| |
| /** Unit-length quaternion instance in positive polar form. */ |
| private final Quaternion quat; |
| |
| /** Simple constructor. The given quaternion is converted to positive polar form. |
| * @param quat quaternion instance |
| * @throws IllegalStateException if the the norm of the given components is zero, |
| * NaN, or infinite |
| */ |
| private QuaternionRotation(final Quaternion quat) { |
| this.quat = quat.positivePolarForm(); |
| } |
| |
| /** Get the underlying quaternion instance. |
| * @return the quaternion instance |
| */ |
| public Quaternion getQuaternion() { |
| return quat; |
| } |
| |
| /** |
| * Get the axis of rotation as a normalized {@link Vector3D}. The rotation axis |
| * is not well defined when the rotation is the identity rotation, ie it has a |
| * rotation angle of zero. In this case, the vector representing the positive |
| * x-axis is returned. |
| * |
| * @return the axis of rotation |
| */ |
| @Override |
| public Vector3D getAxis() { |
| // the most straightforward way to check if we have a normalizable |
| // vector is to just try to normalize it and see if we fail |
| try { |
| return Vector3D.Unit.from(quat.getX(), quat.getY(), quat.getZ()); |
| } catch (IllegalNormException exc) { |
| return Vector3D.Unit.PLUS_X; |
| } |
| } |
| |
| /** |
| * Get the angle of rotation in radians. The returned value is in the range 0 |
| * through {@code pi}. |
| * |
| * @return The rotation angle in the range {@code [0, pi]}. |
| */ |
| @Override |
| public double getAngle() { |
| return 2 * Math.acos(quat.getW()); |
| } |
| |
| /** |
| * Get the inverse of this rotation. The returned rotation has the same |
| * rotation angle but the opposite rotation axis. If {@code r.apply(u)} |
| * is equal to {@code v}, then {@code r.negate().apply(v)} is equal |
| * to {@code u}. |
| * |
| * @return the negation (inverse) of the rotation |
| */ |
| @Override |
| public QuaternionRotation inverse() { |
| return new QuaternionRotation(quat.conjugate()); |
| } |
| |
| /** |
| * Apply this rotation to the given vector. |
| * |
| * @param v vector to rotate |
| * @return the rotated vector |
| */ |
| @Override |
| public Vector3D apply(final Vector3D v) { |
| final double qw = quat.getW(); |
| final double qx = quat.getX(); |
| final double qy = quat.getY(); |
| final double qz = quat.getZ(); |
| |
| final double x = v.getX(); |
| final double y = v.getY(); |
| final double z = v.getZ(); |
| |
| // calculate the Hamilton product of the quaternion and vector |
| final double iw = -(qx * x) - (qy * y) - (qz * z); |
| final double ix = (qw * x) + (qy * z) - (qz * y); |
| final double iy = (qw * y) + (qz * x) - (qx * z); |
| final double iz = (qw * z) + (qx * y) - (qy * x); |
| |
| // calculate the Hamilton product of the intermediate vector and |
| // the inverse quaternion |
| |
| return Vector3D.of( |
| (iw * -qx) + (ix * qw) + (iy * -qz) - (iz * -qy), |
| (iw * -qy) - (ix * -qz) + (iy * qw) + (iz * -qx), |
| (iw * -qz) + (ix * -qy) - (iy * -qx) + (iz * qw) |
| ); |
| } |
| |
| /** {@inheritDoc} |
| * |
| * <p>This method simply calls {@code apply(vec)} since rotations treat |
| * points and vectors similarly.</p> |
| */ |
| @Override |
| public Vector3D applyVector(final Vector3D vec) { |
| return apply(vec); |
| } |
| |
| /** {@inheritDoc} |
| * |
| * <p>This method simply returns true since rotations always preserve the orientation |
| * of the space.</p> |
| */ |
| @Override |
| public boolean preservesOrientation() { |
| return true; |
| } |
| |
| /** {@inheritDoc} */ |
| @Override |
| public AffineTransformMatrix3D toMatrix() { |
| |
| final double qw = quat.getW(); |
| final double qx = quat.getX(); |
| final double qy = quat.getY(); |
| final double qz = quat.getZ(); |
| |
| // pre-calculate products that we'll need |
| final double xx = qx * qx; |
| final double xy = qx * qy; |
| final double xz = qx * qz; |
| final double xw = qx * qw; |
| |
| final double yy = qy * qy; |
| final double yz = qy * qz; |
| final double yw = qy * qw; |
| |
| final double zz = qz * qz; |
| final double zw = qz * qw; |
| |
| final double m00 = 1.0 - (2.0 * (yy + zz)); |
| final double m01 = 2.0 * (xy - zw); |
| final double m02 = 2.0 * (xz + yw); |
| final double m03 = 0.0; |
| |
| final double m10 = 2.0 * (xy + zw); |
| final double m11 = 1.0 - (2.0 * (xx + zz)); |
| final double m12 = 2.0 * (yz - xw); |
| final double m13 = 0.0; |
| |
| final double m20 = 2.0 * (xz - yw); |
| final double m21 = 2.0 * (yz + xw); |
| final double m22 = 1.0 - (2.0 * (xx + yy)); |
| final double m23 = 0.0; |
| |
| return AffineTransformMatrix3D.of( |
| m00, m01, m02, m03, |
| m10, m11, m12, m13, |
| m20, m21, m22, m23 |
| ); |
| } |
| |
| /** |
| * Multiply this instance by the given argument, returning the result as |
| * a new instance. This is equivalent to the expression {@code t * q} where |
| * {@code q} is the argument and {@code t} is this instance. |
| * |
| * <p> |
| * Multiplication of quaternions behaves similarly to transformation |
| * matrices in regard to the order that operations are performed. |
| * For example, if <code>q<sub>1</sub></code> and <code>q<sub>2</sub></code> are unit |
| * quaternions, then the quaternion <code>q<sub>r</sub> = q<sub>1</sub>*q<sub>2</sub></code> |
| * will give the effect of applying the rotation in <code>q<sub>2</sub></code> followed |
| * by the rotation in <code>q<sub>1</sub></code>. In other words, the rightmost element |
| * in the multiplication is applied first. |
| * </p> |
| * |
| * @param q quaternion to multiply with the current instance |
| * @return the result of multiplying this quaternion by the argument |
| */ |
| public QuaternionRotation multiply(final QuaternionRotation q) { |
| final Quaternion product = quat.multiply(q.quat); |
| return new QuaternionRotation(product); |
| } |
| |
| /** Multiply the argument by this instance, returning the result as |
| * a new instance. This is equivalent to the expression {@code q * t} where |
| * {@code q} is the argument and {@code t} is this instance. |
| * |
| * <p> |
| * Multiplication of quaternions behaves similarly to transformation |
| * matrices in regard to the order that operations are performed. |
| * For example, if <code>q<sub>1</sub></code> and <code>q<sub>2</sub></code> are unit |
| * quaternions, then the quaternion <code>q<sub>r</sub> = q<sub>1</sub>*q<sub>2</sub></code> |
| * will give the effect of applying the rotation in <code>q<sub>2</sub></code> followed |
| * by the rotation in <code>q<sub>1</sub></code>. In other words, the rightmost element |
| * in the multiplication is applied first. |
| * </p> |
| * |
| * @param q quaternion to multiply by the current instance |
| * @return the result of multiplying the argument by the current instance |
| */ |
| public QuaternionRotation premultiply(final QuaternionRotation q) { |
| return q.multiply(this); |
| } |
| |
| /** |
| * Creates a {@link Slerp} transform. |
| * |
| * @param end End rotation of the interpolation. |
| * @return the transform. |
| */ |
| public Slerp slerp(QuaternionRotation end) { |
| return new Slerp(quat, end.quat); |
| } |
| |
| /** Get a sequence of axis-angle rotations that produce an overall rotation equivalent to this instance. |
| * |
| * <p> |
| * In most cases, the returned rotation sequence will be unique. However, at points of singularity |
| * (second angle equal to {@code 0} or {@code -pi} for Euler angles and {@code +pi/2} or {@code -pi/2} |
| * for Tait-Bryan angles), there are an infinite number of possible sequences that produce the same result. |
| * In these cases, the result is returned that leaves the last rotation equal to 0 (in the case of a relative |
| * reference frame) or the first rotation equal to 0 (in the case of an absolute reference frame). |
| * </p> |
| * |
| * @param frame the reference frame used to interpret the positions of the rotation axes |
| * @param axes the sequence of rotation axes |
| * @return a sequence of axis-angle rotations equivalent to this rotation |
| */ |
| public AxisAngleSequence toAxisAngleSequence(final AxisReferenceFrame frame, final AxisSequence axes) { |
| if (frame == null) { |
| throw new IllegalArgumentException("Axis reference frame cannot be null"); |
| } |
| if (axes == null) { |
| throw new IllegalArgumentException("Axis sequence cannot be null"); |
| } |
| |
| double[] angles = getAngles(frame, axes); |
| |
| return new AxisAngleSequence(frame, axes, angles[0], angles[1], angles[2]); |
| } |
| |
| /** Get a sequence of axis-angle rotations that produce an overall rotation equivalent to this instance. |
| * Each rotation axis is interpreted relative to the rotated coordinate frame (ie, intrinsic rotation). |
| * @param axes the sequence of rotation axes |
| * @return a sequence of relative axis-angle rotations equivalent to this rotation |
| * @see #toAxisAngleSequence(AxisReferenceFrame, AxisSequence) |
| */ |
| public AxisAngleSequence toRelativeAxisAngleSequence(final AxisSequence axes) { |
| return toAxisAngleSequence(AxisReferenceFrame.RELATIVE, axes); |
| } |
| |
| /** Get a sequence of axis-angle rotations that produce an overall rotation equivalent to this instance. |
| * Each rotation axis is interpreted as part of an absolute, unmoving coordinate frame (ie, extrinsic rotation). |
| * @param axes the sequence of rotation axes |
| * @return a sequence of absolute axis-angle rotations equivalent to this rotation |
| * @see #toAxisAngleSequence(AxisReferenceFrame, AxisSequence) |
| */ |
| public AxisAngleSequence toAbsoluteAxisAngleSequence(final AxisSequence axes) { |
| return toAxisAngleSequence(AxisReferenceFrame.ABSOLUTE, axes); |
| } |
| |
| /** {@inheritDoc} */ |
| @Override |
| public int hashCode() { |
| return quat.hashCode(); |
| } |
| |
| /** {@inheritDoc} */ |
| @Override |
| public boolean equals(Object obj) { |
| if (this == obj) { |
| return true; |
| } |
| if (!(obj instanceof QuaternionRotation)) { |
| return false; |
| } |
| |
| QuaternionRotation other = (QuaternionRotation) obj; |
| return Objects.equals(this.quat, other.quat); |
| } |
| |
| /** {@inheritDoc} */ |
| @Override |
| public String toString() { |
| return quat.toString(); |
| } |
| |
| /** Get a sequence of angles around the given axes that produce a rotation equivalent |
| * to this instance. |
| * @param frame the reference frame used to define the positions of the axes |
| * @param axes the axis sequence |
| * @return a sequence of angles around the given axes that produce a rotation equivalent |
| * to this instance |
| */ |
| private double[] getAngles(final AxisReferenceFrame frame, final AxisSequence axes) { |
| |
| AxisSequenceType sequenceType = axes.getType(); |
| |
| final Vector3D axis1 = axes.getAxis1(); |
| final Vector3D axis2 = axes.getAxis2(); |
| final Vector3D axis3 = axes.getAxis3(); |
| |
| if (frame == AxisReferenceFrame.RELATIVE) { |
| if (sequenceType == AxisSequenceType.TAIT_BRYAN) { |
| return getRelativeTaitBryanAngles(axis1, axis2, axis3); |
| } else if (sequenceType == AxisSequenceType.EULER) { |
| return getRelativeEulerAngles(axis1, axis2, axis3); |
| } |
| } else if (frame == AxisReferenceFrame.ABSOLUTE) { |
| if (sequenceType == AxisSequenceType.TAIT_BRYAN) { |
| return getAbsoluteTaitBryanAngles(axis1, axis2, axis3); |
| } else if (sequenceType == AxisSequenceType.EULER) { |
| return getAbsoluteEulerAngles(axis1, axis2, axis3); |
| } |
| } |
| |
| // all possibilities should have been covered above |
| throw new GeometryInternalError(); |
| } |
| |
| /** Get a sequence of angles around the given Tait-Bryan axes that produce a rotation equivalent |
| * to this instance. The axes are interpreted as being relative to the rotated coordinate frame. |
| * @param axis1 first Tait-Bryan axis |
| * @param axis2 second Tait-Bryan axis |
| * @param axis3 third Tait-Bryan axis |
| * @return a sequence of rotation angles around the relative input axes that produce a rotation equivalent |
| * to this instance |
| */ |
| private double[] getRelativeTaitBryanAngles(final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) { |
| |
| // We can use geometry to get the first and second angles pretty easily here by analyzing the positions |
| // of the transformed rotation axes. The third angle is trickier but we can get it by treating it as |
| // if it were the first rotation in the inverse (which it would be). |
| |
| final Vector3D vec3 = apply(axis3); |
| final Vector3D invVec1 = inverse().apply(axis1); |
| |
| final double angle2Sin = vec3.dot(axis2.cross(axis3)); |
| |
| if (angle2Sin < -AXIS_ANGLE_SINGULARITY_THRESHOLD || |
| angle2Sin > AXIS_ANGLE_SINGULARITY_THRESHOLD) { |
| |
| final Vector3D vec2 = apply(axis2); |
| |
| final double angle1TanY = vec2.dot(axis1.cross(axis2)); |
| final double angle1TanX = vec2.dot(axis2); |
| |
| final double angle2 = angle2Sin > AXIS_ANGLE_SINGULARITY_THRESHOLD ? |
| Geometry.HALF_PI : |
| Geometry.MINUS_HALF_PI; |
| |
| return new double[] { |
| Math.atan2(angle1TanY, angle1TanX), |
| angle2, |
| Geometry.ZERO_PI |
| }; |
| } |
| |
| final Vector3D crossAxis13 = axis1.cross(axis3); |
| |
| final double angle1TanY = vec3.dot(crossAxis13); |
| final double angle1TanX = vec3.dot(axis3); |
| |
| final double angle3TanY = invVec1.dot(crossAxis13); |
| final double angle3TanX = invVec1.dot(axis1); |
| |
| return new double[] { |
| Math.atan2(angle1TanY, angle1TanX), |
| Math.asin(angle2Sin), |
| Math.atan2(angle3TanY, angle3TanX) |
| }; |
| } |
| |
| /** Get a sequence of angles around the given Tait-Bryan axes that produce a rotation equivalent |
| * to this instance. The axes are interpreted as being part of an absolute (unmoving) coordinate frame. |
| * @param axis1 first Tait-Bryan axis |
| * @param axis2 second Tait-Bryan axis |
| * @param axis3 third Tait-Bryan axis |
| * @return a sequence of rotation angles around the absolute input axes that produce a rotation equivalent |
| * to this instance |
| */ |
| private double[] getAbsoluteTaitBryanAngles(final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) { |
| // A relative axis-angle rotation sequence is equivalent to an absolute one with the rotation |
| // sequence reversed, meaning we can reuse our relative logic here. |
| return reverseArray(getRelativeTaitBryanAngles(axis3, axis2, axis1)); |
| } |
| |
| /** Get a sequence of angles around the given Euler axes that produce a rotation equivalent |
| * to this instance. The axes are interpreted as being relative to the rotated coordinate frame. |
| * @param axis1 first Euler axis |
| * @param axis2 second Euler axis |
| * @param axis3 third Euler axis |
| * @return a sequence of rotation angles around the relative input axes that produce a rotation equivalent |
| * to this instance |
| */ |
| private double[] getRelativeEulerAngles(final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) { |
| |
| // Use the same overall approach as with the Tait-Bryan angles: get the first two angles by looking |
| // at the transformed rotation axes and the third by using the inverse. |
| |
| final Vector3D crossAxis = axis1.cross(axis2); |
| |
| final Vector3D vec1 = apply(axis1); |
| final Vector3D invVec1 = inverse().apply(axis1); |
| |
| final double angle2Cos = vec1.dot(axis1); |
| |
| if (angle2Cos < -AXIS_ANGLE_SINGULARITY_THRESHOLD || |
| angle2Cos > AXIS_ANGLE_SINGULARITY_THRESHOLD) { |
| |
| final Vector3D vec2 = apply(axis2); |
| |
| final double angle1TanY = vec2.dot(crossAxis); |
| final double angle1TanX = vec2.dot(axis2); |
| |
| final double angle2 = angle2Cos > AXIS_ANGLE_SINGULARITY_THRESHOLD ? Geometry.ZERO_PI : Geometry.PI; |
| |
| return new double[] { |
| Math.atan2(angle1TanY, angle1TanX), |
| angle2, |
| Geometry.ZERO_PI |
| }; |
| } |
| |
| final double angle1TanY = vec1.dot(axis2); |
| final double angle1TanX = -vec1.dot(crossAxis); |
| |
| final double angle3TanY = invVec1.dot(axis2); |
| final double angle3TanX = invVec1.dot(crossAxis); |
| |
| return new double[] { |
| Math.atan2(angle1TanY, angle1TanX), |
| Math.acos(angle2Cos), |
| Math.atan2(angle3TanY, angle3TanX) |
| }; |
| } |
| |
| /** Get a sequence of angles around the given Euler axes that produce a rotation equivalent |
| * to this instance. The axes are interpreted as being part of an absolute (unmoving) coordinate frame. |
| * @param axis1 first Euler axis |
| * @param axis2 second Euler axis |
| * @param axis3 third Euler axis |
| * @return a sequence of rotation angles around the absolute input axes that produce a rotation equivalent |
| * to this instance |
| */ |
| private double[] getAbsoluteEulerAngles(final Vector3D axis1, final Vector3D axis2, final Vector3D axis3) { |
| // A relative axis-angle rotation sequence is equivalent to an absolute one with the rotation |
| // sequence reversed, meaning we can reuse our relative logic here. |
| return reverseArray(getRelativeEulerAngles(axis3, axis2, axis1)); |
| } |
| |
| /** Create a new instance from the given quaternion. The quaternion is normalized and |
| * converted to positive polar form (ie, with w >= 0). |
| * |
| * @param quat the quaternion to use for the rotation |
| * @return a new instance built from the given quaternion. |
| * @throws IllegalStateException if the the norm of the given components is zero, |
| * NaN, or infinite |
| * @see Quaternion#normalize() |
| * @see Quaternion#positivePolarForm() |
| */ |
| public static QuaternionRotation of(Quaternion quat) { |
| return new QuaternionRotation(quat); |
| } |
| |
| /** |
| * Create a new instance from the given quaternion values. The inputs are |
| * normalized and converted to positive polar form (ie, with w >= 0). |
| * |
| * @param w quaternion scalar component |
| * @param x first quaternion vectorial component |
| * @param y second quaternion vectorial component |
| * @param z third quaternion vectorial component |
| * @return a new instance containing the normalized quaterion components |
| * @throws IllegalStateException if the the norm of the given components is zero, |
| * NaN, or infinite |
| * @see Quaternion#normalize() |
| * @see Quaternion#positivePolarForm() |
| */ |
| public static QuaternionRotation of(final double w, |
| final double x, |
| final double y, |
| final double z) { |
| return of(Quaternion.of(w, x, y, z)); |
| } |
| |
| /** Return an instance representing a rotation of zero. |
| * @return instance representing a rotation of zero. |
| */ |
| public static QuaternionRotation identity() { |
| return IDENTITY_INSTANCE; |
| } |
| |
| /** Create a new instance representing a rotation of {@code angle} radians around |
| * {@code axis}. |
| * |
| * <p> |
| * Rotation direction follows the right-hand rule, meaning that if one |
| * places their right hand such that the thumb points in the direction of the vector, |
| * the curl of the fingers indicates the direction of rotation. |
| * </p> |
| * |
| * <p> |
| * Note that the returned quaternion will represent the defined rotation but the values |
| * returned by {@link #getAxis()} and {@link #getAngle()} may not match the ones given here. |
| * This is because the axis and angle are normalized such that the axis has unit length, |
| * and the angle lies in the range {@code [0, pi]}. Depending on the inputs, the axis may |
| * need to be inverted in order for the angle to lie in this range. |
| * </p> |
| * |
| * @param axis the axis of rotation |
| * @param angle angle of rotation in radians |
| * @return a new instance representing the defined rotation |
| * |
| * @throws IllegalNormException if the given axis cannot be normalized |
| * @throws GeometryValueException if the angle is NaN or infinite |
| */ |
| public static QuaternionRotation fromAxisAngle(final Vector3D axis, final double angle) { |
| // reference formula: |
| // http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm |
| final Vector3D normAxis = axis.normalize(); |
| |
| if (!Double.isFinite(angle)) { |
| throw new GeometryValueException("Invalid angle: " + angle); |
| } |
| |
| final double halfAngle = 0.5 * angle; |
| final double sinHalfAngle = Math.sin(halfAngle); |
| |
| final double w = Math.cos(halfAngle); |
| final double x = sinHalfAngle * normAxis.getX(); |
| final double y = sinHalfAngle * normAxis.getY(); |
| final double z = sinHalfAngle * normAxis.getZ(); |
| |
| return of(w, x, y, z); |
| } |
| |
| /** Return an instance that rotates the first vector to the second. |
| * |
| * <p>Except for a possible scale factor, if the returned instance is |
| * applied to vector {@code u}, it will produce the vector {@code v}. There are an |
| * infinite number of such rotations; this method chooses the one with the smallest |
| * associated angle, meaning the one whose axis is orthogonal to the {@code (u, v)} |
| * plane. If {@code u} and {@code v} are collinear, an arbitrary rotation axis is |
| * chosen.</p> |
| * |
| * @param u origin vector |
| * @param v target vector |
| * @return a new instance that rotates {@code u} to point in the direction of {@code v} |
| * @throws IllegalNormException if either vector has a norm of zero, NaN, or infinity |
| */ |
| public static QuaternionRotation createVectorRotation(final Vector3D u, final Vector3D v) { |
| |
| double normProduct = Vectors.checkedNorm(u) * Vectors.checkedNorm(v); |
| double dot = u.dot(v); |
| |
| if (dot < ANTIPARALLEL_DOT_THRESHOLD * normProduct) { |
| // Special case where u1 = -u2: |
| // create a pi angle rotation around |
| // an arbitrary unit vector orthogonal to u1 |
| final Vector3D axis = u.orthogonal(); |
| |
| return of(0, |
| axis.getX(), |
| axis.getY(), |
| axis.getZ()); |
| } |
| |
| // General case: |
| // (u1, u2) defines a plane so rotate around the normal of the plane |
| |
| // w must equal cos(theta/2); we can calculate this directly using values |
| // we already have with the identity cos(theta/2) = sqrt((1 + cos(theta)) / 2) |
| // and the fact that dot = norm(u1) * norm(u2) * cos(theta). |
| final double w = Math.sqrt(0.5 * (1.0 + (dot / normProduct))); |
| |
| // The cross product u1 x u2 must be normalized and then multiplied by |
| // sin(theta/2) in order to set the vectorial part of the quaternion. To |
| // accomplish this, we'll use the following: |
| // |
| // 1) norm(a x b) = norm(a) * norm(b) * sin(theta) |
| // 2) sin(theta/2) = sqrt((1 - cos(theta)) / 2) |
| // |
| // Our full, combined normalization and sine half angle term factor then becomes: |
| // |
| // sqrt((1 - cos(theta)) / 2) / (norm(u1) * norm(u2) * sin(theta)) |
| // |
| // This can be simplified to the expression below. |
| final double vectorialScaleFactor = 1.0 / (2.0 * w * normProduct); |
| final Vector3D axis = u.cross(v); |
| |
| return of(w, |
| vectorialScaleFactor * axis.getX(), |
| vectorialScaleFactor * axis.getY(), |
| vectorialScaleFactor * axis.getZ()); |
| } |
| |
| /** Return an instance that rotates the basis defined by the first two vectors into the basis |
| * defined by the second two. |
| * |
| * <p> |
| * The given basis vectors do not have to be directly orthogonal. A right-handed orthonormal |
| * basis is created from each pair by normalizing the first vector, making the second vector |
| * orthogonal to the first, and then taking the cross product. A rotation is then calculated |
| * that rotates the first to the second. |
| * </p> |
| * |
| * @param u1 first vector of the source basis |
| * @param u2 second vector of the source basis |
| * @param v1 first vector of the target basis |
| * @param v2 second vector of the target basis |
| * @return an instance that rotates the source basis to the target basis |
| * @throws IllegalNormException if any of the input vectors cannot be normalized |
| * or the vectors defining either basis are colinear |
| */ |
| public static QuaternionRotation createBasisRotation(final Vector3D u1, final Vector3D u2, |
| final Vector3D v1, final Vector3D v2) { |
| |
| // calculate orthonormalized bases |
| final Vector3D a = u1.normalize(); |
| final Vector3D b = a.orthogonal(u2); |
| final Vector3D c = a.cross(b); |
| |
| final Vector3D d = v1.normalize(); |
| final Vector3D e = d.orthogonal(v2); |
| final Vector3D f = d.cross(e); |
| |
| // create an orthogonal rotation matrix representing the change of basis; this matrix will |
| // be the multiplication of the matrix composed of the column vectors d, e, f and the |
| // inverse of the matrix composed of the column vectors a, b, c (which is simply the transpose since |
| // it's orthogonal). |
| final double m00 = LinearCombination.value(d.getX(), a.getX(), e.getX(), b.getX(), f.getX(), c.getX()); |
| final double m01 = LinearCombination.value(d.getX(), a.getY(), e.getX(), b.getY(), f.getX(), c.getY()); |
| final double m02 = LinearCombination.value(d.getX(), a.getZ(), e.getX(), b.getZ(), f.getX(), c.getZ()); |
| |
| final double m10 = LinearCombination.value(d.getY(), a.getX(), e.getY(), b.getX(), f.getY(), c.getX()); |
| final double m11 = LinearCombination.value(d.getY(), a.getY(), e.getY(), b.getY(), f.getY(), c.getY()); |
| final double m12 = LinearCombination.value(d.getY(), a.getZ(), e.getY(), b.getZ(), f.getY(), c.getZ()); |
| |
| final double m20 = LinearCombination.value(d.getZ(), a.getX(), e.getZ(), b.getX(), f.getZ(), c.getX()); |
| final double m21 = LinearCombination.value(d.getZ(), a.getY(), e.getZ(), b.getY(), f.getZ(), c.getY()); |
| final double m22 = LinearCombination.value(d.getZ(), a.getZ(), e.getZ(), b.getZ(), f.getZ(), c.getZ()); |
| |
| |
| return orthogonalRotationMatrixToQuaternion( |
| m00, m01, m02, |
| m10, m11, m12, |
| m20, m21, m22 |
| ); |
| } |
| |
| /** Create a new instance equivalent to the given sequence of axis-angle rotations. |
| * @param sequence the axis-angle rotation sequence to convert to a quaternion rotation |
| * @return instance representing a rotation equivalent to the given axis-angle sequence |
| */ |
| public static QuaternionRotation fromAxisAngleSequence(final AxisAngleSequence sequence) { |
| final AxisSequence axes = sequence.getAxisSequence(); |
| |
| final QuaternionRotation q1 = fromAxisAngle(axes.getAxis1(), sequence.getAngle1()); |
| final QuaternionRotation q2 = fromAxisAngle(axes.getAxis2(), sequence.getAngle2()); |
| final QuaternionRotation q3 = fromAxisAngle(axes.getAxis3(), sequence.getAngle3()); |
| |
| if (sequence.getReferenceFrame() == AxisReferenceFrame.ABSOLUTE) { |
| return q3.multiply(q2).multiply(q1); |
| } |
| |
| return q1.multiply(q2).multiply(q3); |
| } |
| |
| /** Create an instance from an orthogonal rotation matrix. |
| * |
| * @param m00 matrix entry <code>m<sub>0,0</sub></code> |
| * @param m01 matrix entry <code>m<sub>0,1</sub></code> |
| * @param m02 matrix entry <code>m<sub>0,2</sub></code> |
| * @param m10 matrix entry <code>m<sub>1,0</sub></code> |
| * @param m11 matrix entry <code>m<sub>1,1</sub></code> |
| * @param m12 matrix entry <code>m<sub>1,2</sub></code> |
| * @param m20 matrix entry <code>m<sub>2,0</sub></code> |
| * @param m21 matrix entry <code>m<sub>2,1</sub></code> |
| * @param m22 matrix entry <code>m<sub>2,2</sub></code> |
| * @return an instance representing the same 3D rotation as the given matrix |
| */ |
| private static QuaternionRotation orthogonalRotationMatrixToQuaternion( |
| final double m00, final double m01, final double m02, |
| final double m10, final double m11, final double m12, |
| final double m20, final double m21, final double m22) { |
| |
| // reference formula: |
| // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ |
| |
| // The overall approach here is to take the equations for converting a quaternion to |
| // a matrix along with the fact that 1 = x^2 + y^2 + z^2 + w^2 for a normalized quaternion |
| // and solve for the various terms. This can theoretically be done using just the diagonal |
| // terms from the matrix. However, there are a few issues with this: |
| // 1) The term that we end up taking the square root of may be negative. |
| // 2) It's ambiguous as to whether we should use a plus or minus for the value of the |
| // square root. |
| // We'll address these concerns by only calculating a single term from one of the diagonal |
| // elements and then calculate the rest from the non-diagonals, which do not involve |
| // a square root. This solves the first issue since we can make sure to choose a diagonal |
| // element that will not cause us to take a square root of a negative number. The second |
| // issue is solved since only the relative signs between the quaternion terms are important |
| // (q and -q represent the same 3D rotation). It therefore doesn't matter whether we choose |
| // a plus or minus for our initial square root solution. |
| |
| final double trace = m00 + m11 + m22; |
| |
| double w; |
| double x; |
| double y; |
| double z; |
| |
| if (trace > 0.0) { |
| // let s = 4*w |
| final double s = 2.0 * Math.sqrt(1.0 + trace); |
| final double sinv = 1.0 / s; |
| |
| x = (m21 - m12) * sinv; |
| y = (m02 - m20) * sinv; |
| z = (m10 - m01) * sinv; |
| w = 0.25 * s; |
| } else if ((m00 > m11) && (m00 > m22)) { |
| // let s = 4*x |
| final double s = 2.0 * Math.sqrt(1.0 + m00 - m11 - m22); |
| final double sinv = 1.0 / s; |
| |
| x = 0.25 * s; |
| y = (m01 + m10) * sinv; |
| z = (m02 + m20) * sinv; |
| w = (m21 - m12) * sinv; |
| } else if (m11 > m22) { |
| // let s = 4*y |
| final double s = 2.0 * Math.sqrt(1.0 + m11 - m00 - m22); |
| final double sinv = 1.0 / s; |
| |
| x = (m01 + m10) * sinv; |
| y = 0.25 * s; |
| z = (m21 + m12) * sinv; |
| w = (m02 - m20) * sinv; |
| } else { |
| // let s = 4*z |
| final double s = 2.0 * Math.sqrt(1.0 + m22 - m00 - m11); |
| final double sinv = 1.0 / s; |
| |
| x = (m02 + m20) * sinv; |
| y = (m21 + m12) * sinv; |
| z = 0.25 * s; |
| w = (m10 - m01) * sinv; |
| } |
| |
| return of(w, x, y, z); |
| } |
| |
| /** Reverse the elements in {@code arr}. The array is returned. |
| * @param arr the array to reverse |
| * @return the input array with the elements reversed |
| */ |
| private static double[] reverseArray(final double[] arr) { |
| |
| final int len = arr.length; |
| double temp; |
| |
| int i; |
| int j; |
| |
| for (i = 0, j = len - 1; i < len / 2; ++i, --j) { |
| temp = arr[i]; |
| arr[i] = arr[j]; |
| arr[j] = temp; |
| } |
| |
| return arr; |
| } |
| } |
