Quaternion.java
/*
* Copyright (C) 2015 Alberto Irurueta Carro (alberto@irurueta.com)
*
* Licensed 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 com.irurueta.geometry;
import com.irurueta.algebra.ArrayUtils;
import com.irurueta.algebra.Matrix;
import com.irurueta.algebra.WrongSizeException;
import java.io.Serializable;
import java.util.Arrays;
/**
* Contains a representation of a 3D rotation in a more precise and compact way
* than in matrix notation.
* This implementation of a quaternion contains values in the basis 1, i, j, k
* expressed as (a, b, c, d).
* a value is related only to the rotation angle, while b, c, d values are related
* both to the rotation axis and the rotation angle.
*/
@SuppressWarnings("DuplicatedCode")
public class Quaternion extends Rotation3D implements Serializable, Cloneable {
/**
* Number of parameters contained in a quaternion.
*/
public static final int N_PARAMS = 4;
/**
* Number of euler angles.
*/
public static final int N_ANGLES = 3;
/**
* Threshold of axis norm to convert quaternions to axis and rotation angle.
*/
public static final double AXIS_NORM_THRESHOLD = 1e-7;
/**
* Large threshold of axis norm to convert quaternions to axis and rotation
* angle.
*/
public static final double LARGE_AXIS_NORM_THRESHOLD = 1e-6;
/**
* Trace threshold to convert rotation matrices into quaternions.
*/
public static final double TRACE_THRESHOLD = 1e-8;
/**
* Value corresponding to real numbers basis.
*/
private double a;
/**
* Value corresponding to basis i.
*/
private double b;
/**
* Value corresponding to basis j.
*/
private double c;
/**
* Value corresponding to basis k.
*/
private double d;
/**
* Indicates whether quaternion is normalized or not.
*/
private boolean normalized;
/**
* Default constructor.
* Creates a quaternion containing no rotation.
*/
public Quaternion() {
a = 1.0;
}
/**
* Constructor.
*
* @param a value corresponding to real numbers basis.
* @param b value corresponding to basis i.
* @param c value corresponding to basis j.
* @param d value corresponding to basis k.
*/
public Quaternion(final double a, final double b, final double c, final double d) {
this.a = a;
this.b = b;
this.c = c;
this.d = d;
}
/**
* Constructor.
*
* @param quaternion quaternion to be copied from.
*/
public Quaternion(final Quaternion quaternion) {
fromQuaternion(quaternion);
}
/**
* Constructor.
*
* @param values values to be stored in the quaternion expressed in the
* basis (1, i, j, k)
* @throws IllegalArgumentException if provided array does not have length
* 4.
*/
public Quaternion(final double[] values) {
setValues(values);
}
/**
* Constructor.
*
* @param axis a rotation axis.
* @param theta a rotation angle expressed in radians.
* @throws IllegalArgumentException if provided axis array does not have
* length 3.
*/
public Quaternion(final double[] axis, final double theta) {
setFromAxisAndRotation(axis, theta);
}
/**
* Constructor from and axis 3D rotation.
*
* @param axisRotation an axis 3D rotation.
*/
public Quaternion(final AxisRotation3D axisRotation) {
setFromAxisAndRotation(axisRotation);
}
/**
* Constructor from euler angles.
*
* @param roll roll angle expressed in radians.
* @param pitch pitch angle expressed in radians.
* @param yaw yaw angle expressed in radians.
*/
public Quaternion(final double roll, final double pitch, final double yaw) {
setFromEulerAngles(roll, pitch, yaw);
}
/**
* Constructor from matrix rotation.
*
* @param matrixRotation matrix rotation.
*/
public Quaternion(final MatrixRotation3D matrixRotation) {
setFromMatrixRotation(matrixRotation);
}
/**
* Gets value corresponding to real numbers basis.
*
* @return value corresponding to real numbers basis.
*/
public double getA() {
return a;
}
/**
* Sets value corresponding to real numbers basis.
*
* @param a value corresponding to real numbers basis.
*/
public void setA(final double a) {
this.a = a;
normalized = false;
}
/**
* Gets value corresponding to basis i.
*
* @return value corresponding to basis i.
*/
public double getB() {
return b;
}
/**
* Sets value corresponding to basis i.
*
* @param b value corresponding to basis i.
*/
public void setB(final double b) {
this.b = b;
normalized = false;
}
/**
* Gets value corresponding to basis j.
*
* @return value corresponding to basis j.
*/
public double getC() {
return c;
}
/**
* Sets value corresponding to basis j.
*
* @param c value corresponding to basis j.
*/
public void setC(final double c) {
this.c = c;
normalized = false;
}
/**
* Gets value corresponding to basis k.
*
* @return value corresponding to basis k.
*/
public double getD() {
return d;
}
/**
* Sets value corresponding to basis k.
*
* @param d value corresponding to basis k.
*/
public void setD(final double d) {
this.d = d;
normalized = false;
}
/**
* Gets values that parameterize this quaternion.
*
* @return values of this quaternion.
*/
public double[] getValues() {
final var result = new double[N_PARAMS];
values(result);
return result;
}
/**
* Stores values that parameterize this quaternion into provided array.
*
* @param result array where quaternion parameters will be stored.
* @throws IllegalArgumentException if length of provided array is not 4.
*/
public void values(final double[] result) {
if (result.length != N_PARAMS) {
throw new IllegalArgumentException("result length must be 4");
}
result[0] = a;
result[1] = b;
result[2] = c;
result[3] = d;
}
/**
* Sets values that parameterize this quaternion in basis (1, i, j ,k).
*
* @param values values that parameterize this quaternion in basis (1, i, j,
* k).
* @throws IllegalArgumentException if provided array length is not 4.
*/
public final void setValues(final double[] values) {
if (values.length != N_PARAMS) {
throw new IllegalArgumentException("values length must be 4");
}
a = values[0];
b = values[1];
c = values[2];
d = values[3];
normalized = false;
}
/**
* Copies values from provided quaternion into this instance.
*
* @param quaternion quaternion to copy from.
*/
public final void fromQuaternion(final Quaternion quaternion) {
a = quaternion.a;
b = quaternion.b;
c = quaternion.c;
d = quaternion.d;
normalized = quaternion.normalized;
}
/**
* Returns a new quaternion instance containing the same data as this
* instance.
*
* @return a copy of this quaternion instance.
* @throws CloneNotSupportedException if clone fails.
*/
@Override
public Quaternion clone() throws CloneNotSupportedException {
final var result = (Quaternion) super.clone();
copyTo(result);
return result;
}
/**
* Copies this instance data into provided quaternion instance.
*
* @param output destination instance where data is copied to.
*/
public void copyTo(final Quaternion output) {
output.a = a;
output.b = b;
output.c = c;
output.d = d;
output.normalized = normalized;
}
/**
* Sets quaternion parameters from axis and rotation values.
*
* @param axisX x coordinate of rotation axis.
* @param axisY y coordinate of rotation axis.
* @param axisZ z coordinate of rotation axis.
* @param theta rotation angle expressed in radians.
*/
public final void setFromAxisAndRotation(
final double axisX, final double axisY, final double axisZ, final double theta) {
setFromAxisAndRotation(axisX, axisY, axisZ, theta, null, null);
}
/**
* Sets quaternion parameters from axis and rotation values.
*
* @param axisX x coordinate of rotation axis.
* @param axisY y coordinate of rotation axis.
* @param axisZ z coordinate of rotation axis.
* @param theta rotation angle expressed in radians.
* @param jacobianOfTheta if provided, matrix where jacobian of rotation
* angle will be stored. Must be a 4x1 matrix.
* @param jacobianOfAxis if provided, matrix where jacobian of rotation axis
* will be stored. Must be a 4x3 matrix.
* @throws IllegalArgumentException if any of the provided jacobian matrices
* does not have proper size.
* @see <a href="https://github.com/joansola/slamtb">au2q.m at https://github.com/joansola/slamtb</a>
*/
public void setFromAxisAndRotation(
final double axisX, final double axisY, final double axisZ, final double theta,
final Matrix jacobianOfTheta, final Matrix jacobianOfAxis) {
// validations
if (jacobianOfTheta != null && (jacobianOfTheta.getRows() != N_PARAMS || jacobianOfTheta.getColumns() != 1)) {
throw new IllegalArgumentException("jacobian of theta must be 4x1");
}
if (jacobianOfAxis != null && (jacobianOfAxis.getRows() != N_PARAMS
|| jacobianOfAxis.getColumns() != N_ANGLES)) {
throw new IllegalArgumentException("jacobian of axis must be 4x3");
}
final var halfTheta = theta / 2.0;
final var cosine = Math.cos(halfTheta);
final var sine = Math.sin(halfTheta);
a = cosine;
b = axisX * sine;
this.c = axisY * sine;
d = axisZ * sine;
normalized = false;
if (jacobianOfTheta != null) {
final var halfC = cosine / 2.0;
final var halfS = sine / 2.0;
jacobianOfTheta.getBuffer()[0] = -halfS;
jacobianOfTheta.getBuffer()[1] = axisX * halfC;
jacobianOfTheta.getBuffer()[2] = axisY * halfC;
jacobianOfTheta.getBuffer()[3] = axisZ * halfC;
}
if (jacobianOfAxis != null) {
jacobianOfAxis.initialize(0.0);
jacobianOfAxis.setElementAt(1, 0, sine);
jacobianOfAxis.setElementAt(2, 1, sine);
jacobianOfAxis.setElementAt(3, 2, sine);
}
}
/**
* Sets quaternion parameters from axis and rotation values.
*
* @param axis axis values.
* @param theta rotation angle expressed in radians.
* @throws IllegalArgumentException if provided axis array does not have
* length 3.
*/
public final void setFromAxisAndRotation(final double[] axis, final double theta) {
setFromAxisAndRotation(axis, theta, null, null);
}
/**
* Sets quaternion parameters from axis and rotation values.
*
* @param axis axis values.
* @param theta rotation angle expressed in radians.
* @param jacobianOfTheta if provided, matrix where jacobian of rotation
* angle will be stored. Must be a 4x1 matrix.
* @param jacobianOfAxis if provided, matrix where jacobian of rotation axis
* will be stored. Must be a 4x4 matrix.
* @throws IllegalArgumentException if provided axis array does not have
* length 3, or if any of the provided jacobian matrices
* does not have proper size.
*/
public void setFromAxisAndRotation(
final double[] axis, final double theta, final Matrix jacobianOfTheta, final Matrix jacobianOfAxis) {
if (axis.length != AxisRotation3D.AXIS_PARAMS) {
throw new IllegalArgumentException("axis length must be 3");
}
setFromAxisAndRotation(axis[0], axis[1], axis[2], theta, jacobianOfTheta, jacobianOfAxis);
}
/**
* Sets quaternion parameters from an axis 3D rotation.
*
* @param axisRotation an axis 3D rotation.
*/
public final void setFromAxisAndRotation(final AxisRotation3D axisRotation) {
setFromAxisAndRotation(axisRotation, null, null);
}
/**
* Sets quaternion parameters from an axis 3D rotation.
*
* @param axisRotation an axis 3D rotation.
* @param jacobianOfTheta if provided, matrix where jacobian of rotation
* angle will be stored. Must be a 4x1 matrix.
* @param jacobianOfAxis if provided, matrix where jacobian of rotation axis
* will be stored. Must be a 4x4 matrix.
* @throws IllegalArgumentException if any of the provided jacobian matrices
* does not have proper size.
*/
public void setFromAxisAndRotation(
final AxisRotation3D axisRotation, final Matrix jacobianOfTheta, final Matrix jacobianOfAxis) {
final var theta = axisRotation.getRotationAngle();
setFromAxisAndRotation(axisRotation.getAxisX(), axisRotation.getAxisY(), axisRotation.getAxisZ(), theta,
jacobianOfTheta, jacobianOfAxis);
}
/**
* Multiplies this quaternion with provided one and stores the result in
* this instance.
*
* @param q quaternion to multiply with.
*/
public void multiply(final Quaternion q) {
multiply(q, this);
}
/**
* Multiplies this quaternion with provided one and returns the result as a
* new quaternion instance.
*
* @param q quaternion to multiply with.
* @return obtained result.
*/
public Quaternion multiplyAndReturnNew(final Quaternion q) {
final var result = new Quaternion(0.0, 0.0, 0.0, 0.0);
multiply(q, result);
return result;
}
/**
* Multiplies this quaternion with provided one and stores the result into
* provided instance.
*
* @param q quaternion to multiply with.
* @param result instance where result is stored.
*/
public void multiply(final Quaternion q, final Quaternion result) {
product(this, q, result);
}
/**
* Multiplies quaternion q1 with quaternion q2 and stores the result into
* provided instance.
*
* @param q1 1st product operator of quaternions.
* @param q2 2nd product operator of quaternions.
* @param result instance where result of product is stored.
*/
public static void product(final Quaternion q1, final Quaternion q2, final Quaternion result) {
product(q1, q2, result, null, null);
}
/**
* Multiplies quaternion q1 with quaternion q2 and stores the result into
* provided instance. This method also computes the Jacobians wrt of Q1 and
* Q2 if provided.
*
* @param q1 1st product operator of quaternions.
* @param q2 2nd product operator of quaternions.
* @param result instance where result of product is stored.
* @param jacobianQ1 instance where jacobian of q1 is stored.
* @param jacobianQ2 instance where jacobian of q2 is stored.
* @throws IllegalArgumentException if any of the provided jacobian matrices
* is not 4x4.
* @see <a href="https://github.com/joansola/slamtb">qProd.m at https://github.com/joansola/slamtb</a>
*/
public static void product(
final Quaternion q1, final Quaternion q2, final Quaternion result, final Matrix jacobianQ1,
final Matrix jacobianQ2) {
if (jacobianQ1 != null && (jacobianQ1.getRows() != Quaternion.N_PARAMS
|| jacobianQ1.getColumns() != Quaternion.N_PARAMS)) {
throw new IllegalArgumentException("jacobian of q1 must be 4x4");
}
if (jacobianQ2 != null && (jacobianQ2.getRows() != Quaternion.N_PARAMS
|| jacobianQ2.getColumns() != Quaternion.N_PARAMS)) {
throw new IllegalArgumentException("jacobian of q2 must be 4x4");
}
final var q1A = q1.a;
final var q1B = q1.b;
final var q1C = q1.c;
final var q1D = q1.d;
final var q2A = q2.a;
final var q2B = q2.b;
final var q2C = q2.c;
final var q2D = q2.d;
result.a = q1A * q2A - q1B * q2B - q1C * q2C - q1D * q2D;
result.b = q1A * q2B + q1B * q2A + q1C * q2D - q1D * q2C;
result.c = q1A * q2C - q1B * q2D + q1C * q2A + q1D * q2B;
result.d = q1A * q2D + q1B * q2C - q1C * q2B + q1D * q2A;
result.normalized = false;
if (jacobianQ1 != null) {
jacobianQ1.setElementAt(0, 0, q2A);
jacobianQ1.setElementAt(1, 0, q2B);
jacobianQ1.setElementAt(2, 0, q2C);
jacobianQ1.setElementAt(3, 0, q2D);
jacobianQ1.setElementAt(0, 1, -q2B);
jacobianQ1.setElementAt(1, 1, q2A);
jacobianQ1.setElementAt(2, 1, -q2D);
jacobianQ1.setElementAt(3, 1, q2C);
jacobianQ1.setElementAt(0, 2, -q2C);
jacobianQ1.setElementAt(1, 2, q2D);
jacobianQ1.setElementAt(2, 2, q2A);
jacobianQ1.setElementAt(3, 2, -q2B);
jacobianQ1.setElementAt(0, 3, -q2D);
jacobianQ1.setElementAt(1, 3, -q2C);
jacobianQ1.setElementAt(2, 3, q2B);
jacobianQ1.setElementAt(3, 3, q2A);
}
if (jacobianQ2 != null) {
jacobianQ2.setElementAt(0, 0, q1A);
jacobianQ2.setElementAt(1, 0, q1B);
jacobianQ2.setElementAt(2, 0, q1C);
jacobianQ2.setElementAt(3, 0, q1D);
jacobianQ2.setElementAt(0, 1, -q1B);
jacobianQ2.setElementAt(1, 1, q1A);
jacobianQ2.setElementAt(2, 1, q1D);
jacobianQ2.setElementAt(3, 1, -q1C);
jacobianQ2.setElementAt(0, 2, -q1C);
jacobianQ2.setElementAt(1, 2, -q1D);
jacobianQ2.setElementAt(2, 2, q1A);
jacobianQ2.setElementAt(3, 2, q1B);
jacobianQ2.setElementAt(0, 3, -q1D);
jacobianQ2.setElementAt(1, 3, q1C);
jacobianQ2.setElementAt(2, 3, -q1B);
jacobianQ2.setElementAt(3, 3, q1A);
}
}
/**
* Sets quaternion from euler angles (roll, pitch and yaw).
*
* @param roll roll angle expressed in radians. Rotation around x-axis.
* @param pitch pitch angle expressed in radians. Rotation around y-axis.
* @param yaw yaw angle expressed in radians. Rotation around z-axis.
* @param jacobian matrix where jacobian will be stored if provided.
* @throws IllegalArgumentException if provided jacobian matrix does not
* have size 4x3
* @see <a href="https://github.com/joansola/slamtb">e2q.m at https://github.com/joansola/slamtb</a>
*/
public void setFromEulerAngles(
final double roll, final double pitch, final double yaw, final Matrix jacobian) {
if (jacobian != null && (jacobian.getRows() != N_PARAMS || jacobian.getColumns() != N_ANGLES)) {
throw new IllegalArgumentException("jacobian must be 4x3");
}
// roll rotation on X axis
final var qx = new Quaternion(new double[]{1.0, 0.0, 0.0}, roll);
// pitch rotation on Y axis
final var qy = new Quaternion(new double[]{0.0, 1.0, 0.0}, pitch);
// yaw rotation on Z axis (qProd(qProd(qz, qy), qx)
final var qz = new Quaternion(new double[]{0.0, 0.0, 1.0}, yaw);
product(qz, qy, this);
product(this, qx, this);
normalize();
if (jacobian != null) {
final var halfRoll = roll / 2.0;
final var halfPitch = pitch / 2.0;
final var halfYaw = yaw / 2.0;
final var sr = Math.sin(halfRoll);
final var sp = Math.sin(halfPitch);
final var sy = Math.sin(halfYaw);
final var cr = Math.cos(halfRoll);
final var cp = Math.cos(halfPitch);
final var cy = Math.cos(halfYaw);
jacobian.setElementAt(0, 0, 0.5 * (-cy * cp * sr + sy * sp * cr));
jacobian.setElementAt(1, 0, 0.5 * (cy * cp * cr + sy * sp * sr));
jacobian.setElementAt(2, 0, 0.5 * (-cy * sp * sr + sy * cp * cr));
jacobian.setElementAt(3, 0, 0.5 * (-sy * cp * sr - cy * sp * cr));
jacobian.setElementAt(0, 1, 0.5 * (-cy * sp * cr + sy * cp * sr));
jacobian.setElementAt(1, 1, 0.5 * (-cy * sp * sr - sy * cp * cr));
jacobian.setElementAt(2, 1, 0.5 * (cy * cp * cr - sy * sp * sr));
jacobian.setElementAt(3, 1, 0.5 * (-cy * cp * sr - sy * sp * cr));
jacobian.setElementAt(0, 2, 0.5 * (-sy * cp * cr + cy * sp * sr));
jacobian.setElementAt(1, 2, jacobian.getElementAt(3, 0));
jacobian.setElementAt(2, 2, 0.5 * (-sy * sp * cr + cy * cp * sr));
jacobian.setElementAt(3, 2, jacobian.getElementAt(1, 0));
}
}
/**
* Sets quaternion from euler angles (roll, pitch and yaw).
*
* @param roll roll angle expressed in radians. Rotation around x-axis.
* @param pitch pitch angle expressed in radians. Rotation around y-axis.
* @param yaw yaw angle expressed in radians. Rotation around z-axis.
* @see <a href="https://github.com/joansola/slamtb">e2q.m at https://github.com/joansola/slamtb</a>
*/
public final void setFromEulerAngles(final double roll, final double pitch, final double yaw) {
setFromEulerAngles(roll, pitch, yaw, null);
}
/**
* Sets quaternion from euler angles.
*
* @param angles euler angles expressed in radians in the following order:
* roll, pitch and yaw.
* @param jacobian matrix where jacobian will be stored if provided.
* @throws IllegalArgumentException if provided array does not have length
* 3.
*/
public void setFromEulerAngles(final double[] angles, final Matrix jacobian) {
if (angles.length != N_ANGLES) {
throw new IllegalArgumentException("angles length must be 3");
}
setFromEulerAngles(angles[0], angles[1], angles[2], jacobian);
}
/**
* Sets quaternion from euler angles (roll, pitch and yaw).
*
* @param angles euler angles expressed in radians in the following order:
* roll, pitch and yaw.
* @throws IllegalArgumentException if provided array does not have length
* 3.
*/
public void setFromEulerAngles(final double[] angles) {
setFromEulerAngles(angles, null);
}
/**
* Computes the rotation matrix body-to-world corresponding to the body
* orientation given by the Euler angles (roll, pitch, yaw).
*
* @param roll roll angle expressed in radians. Rotation around x-axis.
* @param pitch pitch angle expressed in radians. Rotation around y-axis.
* @param yaw yaw angle expressed in radians. Rotation around z-axis.
* @param result instance where computed rotation will be stored.
* @param jacobian jacobian of computed rotation (optional).
* @throws IllegalArgumentException if provided jacobian is not 9x3.
* @see <a href="https://github.com/joansola/slamtb">e2R.m at https://github.com/joansola/slamtb</a>
*/
public static void eulerToMatrixRotation(
final double roll, final double pitch, final double yaw, final MatrixRotation3D result,
final Matrix jacobian) {
if (jacobian != null && (jacobian.getRows() != 3 * N_ANGLES || jacobian.getColumns() != N_ANGLES)) {
throw new IllegalArgumentException("jacobian must be 9x3");
}
result.setRollPitchYaw(roll, pitch, yaw);
if (jacobian != null) {
final var sr = Math.sin(roll);
final var cr = Math.cos(roll);
final var sp = Math.sin(pitch);
final var cp = Math.cos(pitch);
final var sy = Math.sin(yaw);
final var cy = Math.cos(yaw);
final var tmp1 = sr * sy + cr * sp * cy;
final var tmp2 = -cr * cy - sr * sp * sy;
jacobian.setElementAt(0, 0, 0.0);
jacobian.setElementAt(1, 0, 0.0);
jacobian.setElementAt(2, 0, 0.0);
jacobian.setElementAt(3, 0, tmp1);
jacobian.setElementAt(4, 0, -sr * cy + cr * sp * sy);
jacobian.setElementAt(5, 0, cr * cp);
jacobian.setElementAt(6, 0, cr * sy - sr * sp * cy);
jacobian.setElementAt(7, 0, tmp2);
jacobian.setElementAt(8, 0, -sr * cp);
jacobian.setElementAt(0, 1, -sp * cy);
jacobian.setElementAt(1, 1, -sp * sy);
jacobian.setElementAt(2, 1, -cp);
jacobian.setElementAt(3, 1, sr * cp * cy);
jacobian.setElementAt(4, 1, sr * cp * sy);
jacobian.setElementAt(5, 1, -sr * sp);
jacobian.setElementAt(6, 1, cr * cp * cy);
jacobian.setElementAt(7, 1, cr * cp * sy);
jacobian.setElementAt(8, 1, -cr * sp);
jacobian.setElementAt(0, 2, -cp * sy);
jacobian.setElementAt(1, 2, cp * cy);
jacobian.setElementAt(2, 2, 0.0);
jacobian.setElementAt(3, 2, tmp2);
jacobian.setElementAt(4, 2, -cr * sy + sr * sp * cy);
jacobian.setElementAt(5, 2, 0.0);
jacobian.setElementAt(6, 2, sr * cy - cr * sp * sy);
jacobian.setElementAt(7, 2, tmp1);
jacobian.setElementAt(8, 2, 0.0);
}
}
/**
* Computes the rotation matrix body-to-world corresponding to the body
* orientation given by the Euler angles (roll, pitch, yaw).
*
* @param roll roll angle expressed in radians. Rotation around x-axis.
* @param pitch pitch angle expressed in radians. Rotation around y-axis.
* @param yaw yaw angle expressed in radians. Rotation around z-axis.
* @param result instance where computed rotation will be stored.
* @see <a href="https://github.com/joansola/slamtb">e2R.m at https://github.com/joansola/slamtb</a>
*/
public static void eulerToMatrixRotation(
final double roll, final double pitch, final double yaw, final MatrixRotation3D result) {
eulerToMatrixRotation(roll, pitch, yaw, result, null);
}
/**
* Computes the rotation matrix body-to-world corresponding to the body
* orientation given by the Euler angles (roll, pitch, yaw).
*
* @param angles array containing roll, pitch and yaw angles.
* @param result instance where computed rotation will be stored.
* @param jacobian jacobian of computed rotation (optional).
* @throws IllegalArgumentException if provided angles length is not 3, or
* if provided jacobian is not 9x3.
* @see <a href="https://github.com/joansola/slamtb">e2R.m at https://github.com/joansola/slamtb</a>
*/
public static void eulerToMatrixRotation(
final double[] angles, final MatrixRotation3D result, final Matrix jacobian) {
if (angles.length != N_ANGLES) {
throw new IllegalArgumentException("angles must have length 3");
}
eulerToMatrixRotation(angles[0], angles[1], angles[2], result, jacobian);
}
/**
* Computes the rotation matrix body-to-world corresponding to the body
* orientation given by the Euler angles (roll, pitch, yaw).
*
* @param angles array containing roll, pitch and yaw angles.
* @param result instance where computed rotation will be stored.
* @throws IllegalArgumentException if provided angles length is not 3.
* @see <a href="https://github.com/joansola/slamtb">e2R.m at https://github.com/joansola/slamtb</a>
*/
public static void eulerToMatrixRotation(final double[] angles, final MatrixRotation3D result) {
eulerToMatrixRotation(angles, result, null);
}
/**
* Computes rotation angle and axis of this instance.
*
* @param axis array where normalized rotation axis will be stored.
* @param jacobianAngle matrix where jacobian of angle will be stored, if
* provided. Must be 1x4.
* @param jacobianAxis matrix where jacobian of axis will be stored, if
* provided. Must be 3x4.
* @return rotation angle expressed in radians.
* @throws IllegalArgumentException if length of axis or size of provided
* jacobians is not correct.
* @see <a href="https://github.com/joansola/slamtb">q2au.m at https://github.com/joansola/slamtb</a>
*/
public double toAxisAndRotationAngle(
final double[] axis, final Matrix jacobianAngle, final Matrix jacobianAxis) {
if (axis.length != AxisRotation3D.AXIS_PARAMS) {
throw new IllegalArgumentException("axis length must be 3");
}
if (jacobianAngle != null && (jacobianAngle.getRows() != 1 || jacobianAngle.getColumns() != N_PARAMS)) {
throw new IllegalArgumentException("jacobian of angle must be 1x4");
}
if (jacobianAxis != null && (jacobianAxis.getRows() != AxisRotation3D.AXIS_PARAMS
|| jacobianAxis.getColumns() != N_PARAMS)) {
throw new IllegalArgumentException("jacobian of axis must be 3x4");
}
// non-normalized rotation axis
final var v = new double[]{b, c, d};
// norm of rotation axis
final var n = com.irurueta.algebra.Utils.normF(v);
// normalized rotation axis
if (n > 0.0) {
ArrayUtils.multiplyByScalar(v, 1.0 / n, axis);
} else {
axis[0] = axis[1] = 0.0;
axis[2] = 1.0;
}
// scalar part
final var s = a;
final var aValue = 2.0 * Math.atan2(n, s);
if (jacobianAngle != null) {
if (n > AXIS_NORM_THRESHOLD) {
final var denom = n * n + s * s;
final var aN = 2.0 * s / denom;
final var aS = -2.0 * n / denom;
final var aV = ArrayUtils.multiplyByScalarAndReturnNew(axis, aN);
jacobianAngle.setElementAtIndex(0, aS);
jacobianAngle.setElementAtIndex(1, aV[0]);
jacobianAngle.setElementAtIndex(2, aV[1]);
jacobianAngle.setElementAtIndex(3, aV[2]);
} else {
jacobianAngle.initialize(0.0);
}
}
if (jacobianAxis != null) {
jacobianAxis.initialize(0.0);
try {
if (n > AXIS_NORM_THRESHOLD) {
// uV = (eye(3)*n - v * axis') / n^2
final var uV = Matrix.identity(AxisRotation3D.AXIS_PARAMS, AxisRotation3D.AXIS_PARAMS);
uV.multiplyByScalar(n);
uV.subtract(Matrix.newFromArray(v, true).multiplyAndReturnNew(
Matrix.newFromArray(axis, false)));
uV.multiplyByScalar(1.0 / (n * n));
// uQ = [zeros(3, 1) uV]
jacobianAxis.setSubmatrix(0, 1, 2, 3, uV);
} else {
// 2*eye(3)
final var m = Matrix.identity(AxisRotation3D.AXIS_PARAMS, AxisRotation3D.AXIS_PARAMS);
m.multiplyByScalar(2.0);
// uQ = [zeros(3,1) 2*eye(3)]
jacobianAxis.setSubmatrix(0, 1, 2, 3, m);
}
} catch (final WrongSizeException ignore) {
// never thrown
}
}
return aValue;
}
/**
* Computes rotation angle and axis.
*
* @param axis normalized rotation axis.
* @return rotation angle expressed in radians.
* @throws IllegalArgumentException if length of axis is not 3.
*/
public double toAxisAndRotationAngle(final double[] axis) {
return toAxisAndRotationAngle(axis, null, null);
}
/**
* Converts this quaternion into an axis 3D rotation and stores the result
* into provided rotation instance.
*
* @param result rotation instance where result will be stored.
*/
@Override
public void toAxisRotation(final AxisRotation3D result) {
final var axis = new double[AxisRotation3D.AXIS_PARAMS];
final var theta = toAxisAndRotationAngle(axis, null, null);
result.setAxisAndRotation(axis, theta);
}
/**
* Converts this quaternion into an axis 3D rotation.
*
* @return a new axis 3D rotation equivalent to this quaternion.
*/
@Override
public AxisRotation3D toAxisRotation() {
final var result = new AxisRotation3D();
toAxisRotation(result);
return result;
}
/**
* Computes rotation vector, which is equivalent to the rotation axis but
* having a norm equal to the rotation angle.
*
* @param result array where rotation vector is stored.
* @param jacobian matrix where jacobian of vector will be stored, if
* provided.
* @throws IllegalArgumentException if length of result is not 3 or size of
* provided jacobian is not 3x4.
* @see <a href="https://github.com/joansola/slamtb">q2v.m at https://github.com/joansola/slamtb</a>
*/
public void toRotationVector(final double[] result, final Matrix jacobian) {
if (result.length != AxisRotation3D.AXIS_PARAMS) {
throw new IllegalArgumentException("result length must be 3");
}
if (jacobian != null && (jacobian.getRows() != AxisRotation3D.AXIS_PARAMS
|| jacobian.getColumns() != N_PARAMS)) {
throw new IllegalArgumentException("jacobian must be 3x4");
}
if (jacobian == null) {
final var theta = toAxisAndRotationAngle(result, null, null);
ArrayUtils.multiplyByScalar(result, theta, result);
} else {
try {
final var jacobianAngle = new Matrix(1, N_PARAMS);
final var jacobianAxis = new Matrix(AxisRotation3D.AXIS_PARAMS, N_PARAMS);
final var axis = new double[AxisRotation3D.AXIS_PARAMS];
final var theta = toAxisAndRotationAngle(axis, jacobianAngle, jacobianAxis);
ArrayUtils.multiplyByScalar(axis, theta, result);
final var vA = Matrix.newFromArray(axis, true);
final var vU = Matrix.diagonal(new double[]{theta, theta, theta});
if (theta > AXIS_NORM_THRESHOLD) {
// vA * jacobianAngle + vU * jacobianAxis //3x1 * 1x4 + 3x3 * 3x4
// vA * jacobianAngle
vA.multiply(jacobianAngle);
// vU * jacobianAxis
vU.multiply(jacobianAxis);
// vA * jacobianAngle + vU * jacobianAxis
vA.add(vU);
jacobian.copyFrom(vA);
} else {
// 2*eye(3)
final var m = Matrix.identity(AxisRotation3D.AXIS_PARAMS,
AxisRotation3D.AXIS_PARAMS);
m.multiplyByScalar(2.0);
// uQ = [zeros(3,1) 2*eye(3)]
jacobian.setSubmatrix(0, 1, 2, 3, m);
}
} catch (final WrongSizeException ignore) {
// never thrown
}
}
}
/**
* Computes rotation vector, which is equivalent to the rotation axis but
* having a norm equal to the rotation angle.
*
* @param result array where rotation vector is stored.
* @throws IllegalArgumentException if length of result is not 3.
*/
public void toRotationVector(final double[] result) {
toRotationVector(result, null);
}
/**
* Computes the euler angles (roll, pitch, yaw) equivalent to this
* quaternion rotation and stores the result into provided array.
* If provided, this method also computes the jacobian matrix.
*
* @param angles euler angles (roll, pitch, yaw).
* @param jacobian matrix where jacobian is stored, if provided.
* @throws IllegalArgumentException if provided angles array length is not 3
* or if provided jacobian matrix is not 3x4.
* @see <a href="https://github.com/joansola/slamtb">q2e.m at https://github.com/joansola/slamtb</a>
*/
public void toEulerAngles(final double[] angles, final Matrix jacobian) {
if (angles.length != N_ANGLES) {
throw new IllegalArgumentException("angles length must be 3");
}
if (jacobian != null && (jacobian.getRows() != N_ANGLES || jacobian.getColumns() != N_PARAMS)) {
throw new IllegalArgumentException("jacobian must be 3x4");
}
final var y1 = 2.0 * c * d + 2.0 * a * b;
final var x1 = a * a - b * b - c * c + d * d;
final var z2 = -2.0 * b * d + 2.0 * a * c;
final var y3 = 2.0 * b * c + 2.0 * a * d;
final var x3 = a * a + b * b - c * c - d * d;
// roll
angles[0] = Math.atan2(y1, x1);
// pitch
angles[1] = Math.asin(z2);
// yaw
angles[2] = Math.atan2(y3, x3);
if (jacobian != null) {
final var dx1dq = new double[]{2 * a, -2 * b, -2 * c, 2 * d};
final var dy1dq = new double[]{2 * b, 2 * a, 2 * d, 2 * c};
final var dz2dq = new double[]{2 * c, -2 * d, 2 * a, -2 * b};
final var dx3dq = new double[]{2 * a, 2 * b, -2 * c, -2 * d};
final var dy3dq = new double[]{2 * d, 2 * c, 2 * b, 2 * a};
final var de1dx1 = -y1 / (x1 * x1 + y1 * y1);
final var de1dy1 = x1 / (x1 * x1 + y1 * y1);
final var de2dz2 = 1 / Math.sqrt(1 - z2 * z2);
final var de3dx3 = -y3 / (x3 * x3 + y3 * y3);
final var de3dy3 = x3 / (x3 * x3 + y3 * y3);
// de1dq = de1dx1 * dx1dq + de1dy * dy1dq
ArrayUtils.multiplyByScalar(dx1dq, de1dx1, dx1dq);
ArrayUtils.multiplyByScalar(dy1dq, de1dy1, dy1dq);
final var de1dq = ArrayUtils.sumAndReturnNew(dx1dq, dy1dq);
// de2dq = de2dz2 * dz2dq
final var de2dq = ArrayUtils.multiplyByScalarAndReturnNew(dz2dq, de2dz2);
// de3dq = de3dx3 * dx3dq + de3dy3 * dy3dq
ArrayUtils.multiplyByScalar(dx3dq, de3dx3, dx3dq);
ArrayUtils.multiplyByScalar(dy3dq, de3dy3, dy3dq);
final var de3dq = ArrayUtils.sumAndReturnNew(dx3dq, dy3dq);
jacobian.setSubmatrix(0, 0, 0, N_PARAMS - 1, de1dq);
jacobian.setSubmatrix(1, 0, 1, N_PARAMS - 1, de2dq);
jacobian.setSubmatrix(2, 0, 2, N_PARAMS - 1, de3dq);
}
}
/**
* Computes the euler angles (roll, pitch, yaw) equivalent to this
* quaternion rotation and stores the result into provided array.
*
* @param angles euler angles (roll, pitch, yaw).
* @throws IllegalArgumentException if provided angles array length is not
* 3.
*/
public void toEulerAngles(final double[] angles) {
toEulerAngles(angles, null);
}
/**
* Computes the euler angles (roll, pitch, yaw) resulting in an equivalent
* rotation to this quaternion.
*
* @return euler angles (roll, pitch, yaw)
*/
public double[] toEulerAngles() {
final var result = new double[N_ANGLES];
toEulerAngles(result, null);
return result;
}
/**
* Converts this quaternion into a quaternion matrix so that the quaternion
* product q1 x q2 is equivalent to the matrix product:
* q1.toQuaternionMatrix().multiplyAndReturnNew(q2.toQuaternionMatrix())
*
* @param result matrix where result will be stored.
* @throws IllegalArgumentException if provided matrix is not 4x4.
* @see <a href="https://github.com/joansola/slamtb">q2Q.m at https://github.com/joansola/slamtb</a>
*/
public void quaternionMatrix(final Matrix result) {
if (result.getRows() != N_PARAMS || result.getColumns() != N_PARAMS) {
throw new IllegalArgumentException("matrix must be 4x4");
}
result.setElementAt(0, 0, a);
result.setElementAt(1, 0, b);
result.setElementAt(2, 0, c);
result.setElementAt(3, 0, d);
result.setElementAt(0, 1, -b);
result.setElementAt(1, 1, a);
result.setElementAt(2, 1, d);
result.setElementAt(3, 1, -c);
result.setElementAt(0, 2, -c);
result.setElementAt(1, 2, -d);
result.setElementAt(2, 2, a);
result.setElementAt(3, 2, b);
result.setElementAt(0, 3, -d);
result.setElementAt(1, 3, c);
result.setElementAt(2, 3, -b);
result.setElementAt(3, 3, a);
}
/**
* Converts this quaternion into a quaternion matrix so that quaternion
* product q1 x q2 is equivalent to the matrix product:
* q1.toQuaternionMatrix().multiplyAndReturnNew(q2.toQuaternionMatrix())
*
* @return the quaternion matrix.
* @see <a href="https://github.com/joansola/slamtb">q2Q.m at https://github.com/joansola/slamtb</a>
*/
public Matrix toQuaternionMatrix() {
Matrix result = null;
try {
result = new Matrix(N_PARAMS, N_PARAMS);
quaternionMatrix(result);
} catch (final WrongSizeException ignore) {
// never thrown
}
return result;
}
/**
* Computes the conjugate of this quaternion and stores the result into
* provided instance.
*
* @param result instance where result is stored.
* @param jacobian matrix where jacobian is stored.
* @throws IllegalArgumentException if provided jacobian matrix is not 4x4.
* @see <a href="https://github.com/joansola/slamtb">q2qc.m at https://github.com/joansola/slamtb</a>
*/
public void conjugate(final Quaternion result, final Matrix jacobian) {
if (jacobian != null && (jacobian.getRows() != N_PARAMS || jacobian.getColumns() != N_PARAMS)) {
throw new IllegalArgumentException("jacobian must be 4x4");
}
result.a = a;
result.b = -b;
result.c = -c;
result.d = -d;
result.normalized = normalized;
if (jacobian != null) {
jacobian.initialize(0.0);
jacobian.setElementAt(0, 0, 1.0);
for (int i = 1; i < N_PARAMS; i++) {
jacobian.setElementAt(i, i, -1.0);
}
}
}
/**
* Computes the conjugate of this quaternion and stores the result into
* provided instance.
*
* @param result instance where result is stored.
* @see <a href="https://github.com/joansola/slamtb">q2qc.m at https://github.com/joansola/slamtb</a>
*/
public void conjugate(final Quaternion result) {
conjugate(result, null);
}
/**
* Computes the conjugate of this quaternion.
*
* @return conjugate of this quaternion.
* @see <a href="https://github.com/joansola/slamtb">q2qc.m at https://github.com/joansola/slamtb</a>
*/
public Quaternion conjugateAndReturnNew() {
final var q = new Quaternion();
conjugate(q);
return q;
}
/**
* Converts this quaternion into a quaternion matrix so that the quaternion
* product q1 x q2 is equivalent to the matrix product:
* q2.toQuaternionMatrixN().multiplyAndReturnNew(q1.toQuaternionMatrixN()).
* Notice that matrix order in the product is the opposite as the order used
* when multiplying matrices obtained by method #toQuaternionMatrix().
*
* @param result matrix where result will be stored.
* @throws IllegalArgumentException if provided matrix is not 4x4.
* @see <a href="https://github.com/joansola/slamtb">q2Qn.m at https://github.com/joansola/slamtb</a>
*/
public void quaternionMatrixN(final Matrix result) {
if (result.getRows() != N_PARAMS || result.getColumns() != N_PARAMS) {
throw new IllegalArgumentException("matrix must be 4x4");
}
result.setElementAt(0, 0, a);
result.setElementAt(1, 0, b);
result.setElementAt(2, 0, c);
result.setElementAt(3, 0, d);
result.setElementAt(0, 1, -b);
result.setElementAt(1, 1, a);
result.setElementAt(2, 1, -d);
result.setElementAt(3, 1, c);
result.setElementAt(0, 2, -c);
result.setElementAt(1, 2, d);
result.setElementAt(2, 2, a);
result.setElementAt(3, 2, -b);
result.setElementAt(0, 3, -d);
result.setElementAt(1, 3, -c);
result.setElementAt(2, 3, b);
result.setElementAt(3, 3, a);
}
/**
* Converts this quaternion into a quaternion matrix so that quaternion
* product q1 x q2 is equivalent to the matrix product:
* q2.toQuaternionMatrixN().multiplyAndReturnNew(q1.toQuaternionMatrixN()).
* Notice that matrix order in the product is the opposite as the order used
* when multiplying matrices obtained by method #toQuaternionMatrix().
*
* @return the quaternion matrix.
* @see <a href="https://github.com/joansola/slamtb">q2Qn.m at https://github.com/joansola/slamtb</a>
*/
public Matrix toQuaternionMatrixN() {
Matrix result = null;
try {
result = new Matrix(N_PARAMS, N_PARAMS);
quaternionMatrixN(result);
} catch (final WrongSizeException ignore) {
// never thrown
}
return result;
}
/**
* Computes the matrix representing this quaternion rotation.
*
* @param result matrix where rotation data will be stored.
* @param jacobian jacobian wrt of this quaternion.
* @throws IllegalArgumentException if provided result matrix is not 3x3 o
* jacobian matrix is not 9x4.
* @see <a href="https://github.com/joansola/slamtb">q2R.m at https://github.com/joansola/slamtb</a>
*/
public void toMatrixRotation(final Matrix result, final Matrix jacobian) {
if (result.getRows() != MatrixRotation3D.ROTATION3D_INHOM_MATRIX_ROWS
|| result.getColumns() != MatrixRotation3D.ROTATION3D_INHOM_MATRIX_COLS) {
throw new IllegalArgumentException("result matrix is not 3x3");
}
if (jacobian != null && (jacobian.getRows() != 9 || jacobian.getColumns() != 4)) {
throw new IllegalArgumentException("jacobian matrix is not 9x4");
}
final var aa = a * a;
final var ab = 2.0 * a * b;
final var ac = 2.0 * a * c;
final var ad = 2.0 * a * d;
final var bb = b * b;
final var bc = 2.0 * b * c;
final var bd = 2.0 * b * d;
final var cc = c * c;
final var cd = 2.0 * c * d;
final var dd = d * d;
result.setElementAt(0, 0, aa + bb - cc - dd);
result.setElementAt(1, 0, bc + ad);
result.setElementAt(2, 0, bd - ac);
result.setElementAt(0, 1, bc - ad);
result.setElementAt(1, 1, aa - bb + cc - dd);
result.setElementAt(2, 1, cd + ab);
result.setElementAt(0, 2, bd + ac);
result.setElementAt(1, 2, cd - ab);
result.setElementAt(2, 2, aa - bb - cc + dd);
if (jacobian != null) {
final var a2 = 2.0 * a;
final var b2 = 2.0 * b;
final var c2 = 2.0 * c;
final var d2 = 2.0 * d;
jacobian.setElementAt(0, 0, a2);
jacobian.setElementAt(1, 0, d2);
jacobian.setElementAt(2, 0, -c2);
jacobian.setElementAt(3, 0, -d2);
jacobian.setElementAt(4, 0, a2);
jacobian.setElementAt(5, 0, b2);
jacobian.setElementAt(6, 0, c2);
jacobian.setElementAt(7, 0, -b2);
jacobian.setElementAt(8, 0, a2);
jacobian.setElementAt(0, 1, b2);
jacobian.setElementAt(1, 1, c2);
jacobian.setElementAt(2, 1, d2);
jacobian.setElementAt(3, 1, c2);
jacobian.setElementAt(4, 1, -b2);
jacobian.setElementAt(5, 1, a2);
jacobian.setElementAt(6, 1, d2);
jacobian.setElementAt(7, 1, -a2);
jacobian.setElementAt(8, 1, -b2);
jacobian.setElementAt(0, 2, -c2);
jacobian.setElementAt(1, 2, b2);
jacobian.setElementAt(2, 2, -a2);
jacobian.setElementAt(3, 2, b2);
jacobian.setElementAt(4, 2, c2);
jacobian.setElementAt(5, 2, d2);
jacobian.setElementAt(6, 2, a2);
jacobian.setElementAt(7, 2, d2);
jacobian.setElementAt(8, 2, -c2);
jacobian.setElementAt(0, 3, -d2);
jacobian.setElementAt(1, 3, a2);
jacobian.setElementAt(2, 3, b2);
jacobian.setElementAt(3, 3, -a2);
jacobian.setElementAt(4, 3, -d2);
jacobian.setElementAt(5, 3, c2);
jacobian.setElementAt(6, 3, b2);
jacobian.setElementAt(7, 3, c2);
jacobian.setElementAt(8, 3, d2);
}
}
/**
* Computes the matrix representing this quaternion rotation.
*
* @param result matrix where rotation data will be stored.
* @throws IllegalArgumentException if provided result matrix is not 3x3.
* @see <a href="https://github.com/joansola/slamtb">q2R.m at https://github.com/joansola/slamtb</a>
*/
public void toMatrixRotation(final Matrix result) {
toMatrixRotation(result, null);
}
/**
* Converts this quaternion into a 3D matrix rotation.
*
* @param result matrix rotation instance where result will be stored.
* @see <a href="https://github.com/joansola/slamtb">q2R.m at https://github.com/joansola/slamtb</a>
*/
@Override
public void toMatrixRotation(final MatrixRotation3D result) {
toMatrixRotation(result.internalMatrix);
}
/**
* Converts this quaternion into a 3D matrix rotation.
*
* @return a 3D matrix rotation.
* @see <a href="https://github.com/joansola/slamtb">q2R.m at https://github.com/joansola/slamtb</a>
*/
@Override
public MatrixRotation3D toMatrixRotation() {
final var rotation = new MatrixRotation3D();
toMatrixRotation(rotation);
return rotation;
}
/**
* Rotates a 3D point using the origin of coordinates as the axis of
* rotation.
* Point will be rotated by the amount of rotation contained in provided
* quaternion.
*
* @param q a quaternion.
* @param inputPoint input point to be rotated.
* @param resultPoint rotated point.
* @param jacobianPoint jacobian wrt of point.
* @param jacobianQuaternion jacobian wrt of quaternion.
* @throws IllegalArgumentException if jacobian of point is not 3x3 or
* jacobian of quaternion is not 3x4.
* @see <a href="https://github.com/joansola/slamtb">qRot.m at https://github.com/joansola/slamtb</a>
*/
public static void rotate(final Quaternion q, final Point3D inputPoint, final Point3D resultPoint,
final Matrix jacobianPoint, final Matrix jacobianQuaternion) {
if (jacobianPoint != null && (jacobianPoint.getRows() != N_ANGLES || jacobianPoint.getColumns() != N_ANGLES)) {
throw new IllegalArgumentException("jacobian of point must be 3x3");
}
if (jacobianQuaternion != null && (jacobianQuaternion.getRows() != N_ANGLES
|| jacobianQuaternion.getColumns() != N_PARAMS)) {
throw new IllegalArgumentException("jacobian of quaternion must be 3x4");
}
final var v0 = new Quaternion(0.0, inputPoint.getInhomX(), inputPoint.getInhomY(), inputPoint.getInhomZ());
final var tmp = q.multiplyAndReturnNew(v0).multiplyAndReturnNew(q.conjugateAndReturnNew());
resultPoint.setInhomogeneousCoordinates(tmp.getB(), tmp.getC(), tmp.getD());
if (jacobianPoint != null) {
q.toMatrixRotation(jacobianPoint);
}
if (jacobianQuaternion != null) {
final var a = q.a;
final var b = q.b;
final var c = q.c;
final var d = q.d;
final var x = inputPoint.getInhomX();
final var y = inputPoint.getInhomY();
final var z = inputPoint.getInhomZ();
final var axdycz = 2.0 * (a * x - d * y + c * z);
final var bxcydz = 2.0 * (b * x + c * y + d * z);
final var cxbyaz = 2.0 * (c * x - b * y - a * z);
final var dxaybz = 2.0 * (d * x + a * y - b * z);
jacobianQuaternion.setElementAt(0, 0, axdycz);
jacobianQuaternion.setElementAt(1, 0, dxaybz);
jacobianQuaternion.setElementAt(2, 0, -cxbyaz);
jacobianQuaternion.setElementAt(0, 1, bxcydz);
jacobianQuaternion.setElementAt(1, 1, cxbyaz);
jacobianQuaternion.setElementAt(2, 1, dxaybz);
jacobianQuaternion.setElementAt(0, 2, -cxbyaz);
jacobianQuaternion.setElementAt(1, 2, bxcydz);
jacobianQuaternion.setElementAt(2, 2, -axdycz);
jacobianQuaternion.setElementAt(0, 3, -dxaybz);
jacobianQuaternion.setElementAt(1, 3, axdycz);
jacobianQuaternion.setElementAt(2, 3, bxcydz);
}
}
/**
* Rotates a 3D point using the origin of coordinates as the axis of
* rotation.
* Point will be rotated by the amount of rotation contained in this
* quaternion instance.
*
* @param inputPoint input point to be rotated.
* @param resultPoint rotated point.
* @param jacobianPoint jacobian wrt of point.
* @param jacobianQuaternion jacobian wrt of quaternion.
* @throws IllegalArgumentException if jacobian of point is not 3x3 or
* jacobian of quaternion is no 3x4.
* @see <a href="https://github.com/joansola/slamtb">qRot.m at https://github.com/joansola/slamtb</a>
*/
public void rotate(final Point3D inputPoint, final Point3D resultPoint, final Matrix jacobianPoint,
final Matrix jacobianQuaternion) {
rotate(this, inputPoint, resultPoint, jacobianPoint, jacobianQuaternion);
}
/**
* Rotates a 3D point using the origin of coordinates as the axis of
* rotation.
* Point will be rotated by the amount of rotation contained in this
* quaternion instance.
*
* @param inputPoint Input point to be rotated.
* @param resultPoint Rotated point.
* @see <a href="https://github.com/joansola/slamtb">qRot.m at https://github.com/joansola/slamtb</a>
*/
@Override
public void rotate(final Point3D inputPoint, final Point3D resultPoint) {
rotate(inputPoint, resultPoint, null, null);
}
/**
* Returns a 3D point containing a rotated version of provided point.
* Point will be rotated using the origin of the coordinates as the axis of
* rotation.
* Point will be rotated by the amount of rotation contained in this
* quaternion instance.
*
* @param point Point to be rotated.
* @return Rotated point.
* @see <a href="https://github.com/joansola/slamtb">qRot.m at https://github.com/joansola/slamtb</a>
*/
@Override
public Point3D rotate(final Point3D point) {
final var result = new HomogeneousPoint3D();
rotate(point, result);
return result;
}
/**
* Converts rotation matrix into a quaternion.
*
* @param r a rotation matrix to be converted from.
* @param result quaternion where result is stored.
* @throws IllegalArgumentException if provided matrix is not 3x3
* @see <a href="https://github.com/joansola/slamtb">R2q.m at https://github.com/joansola/slamtb</a>
*/
public static void matrixRotationToQuaternion(final Matrix r, final Quaternion result) {
if (r.getRows() != MatrixRotation3D.ROTATION3D_INHOM_MATRIX_ROWS
|| r.getColumns() != MatrixRotation3D.ROTATION3D_INHOM_MATRIX_COLS) {
throw new IllegalArgumentException("rotation matrix must be 3x3");
}
final var trace = com.irurueta.algebra.Utils.trace(r) + 1.0;
double s;
double a;
double b;
double c;
double d;
if (trace > TRACE_THRESHOLD) {
// to avoid large distortions
s = 2.0 * Math.sqrt(trace);
a = 0.25 * s;
b = (r.getElementAt(1, 2) - r.getElementAt(2, 1)) / s;
c = (r.getElementAt(2, 0) - r.getElementAt(0, 2)) / s;
d = (r.getElementAt(0, 1) - r.getElementAt(1, 0)) / s;
} else {
if (r.getElementAt(0, 0) > r.getElementAt(1, 1)
&& r.getElementAt(0, 0) > r.getElementAt(2, 2)) {
// column 1:
// tested with R2 = diag([1 -1 -1])
s = 2.0 * Math.sqrt(1.0 + r.getElementAt(0, 0) - r.getElementAt(1, 1)
- r.getElementAt(2, 2));
a = (r.getElementAt(1, 2) - r.getElementAt(2, 1)) / s;
b = 0.25 * s;
c = (r.getElementAt(0, 1) + r.getElementAt(1, 0)) / s;
d = (r.getElementAt(2, 0) + r.getElementAt(0, 2)) / s;
} else if (r.getElementAt(1, 1) > r.getElementAt(2, 2)) {
// column 2:
// tested with R3 = [0 1 0; 1 0 0; 0 0 -1]
s = 2.0 * Math.sqrt(1.0 + r.getElementAt(1, 1) - r.getElementAt(0, 0)
- r.getElementAt(2, 2));
a = (r.getElementAt(2, 0) - r.getElementAt(0, 2)) / s;
b = (r.getElementAt(0, 1) + r.getElementAt(1, 0)) / s;
c = 0.25 * s;
d = (r.getElementAt(1, 2) + r.getElementAt(2, 1)) / s;
} else {
// column 3:
// tested with R4 = [-1 0 0; 0 0 1; 0 1 0]
s = 2.0 * Math.sqrt(1.0 + r.getElementAt(2, 2)
- r.getElementAt(0, 0) - r.getElementAt(1, 1));
a = (r.getElementAt(0, 1) - r.getElementAt(1, 0)) / s;
b = (r.getElementAt(2, 0) + r.getElementAt(0, 2)) / s;
c = (r.getElementAt(1, 2) + r.getElementAt(2, 1)) / s;
d = 0.25 * s;
}
}
result.a = a;
result.b = -b;
result.c = -c;
result.d = -d;
result.normalized = false;
}
/**
* Converts 3D matrix rotation into a quaternion.
*
* @param rotation a 3D matrix rotation to be converted from.
* @param result quaternion where result is stored.
* @see <a href="https://github.com/joansola/slamtb">R2q.m at https://github.com/joansola/slamtb</a>
*/
public static void matrixRotationToQuaternion(final MatrixRotation3D rotation, final Quaternion result) {
matrixRotationToQuaternion(rotation.internalMatrix, result);
}
/**
* Sets quaternion values associated to provided rotation.
*
* @param matrix a rotation matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @see <a href="https://github.com/joansola/slamtb">R2q.m at https://github.com/joansola/slamtb</a>
*/
public void setFromMatrixRotation(final Matrix matrix) {
matrixRotationToQuaternion(matrix, this);
}
/**
* Sets quaternion values associated to provided rotation.
*
* @param rotation a rotation to be converted into a quaternion.
* @see <a href="https://github.com/joansola/slamtb">R2q.m at https://github.com/joansola/slamtb</a>
*/
public final void setFromMatrixRotation(final MatrixRotation3D rotation) {
matrixRotationToQuaternion(rotation, this);
}
/**
* Converts a rotation vector (rotation axis having a norm equal to the
* rotation angle) into a normalized rotation axis and its corresponding
* rotation angle.
*
* @param rotationVector input rotation vector to be converted.
* @param axis obtained normalized rotation axis.
* @param jacobianAlpha jacobian wrt of angle.
* @param jacobianRotationVector jacobian wrt of rotation vector.
* @return rotation angle.
* @throws IllegalArgumentException if provided rotation vector length is
* not 3, jacobian of angle is not 1x3 or jacobian of rotation vector is
* not 3x3.
* @see <a href="https://github.com/joansola/slamtb">v2au.m at https://github.com/joansola/slamtb</a>
*/
public static double rotationVectorToRotationAxisAndAngle(
final double[] rotationVector, final double[] axis, final Matrix jacobianAlpha,
final Matrix jacobianRotationVector) {
if (rotationVector.length != AxisRotation3D.AXIS_PARAMS) {
throw new IllegalArgumentException("rotation vector length must be 3");
}
if (jacobianAlpha != null && (jacobianAlpha.getRows() != 1 || jacobianAlpha.getColumns() != N_ANGLES)) {
throw new IllegalArgumentException("jacobian alpha must be 1x3");
}
if (jacobianRotationVector != null && (jacobianRotationVector.getRows() != N_ANGLES
|| jacobianRotationVector.getColumns() != N_ANGLES)) {
throw new IllegalArgumentException("jacobian rotation vector must be 3x3");
}
var alpha = com.irurueta.algebra.Utils.normF(rotationVector);
if (alpha > AXIS_NORM_THRESHOLD) {
ArrayUtils.multiplyByScalar(rotationVector, 1.0 / alpha, axis);
if (jacobianAlpha != null) {
jacobianAlpha.setSubmatrix(0, 0, 0,
axis.length - 1, axis);
}
if (jacobianRotationVector != null) {
jacobianRotationVector.setElementAt(0, 0, 1.0 / alpha - axis[0] * axis[0] / alpha);
jacobianRotationVector.setElementAt(1, 0, -axis[0] / alpha * axis[1]);
jacobianRotationVector.setElementAt(2, 0, -axis[0] / alpha * axis[2]);
jacobianRotationVector.setElementAt(0, 1, -axis[0] / alpha * axis[1]);
jacobianRotationVector.setElementAt(1, 1, 1.0 / alpha - axis[1] * axis[1] / alpha);
jacobianRotationVector.setElementAt(2, 1, -axis[1] / alpha * axis[2]);
jacobianRotationVector.setElementAt(0, 2, -axis[0] / alpha * axis[2]);
jacobianRotationVector.setElementAt(1, 2, -axis[1] / alpha * axis[2]);
jacobianRotationVector.setElementAt(2, 2, 1.0 / alpha - axis[2] * axis[2] / alpha);
}
} else {
alpha = 0.0;
Arrays.fill(axis, 0.0);
if (jacobianAlpha != null) {
jacobianAlpha.initialize(0.0);
}
if (jacobianRotationVector != null) {
jacobianRotationVector.initialize(0.0);
}
}
return alpha;
}
/**
* Converts a rotation vector (rotation axis having a norm equal to the
* rotation angle) into a normalized rotation axis and its corresponding
* rotation angle.
*
* @param rotationVector input rotation vector to be converted.
* @param axis obtained normalized rotation axis.
* @return rotation angle.
* @throws IllegalArgumentException if provided rotation vector length is
* not 3.
* @see <a href="https://github.com/joansola/slamtb">v2au.m at https://github.com/joansola/slamtb</a>
*/
public static double rotationVectorToRotationAxisAndAngle(final double[] rotationVector, final double[] axis) {
return rotationVectorToRotationAxisAndAngle(rotationVector, axis, null, null);
}
/**
* Converts a rotation vector (rotation axis having a norm equal to the
* rotation angle) into a quaternion, and stores the corresponding jacobian
* of the quaternion respect to the vector if provided.
*
* @param rotationVector input rotation vector to be converted.
* @param result quaternion where result will be stored.
* @param jacobian if provided, matrix where jacobian of the quaternion
* respect to the vector will be stored. Must be 4x3.
* @throws IllegalArgumentException if provided rotation vector is not
* length 3 or if provided jacobian matrix is not 4x3.
* @see <a href="https://github.com/joansola/slamtb">v2q.m at https://github.com/joansola/slamtb</a>
*/
public static void rotationVectorToQuaternion(
final double[] rotationVector, final Quaternion result, final Matrix jacobian) {
if (rotationVector.length != AxisRotation3D.AXIS_PARAMS) {
throw new IllegalArgumentException("rotation vector length must be 3");
}
if (jacobian != null && (jacobian.getRows() != N_PARAMS || jacobian.getColumns() != N_ANGLES)) {
throw new IllegalArgumentException("jacobian must be 4x3");
}
final var axis = new double[AxisRotation3D.AXIS_PARAMS];
if (jacobian == null) {
final var alpha = rotationVectorToRotationAxisAndAngle(rotationVector, axis);
result.setFromAxisAndRotation(axis, alpha);
} else {
var alpha = com.irurueta.algebra.Utils.normF(rotationVector);
if (alpha < LARGE_AXIS_NORM_THRESHOLD) {
// use small signal approximation
result.a = 1 - alpha * alpha / 8.0;
result.b = rotationVector[0] / 2.0;
result.c = rotationVector[1] / 2.0;
result.d = rotationVector[2] / 2.0;
result.normalized = false;
jacobian.setElementAt(0, 0, -0.25 * rotationVector[0]);
jacobian.setElementAt(0, 1, -0.25 * rotationVector[1]);
jacobian.setElementAt(0, 2, -0.25 * rotationVector[2]);
jacobian.setElementAt(1, 0, 0.5);
jacobian.setElementAt(1, 1, 0.0);
jacobian.setElementAt(1, 2, 0.0);
jacobian.setElementAt(2, 0, 0.0);
jacobian.setElementAt(2, 1, 0.5);
jacobian.setElementAt(2, 2, 0.0);
jacobian.setElementAt(3, 0, 0.0);
jacobian.setElementAt(3, 1, 0.0);
jacobian.setElementAt(3, 2, 0.5);
} else {
try {
// Av
final var jacobianAlpha = new Matrix(1, N_ANGLES);
// Uv
final var jacobianRotationVector = new Matrix(N_ANGLES, N_ANGLES);
alpha = rotationVectorToRotationAxisAndAngle(rotationVector, axis, jacobianAlpha,
jacobianRotationVector);
// Qa
final var jacobianOfTheta = new Matrix(N_PARAMS, 1);
// Qu
final var jacobianOfAxis = new Matrix(N_PARAMS, N_ANGLES);
result.setFromAxisAndRotation(axis, alpha, jacobianOfTheta, jacobianOfAxis);
// Qv = Qa * Av + Qu * Uv
// Qa * Av
jacobianOfTheta.multiply(jacobianAlpha);
// Qu * Uv
jacobianOfAxis.multiply(jacobianRotationVector);
jacobian.copyFrom(jacobianOfTheta);
jacobian.add(jacobianOfAxis);
} catch (final WrongSizeException e) {
throw new IllegalArgumentException(e);
}
}
}
}
/**
* Converts a rotation vector (rotation axis having a norm equal to the
* rotation angle) into a quaternion.
*
* @param rotationVector input rotation vector to be converted.
* @param result quaternion where result will be stored.
* @throws IllegalArgumentException if provided rotation vector is not
* length 3.
* @see <a href="https://github.com/joansola/slamtb">v2q.m at https://github.com/joansola/slamtb</a>
*/
public static void rotationVectorToQuaternion(final double[] rotationVector, final Quaternion result) {
rotationVectorToQuaternion(rotationVector, result, null);
}
/**
* Sets values of this quaternion from provided rotation vector.
* A rotation vector is a rotation axis having a norm equal to the rotation
* angle.
*
* @param rotationVector input rotation vector to obtain quaternion values
* from.
* @throws IllegalArgumentException if provided rotation vector does not
* have length 3.
* @see <a href="https://github.com/joansola/slamtb">v2q.m at https://github.com/joansola/slamtb</a>
*/
public void setFromRotationVector(final double[] rotationVector) {
rotationVectorToQuaternion(rotationVector, this);
}
/**
* Converts a rotation vector into a rotation matrix.
* A rotation vector is a rotation axis having a norm equal to the rotation
* angle.
*
* @param rotationVector a rotation vector to be converted into a 3D matrix
* rotation.
* @param result matrix where result is stored.
* @throws IllegalArgumentException if provided rotation vector does not
* have length 3.
* @see <a href="https://github.com/joansola/slamtb">v2R.m at https://github.com/joansola/slamtb</a>
*/
public static void rotationVectorToMatrixRotation(final double[] rotationVector, final Matrix result) {
final var axis = new double[AxisRotation3D.AXIS_PARAMS];
final var alpha = rotationVectorToRotationAxisAndAngle(rotationVector, axis);
final var r = new AxisRotation3D(axis, alpha);
r.asInhomogeneousMatrix(result);
}
/**
* Converts a rotation vector into a rotation matrix.
* A rotation vector is a rotation axis having a norm equal to the rotation
* angle.
*
* @param rotationVector a rotation vector to be converted into a 3D matrix
* rotation.
* @param result 3D matrix rotation where result is stored.
* @throws IllegalArgumentException if provided rotation vector does not
* have length 3.
* @see <a href="https://github.com/joansola/slamtb">v2R.m at https://github.com/joansola/slamtb</a>
*/
public static void rotationVectorToMatrixRotation(final double[] rotationVector, final MatrixRotation3D result) {
rotationVectorToMatrixRotation(rotationVector, result.internalMatrix);
}
/**
* Returns type of this rotation.
*
* @return Type of this rotation.
*/
@Override
public Rotation3DType getType() {
return Rotation3DType.QUATERNION;
}
/**
* Sets the axis and rotation of this instance.
* Once set, points will rotate around provided axis an amount equal to
* provided rotation angle in radians.
* Note: to avoid numerical instabilities and improve accuracy, axis
* coordinates should be normalized (e.g. norm equal to 1).
*
* @param axisX X coordinate of rotation axis.
* @param axisY Y coordinate of rotation axis.
* @param axisZ Z coordinate of rotation axis.
* @param theta Amount of rotation in radians.
*/
@Override
public void setAxisAndRotation(
final double axisX, final double axisY, final double axisZ, final double theta) {
setFromAxisAndRotation(axisX, axisY, axisZ, theta);
}
/**
* Returns rotation axis corresponding to this instance.
* Result is stored in provided axis array, which must have length 3.
*
* @param axis Array where axis coordinates will be stored.
* @throws IllegalArgumentException Raised if provided array does not have
* length 3.
*/
@Override
public void rotationAxis(final double[] axis) {
toAxisAndRotationAngle(axis);
}
/**
* Returns rotation amount or angle in radians around the rotation axis
* associated to this instance.
*
* @return Rotation angle in radians.
*/
@Override
public double getRotationAngle() {
// norm of rotation axis
final var n = Math.sqrt(b * b + c * c + d * d);
return 2.0 * Math.atan2(n, a);
}
/**
* Returns this 3D rotation instance expressed as a 3x3 inhomogeneous
* matrix.
* This is equivalent to call getInternalMatrix().
*
* @return Rotation matrix expressed in inhomogeneous coordinates.
*/
@Override
public Matrix asInhomogeneousMatrix() {
Matrix m = null;
try {
m = new Matrix(MatrixRotation3D.ROTATION3D_INHOM_MATRIX_ROWS,
MatrixRotation3D.ROTATION3D_INHOM_MATRIX_COLS);
toMatrixRotation(m);
} catch (final WrongSizeException ignore) {
// never thrown
}
return m;
}
/**
* Sets into provided Matrix instance this 3D rotation expressed as a
* 3x3 inhomogeneous matrix.
*
* @param result Matrix where rotation will be set.
* @throws IllegalArgumentException Raised if provided instance does not
* have size 3x3.
*/
@Override
public void asInhomogeneousMatrix(final Matrix result) {
toMatrixRotation(result);
}
/**
* Returns this 3D rotation instance expressed as a 4x4 homogeneous matrix.
*
* @return Rotation matrix expressed in homogeneous coordinates.
*/
@Override
public Matrix asHomogeneousMatrix() {
Matrix m = null;
try {
m = new Matrix(HOM_COORDS, HOM_COORDS);
asHomogeneousMatrix(m);
} catch (final WrongSizeException ignore) {
// never thrown
}
return m;
}
/**
* Sets into provided Matrix instance this 3D rotation expressed as a
* 4x4 homogeneous matrix.
*
* @param result Matrix where rotation will be set.
* @throws IllegalArgumentException Raised if provided instance does not
* have size 4x4.
*/
@Override
public void asHomogeneousMatrix(final Matrix result) {
result.initialize(0.0);
result.setElementAt(HOM_COORDS - 1, HOM_COORDS - 1, 1.0);
result.setSubmatrix(0, 0, INHOM_COORDS - 1,
INHOM_COORDS - 1, asInhomogeneousMatrix());
}
/**
* Sets amount of rotation from provided inhomogeneous rotation matrix.
* Provided matrix must be orthogonal (i.e. squared, non-singular, it's
* transpose must be its inverse) and must have determinant equal to 1.
* Provided matrix must also have size 3x3.
*
* @param m Provided rotation matrix.
* @param threshold Threshold to determine whether matrix is orthonormal.
* @throws IllegalArgumentException Raised if provided threshold is
* negative.
* {@link #isValidRotationMatrix(Matrix)}
*/
@Override
public void fromInhomogeneousMatrix(final Matrix m, final double threshold) {
setFromMatrixRotation(m);
}
/**
* Sets amount of rotation from provided homogeneous rotation matrix.
* Provided matrix must be orthogonal (i.e. squared, non-singular, it's
* transpose must be its inverse) and must have determinant equal to 1.
* Provided matrix must also have size 4x4, and its last row and column must
* be zero, except for element in last row and column which must be 1.
*
* @param m Provided rotation matrix.
* @param threshold Threshold to determine whether matrix is orthonormal.
* @throws IllegalArgumentException Raised if provided threshold is
* negative.
* {@link #isValidRotationMatrix(Matrix)}
*/
@Override
public void fromHomogeneousMatrix(final Matrix m, final double threshold) {
setFromMatrixRotation(m.getSubmatrix(0, 0, INHOM_COORDS - 1,
INHOM_COORDS - 1));
}
/**
* Inverts a quaternion so that q * q^-1 = 1.
*
* @param q quaternion to be inverted.
* @param result quaternion instance where the result will be stored.
*/
public static void inverse(final Quaternion q, final Quaternion result) {
// the inverse is the conjugate divided by the quaternion square norm
final var sqrNorm = q.a * q.a + q.b * q.b + q.c * q.c + q.d * q.d;
q.conjugate(result);
result.a /= sqrNorm;
result.b /= sqrNorm;
result.c /= sqrNorm;
result.d /= sqrNorm;
result.normalized = false;
}
/**
* Inverts a quaternion so that q * q^-1 = 1.
*
* @param q quaternion to be inverted.
* @return a new quaternion containing the inverse.
*/
public static Quaternion inverseAndReturnNew(final Quaternion q) {
final var result = new Quaternion();
inverse(q, result);
return result;
}
/**
* Inverts this quaternion instance so that q * q^-1 = 1.
*
* @param result instance where quaternion inverse is stored.
*/
public void inverse(final Quaternion result) {
inverse(this, result);
}
/**
* Inverts this quaternion instance so that q * q^-1 = 1.
*
* @return a new quaternion containing the inverse of this quaternion.
*/
public Quaternion inverseAndReturnNew() {
final var result = new Quaternion();
inverse(result);
return result;
}
/**
* Inverts this quaternion.
*/
public void inverse() {
inverse(this);
}
/**
* Returns a 3D rotation which is inverse to this instance.
* In other words, the combination of this rotation with its inverse
* produces no change.
*
* @return Inverse 3D rotation.
*/
@Override
public Rotation3D inverseRotationAndReturnNew() {
final var q = new Quaternion();
inverseRotation(q);
return q;
}
/**
* Inverts this quaternion instance so that q * q^-1 = 1.
*
* @param result instance where quaternion inverse is stored.
*/
public void inverseRotation(final Quaternion result) {
inverse(result);
}
/**
* Sets into provided Rotation3D instance a rotation inverse to this
* instance.
* The combination of this rotation with its inverse produces no change.
*
* @param result Instance where inverse rotation will be set.
*/
@Override
public void inverseRotation(final Rotation3D result) {
final var inverse = inverseAndReturnNew();
result.fromRotation(inverse);
}
/**
* Reverses the rotation of this instance.
*/
@Override
public void inverseRotation() {
inverse();
}
/**
* Combines provided quaternions q1 and q2 to produce a resulting quaternion
* equivalent to the combined rotation of both quaternions.
*
* @param q1 1st quaternion.
* @param q2 2nd quaternion.
* @param result combined quaternion where result is stored.
*/
public static void combine(final Quaternion q1, final Quaternion q2, final Quaternion result) {
product(q1, q2, result);
}
/**
* Combines provided quaternion with this quaternion and returns the result
* as a new quaternion instance.
*
* @param q input quaternion to be combined.
* @return combined quaternion, which is equal to the multiplication of
* quaternions.
*/
public Quaternion combineAndReturnNew(final Quaternion q) {
final var result = new Quaternion();
combine(this, q, result);
return result;
}
/**
* Combines provided quaternion into this quaternion, resulting in the
* multiplication of both quaternion representations.
*
* @param q input quaternion to be combined.
*/
public void combine(final Quaternion q) {
combine(this, q, this);
}
/**
* Combines provided rotation with this quaternion and returns the result as
* a new quaternion instance.
*
* @param rotation input rotation to be combined.
* @return combined rotation, which is equal to the multiplication of the
* internal quaternion representations.
*/
@Override
public Rotation3D combineAndReturnNew(final Rotation3D rotation) {
return combineAndReturnNew(rotation.toQuaternion());
}
/**
* Combines provided rotation into this quaternion, resulting in the
* multiplication of both quaternion representations.
*
* @param rotation input rotation to be combined.
*/
@Override
public void combine(final Rotation3D rotation) {
combine(rotation.toQuaternion());
}
/**
* Sets values of this rotation from a 3D matrix rotation.
*
* @param rot 3D matrix rotation to set values from.
*/
@Override
public void fromRotation(final MatrixRotation3D rot) {
setFromMatrixRotation(rot);
}
/**
* Sets values of this rotation from a 3D axis rotation.
*
* @param rot an axis rotation to set values from.
*/
@Override
public void fromRotation(final AxisRotation3D rot) {
setFromAxisAndRotation(rot);
}
/**
* Sets values of this rotation from a quaternion.
*
* @param q a quaternion to set values from.
*/
@Override
public void fromRotation(final Quaternion q) {
a = q.a;
b = q.b;
c = q.c;
d = q.d;
normalized = q.normalized;
}
/**
* Converts this 3D rotation into a quaternion storing the result into
* provided instance.
*
* @param result instance where result will be stored.
*/
@Override
public void toQuaternion(final Quaternion result) {
result.fromQuaternion(this);
}
/**
* Indicates whether quaternion is already normalized or not.
*
* @return true if quaternion is normalized, false otherwise.
*/
public boolean isNormalized() {
return normalized;
}
/**
* Normalizes this quaternion if not already normalized.
*/
public void normalize() {
if (!normalized) {
final var norm = Math.sqrt(a * a + b * b + c * c + d * d);
internalNormalize(norm);
}
}
/**
* Normalizes this quaternion if not already normalized and stores the
* corresponding jacobian into provided matrix (if provided).
*
* @param jacobian matrix where jacobian will be stored (if provided). Must
* be 4x4.
* @throws IllegalArgumentException if provided jacobian is not 4x4.
*/
public void normalize(final Matrix jacobian) {
if (jacobian != null && (jacobian.getRows() != N_PARAMS || jacobian.getColumns() != N_PARAMS)) {
throw new IllegalArgumentException("jacobian must be 4x4");
}
final var aValue = this.a;
final var bValue = this.b;
final var cValue = this.c;
final var dValue = this.d;
final var norm = Math.sqrt(aValue * aValue + bValue * bValue + cValue * cValue + dValue * dValue);
internalNormalize(norm);
if (jacobian != null) {
final var norm3 = norm * norm * norm;
jacobian.setElementAt(0, 0, (bValue * bValue + cValue * cValue + dValue * dValue)
/ norm3);
jacobian.setElementAt(1, 0, -aValue / norm3 * bValue);
jacobian.setElementAt(2, 0, -aValue / norm3 * cValue);
jacobian.setElementAt(3, 0, -aValue / norm3 * dValue);
jacobian.setElementAt(0, 1, -aValue / norm3 * bValue);
jacobian.setElementAt(1, 1, (aValue * aValue + cValue * cValue + dValue * dValue)
/ norm3);
jacobian.setElementAt(2, 1, -bValue / norm3 * cValue);
jacobian.setElementAt(3, 1, -bValue / norm3 * dValue);
jacobian.setElementAt(0, 2, -aValue / norm3 * cValue);
jacobian.setElementAt(1, 2, -bValue / norm3 * cValue);
jacobian.setElementAt(2, 2, (aValue * aValue + bValue * bValue + dValue * dValue)
/ norm3);
jacobian.setElementAt(3, 2, -cValue / norm3 * dValue);
jacobian.setElementAt(0, 3, -aValue / norm3 * dValue);
jacobian.setElementAt(1, 3, -bValue / norm3 * dValue);
jacobian.setElementAt(2, 3, -cValue / norm3 * dValue);
jacobian.setElementAt(3, 3, (aValue * aValue + bValue * bValue + cValue * cValue)
/ norm3);
}
}
/**
* Computes a linear interpolation between this quaternion and provided quaternion using
* provided value as the interpolation ratio.
*
* @param q quaternion to interpolate.
* @param t interpolation ratio. Must be a value between 0.0 and 1.0, both
* included. The closer the value is to 0.0, the more similar result will
* be to this instance. Conversely, the closer the value is to 1.0, the
* more similar result will be to q.
* @return a new interpolated quaternion instance.
* @throws IllegalArgumentException if provided interpolation ratio is not between 0.0
* and 1.0, both included.
*/
public Quaternion slerpAndReturnNew(final Quaternion q, final double t) {
final var result = new Quaternion();
slerp(q, t, result);
return result;
}
/**
* Computes a linear interpolation between this quaternion and provided quaternion using
* provided value as the interpolation ratio and stores the result into provided result
* quaternion.
*
* @param q quaternion to interpolate.
* @param t interpolation ratio. Must be a value between 0.0 and 1.0, both
* included. The closer the value is to 0.0, the more similar result will
* be to this instance. Conversely, the closer the value is to 1.0, the
* more similar result will be to q.
* @param result instance where interpolated quaternion will be stored.
* @throws IllegalArgumentException if provided interpolation ratio is not between 0.0
* and 1.0, both included.
*/
public void slerp(final Quaternion q, final double t, final Quaternion result) {
slerp(this, q, t, result);
}
/**
* Computes a linear interpolation between provided quaternions using provided value
* as the interpolation ratio.
*
* @param q1 1st quaternion to interpolate.
* @param q2 2nd quaternion to interpolate.
* @param t interpolation ratio. Must be a value between 0.0 and 1.0, both
* included. The closer the value is to 0.0, the more similar result will
* be to q1. Conversely, the closer the value is to 1.0, the more similar
* result will be to q2.
* @return a new interpolated quaternion instance.
* @throws IllegalArgumentException if provided interpolation ratio is not between 0.0
* and 1.0, both included.
*/
public static Quaternion slerpAndReturnNew(final Quaternion q1, final Quaternion q2, final double t) {
final var result = new Quaternion();
slerp(q1, q2, t, result);
return result;
}
/**
* Computes a linear interpolation between provided quaternions using provided value
* as the interpolation ratio, and stores the result into provided result quaternion.
*
* @param q1 1st quaternion to interpolate.
* @param q2 2nd quaternion to interpolate.
* @param t interpolation ratio. Must be a value between 0.0 and 1.0, both
* included. The closer the value is to 0.0, the more similar result will
* be to q1. Conversely, the closer the value is to 1.0, the more similar
* result will be to q2.
* @param result instance where interpolated quaternion will be stored.
* @throws IllegalArgumentException if provided interpolation ratio is not between 0.0
* and 1.0, both included.
*/
public static void slerp(final Quaternion q1, final Quaternion q2, final double t, final Quaternion result) {
if (t < 0.0 || t > 1.0) {
throw new IllegalArgumentException();
}
// only unit quaternions are valid rotations.
// normalize to avoid undefined behavior.
q1.normalize();
q2.normalize();
// calculate angle between input quaternions
var dot = q1.a * q2.a + q1.b * q2.b + q1.c * q2.c + q1.d * q2.d;
// if q1 = q2 or q1 = -q2 (both quaternions are equal), then the angle
// between input quaternions is theta0 = 0.
// To avoid singularity caused by sinTheta0, we return q1
if (Math.abs(dot) >= 1.0) {
result.a = q1.a;
result.b = q1.b;
result.c = q1.c;
result.d = q1.d;
return;
}
// if the dot product is negative, slerp won't take the shorter path.
// note that q2 and -q2 are equivalent when the negation is applied to all four
// components. Fix by reversing one quaternion.
Quaternion q2b;
if (dot < 0.0) {
q2b = new Quaternion(-q2.a, -q2.b, -q2.c, -q2.d);
dot = -dot;
} else {
q2b = q2;
}
final var theta0 = Math.acos(dot);
final var theta = theta0 * t;
final var sinTheta = Math.sin(theta);
final var sinTheta0 = Math.sin(theta0);
final var s2 = sinTheta / sinTheta0;
// line below is equal to sin(theta0 - theta) / sinTheta0
final var s1 = Math.cos(theta) - dot * s2;
result.a = s1 * q1.a + s2 * q2b.a;
result.b = s1 * q1.b + s2 * q2b.b;
result.c = s1 * q1.c + s2 * q2b.c;
result.d = s1 * q1.d + s2 * q2b.d;
}
/**
* Normalizes this quaternion if not already normalized.
*
* @param norm norm to normalize this quaternion with.
*/
private void internalNormalize(final double norm) {
if (!normalized) {
a /= norm;
b /= norm;
c /= norm;
d /= norm;
normalized = true;
}
}
}