Quaternion.java
/*
* 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.numbers.quaternion;
import java.util.Arrays;
import java.util.function.ToDoubleFunction;
import java.util.function.BiPredicate;
import java.io.Serializable;
import org.apache.commons.numbers.core.Precision;
/**
* This class implements <a href="http://mathworld.wolfram.com/Quaternion.html">
* quaternions</a> (Hamilton's hypercomplex numbers).
*
* <p>Wherever quaternion components are listed in sequence, this class follows the
* convention of placing the scalar ({@code w}) component first, e.g. [{@code w, x, y, z}].
* Other libraries and textbooks may place the {@code w} component last.</p>
*
* <p>Instances of this class are guaranteed to be immutable.</p>
*/
public final class Quaternion implements Serializable {
/** Zero quaternion. */
public static final Quaternion ZERO = of(0, 0, 0, 0);
/** Identity quaternion. */
public static final Quaternion ONE = new Quaternion(Type.POSITIVE_POLAR_FORM, 1, 0, 0, 0);
/** i. */
public static final Quaternion I = new Quaternion(Type.POSITIVE_POLAR_FORM, 0, 1, 0, 0);
/** j. */
public static final Quaternion J = new Quaternion(Type.POSITIVE_POLAR_FORM, 0, 0, 1, 0);
/** k. */
public static final Quaternion K = new Quaternion(Type.POSITIVE_POLAR_FORM, 0, 0, 0, 1);
/** Serializable version identifier. */
private static final long serialVersionUID = 20170118L;
/** Error message. */
private static final String ILLEGAL_NORM_MSG = "Illegal norm: ";
/** {@link #toString() String representation}. */
private static final String FORMAT_START = "[";
/** {@link #toString() String representation}. */
private static final String FORMAT_END = "]";
/** {@link #toString() String representation}. */
private static final String FORMAT_SEP = " ";
/** The number of dimensions for the vector part of the quaternion. */
private static final int VECTOR_DIMENSIONS = 3;
/** The number of parts when parsing a text representation of the quaternion. */
private static final int NUMBER_OF_PARTS = 4;
/** For enabling specialized method implementations. */
private final Type type;
/** First component (scalar part). */
private final double w;
/** Second component (first vector part). */
private final double x;
/** Third component (second vector part). */
private final double y;
/** Fourth component (third vector part). */
private final double z;
/**
* For enabling optimized implementations.
*/
private enum Type {
/** Default implementation. */
DEFAULT(Default.NORMSQ,
Default.NORM,
Default.IS_UNIT),
/** Quaternion has unit norm. */
NORMALIZED(Normalized.NORM,
Normalized.NORM,
Normalized.IS_UNIT),
/** Quaternion has positive scalar part. */
POSITIVE_POLAR_FORM(Normalized.NORM,
Normalized.NORM,
Normalized.IS_UNIT);
/** {@link Quaternion#normSq()}. */
private final ToDoubleFunction<Quaternion> normSq;
/** {@link Quaternion#norm()}. */
private final ToDoubleFunction<Quaternion> norm;
/** {@link Quaternion#isUnit(double)}. */
private final BiPredicate<Quaternion, Double> testIsUnit;
/** Default implementations. */
private static final class Default {
/** {@link Quaternion#normSq()}. */
static final ToDoubleFunction<Quaternion> NORMSQ = q ->
q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;
/** {@link Quaternion#norm()}. */
private static final ToDoubleFunction<Quaternion> NORM = q ->
Math.sqrt(NORMSQ.applyAsDouble(q));
/** {@link Quaternion#isUnit(double)}. */
private static final BiPredicate<Quaternion, Double> IS_UNIT = (q, eps) ->
Precision.equals(NORM.applyAsDouble(q), 1d, eps);
}
/** Implementations for normalized quaternions. */
private static final class Normalized {
/** {@link Quaternion#norm()} returns 1. */
static final ToDoubleFunction<Quaternion> NORM = q -> 1;
/** {@link Quaternion#isUnit(double)} returns 1. */
static final BiPredicate<Quaternion, Double> IS_UNIT = (q, eps) -> true;
}
/**
* @param normSq {@code normSq} method.
* @param norm {@code norm} method.
* @param isUnit {@code isUnit} method.
*/
Type(ToDoubleFunction<Quaternion> normSq,
ToDoubleFunction<Quaternion> norm,
BiPredicate<Quaternion, Double> isUnit) {
this.normSq = normSq;
this.norm = norm;
this.testIsUnit = isUnit;
}
/**
* @param q Quaternion.
* @return the norm squared.
*/
double normSq(Quaternion q) {
return normSq.applyAsDouble(q);
}
/**
* @param q Quaternion.
* @return the norm.
*/
double norm(Quaternion q) {
return norm.applyAsDouble(q);
}
/**
* @param q Quaternion.
* @param eps Tolerance.
* @return whether {@code q} has unit norm within the allowed tolerance.
*/
boolean isUnit(Quaternion q,
double eps) {
return testIsUnit.test(q, eps);
}
}
/**
* Builds a quaternion from its components.
*
* @param type Quaternion type.
* @param w Scalar component.
* @param x First vector component.
* @param y Second vector component.
* @param z Third vector component.
*/
private Quaternion(Type type,
final double w,
final double x,
final double y,
final double z) {
this.type = type;
this.w = w;
this.x = x;
this.y = y;
this.z = z;
}
/**
* Copies the given quaternion, but change its {@link Type}.
*
* @param type Quaternion type.
* @param q Quaternion whose components will be copied.
*/
private Quaternion(Type type,
Quaternion q) {
this.type = type;
w = q.w;
x = q.x;
y = q.y;
z = q.z;
}
/**
* Builds a quaternion from its components.
*
* @param w Scalar component.
* @param x First vector component.
* @param y Second vector component.
* @param z Third vector component.
* @return a quaternion instance.
*/
public static Quaternion of(final double w,
final double x,
final double y,
final double z) {
return new Quaternion(Type.DEFAULT,
w, x, y, z);
}
/**
* Builds a quaternion from scalar and vector parts.
*
* @param scalar Scalar part of the quaternion.
* @param v Components of the vector part of the quaternion.
* @return a quaternion instance.
*
* @throws IllegalArgumentException if the array length is not 3.
*/
public static Quaternion of(final double scalar,
final double[] v) {
if (v.length != VECTOR_DIMENSIONS) {
throw new IllegalArgumentException("Size of array must be 3");
}
return of(scalar, v[0], v[1], v[2]);
}
/**
* Builds a pure quaternion from a vector (assuming that the scalar
* part is zero).
*
* @param v Components of the vector part of the pure quaternion.
* @return a quaternion instance.
*/
public static Quaternion of(final double[] v) {
return of(0, v);
}
/**
* Returns the conjugate of this quaternion number.
* The conjugate of {@code a + bi + cj + dk} is {@code a - bi -cj -dk}.
*
* @return the conjugate of this quaternion object.
*/
public Quaternion conjugate() {
return of(w, -x, -y, -z);
}
/**
* Returns the Hamilton product of two quaternions.
*
* @param q1 First quaternion.
* @param q2 Second quaternion.
* @return the product {@code q1} and {@code q2}, in that order.
*/
public static Quaternion multiply(final Quaternion q1,
final Quaternion q2) {
// Components of the first quaternion.
final double q1a = q1.w;
final double q1b = q1.x;
final double q1c = q1.y;
final double q1d = q1.z;
// Components of the second quaternion.
final double q2a = q2.w;
final double q2b = q2.x;
final double q2c = q2.y;
final double q2d = q2.z;
// Components of the product.
final double w = q1a * q2a - q1b * q2b - q1c * q2c - q1d * q2d;
final double x = q1a * q2b + q1b * q2a + q1c * q2d - q1d * q2c;
final double y = q1a * q2c - q1b * q2d + q1c * q2a + q1d * q2b;
final double z = q1a * q2d + q1b * q2c - q1c * q2b + q1d * q2a;
return of(w, x, y, z);
}
/**
* Returns the Hamilton product of the instance by a quaternion.
*
* @param q Quaternion.
* @return the product of this instance with {@code q}, in that order.
*/
public Quaternion multiply(final Quaternion q) {
return multiply(this, q);
}
/**
* Computes the sum of two quaternions.
*
* @param q1 Quaternion.
* @param q2 Quaternion.
* @return the sum of {@code q1} and {@code q2}.
*/
public static Quaternion add(final Quaternion q1,
final Quaternion q2) {
return of(q1.w + q2.w,
q1.x + q2.x,
q1.y + q2.y,
q1.z + q2.z);
}
/**
* Computes the sum of the instance and another quaternion.
*
* @param q Quaternion.
* @return the sum of this instance and {@code q}.
*/
public Quaternion add(final Quaternion q) {
return add(this, q);
}
/**
* Subtracts two quaternions.
*
* @param q1 First Quaternion.
* @param q2 Second quaternion.
* @return the difference between {@code q1} and {@code q2}.
*/
public static Quaternion subtract(final Quaternion q1,
final Quaternion q2) {
return of(q1.w - q2.w,
q1.x - q2.x,
q1.y - q2.y,
q1.z - q2.z);
}
/**
* Subtracts a quaternion from the instance.
*
* @param q Quaternion.
* @return the difference between this instance and {@code q}.
*/
public Quaternion subtract(final Quaternion q) {
return subtract(this, q);
}
/**
* Computes the dot-product of two quaternions.
*
* @param q1 Quaternion.
* @param q2 Quaternion.
* @return the dot product of {@code q1} and {@code q2}.
*/
public static double dot(final Quaternion q1,
final Quaternion q2) {
return q1.w * q2.w +
q1.x * q2.x +
q1.y * q2.y +
q1.z * q2.z;
}
/**
* Computes the dot-product of the instance by a quaternion.
*
* @param q Quaternion.
* @return the dot product of this instance and {@code q}.
*/
public double dot(final Quaternion q) {
return dot(this, q);
}
/**
* Computes the norm of the quaternion.
*
* @return the norm.
*/
public double norm() {
return type.norm(this);
}
/**
* Computes the square of the norm of the quaternion.
*
* @return the square of the norm.
*/
public double normSq() {
return type.normSq(this);
}
/**
* Computes the normalized quaternion (the versor of the instance).
* The norm of the quaternion must not be near zero.
*
* @return a normalized quaternion.
* @throws IllegalStateException if the norm of the quaternion is NaN, infinite,
* or near zero.
*/
public Quaternion normalize() {
switch (type) {
case NORMALIZED:
case POSITIVE_POLAR_FORM:
return this;
case DEFAULT:
final double norm = norm();
if (norm < Precision.SAFE_MIN ||
!Double.isFinite(norm)) {
throw new IllegalStateException(ILLEGAL_NORM_MSG + norm);
}
final Quaternion unit = divide(norm);
return w >= 0 ?
new Quaternion(Type.POSITIVE_POLAR_FORM, unit) :
new Quaternion(Type.NORMALIZED, unit);
default:
throw new IllegalStateException(); // Should never happen.
}
}
/**
* {@inheritDoc}
*/
@Override
public boolean equals(Object other) {
if (this == other) {
return true;
}
if (other instanceof Quaternion) {
final Quaternion q = (Quaternion) other;
return ((Double) w).equals(q.w) &&
((Double) x).equals(q.x) &&
((Double) y).equals(q.y) &&
((Double) z).equals(q.z);
}
return false;
}
/**
* {@inheritDoc}
*/
@Override
public int hashCode() {
return Arrays.hashCode(new double[] {w, x, y, z});
}
/**
* Checks whether this instance is equal to another quaternion
* within a given tolerance.
*
* @param q Quaternion with which to compare the current quaternion.
* @param eps Tolerance.
* @return {@code true} if the each of the components are equal
* within the allowed absolute error.
*/
public boolean equals(final Quaternion q,
final double eps) {
return Precision.equals(w, q.w, eps) &&
Precision.equals(x, q.x, eps) &&
Precision.equals(y, q.y, eps) &&
Precision.equals(z, q.z, eps);
}
/**
* Checks whether the instance is a unit quaternion within a given
* tolerance.
*
* @param eps Tolerance (absolute error).
* @return {@code true} if the norm is 1 within the given tolerance,
* {@code false} otherwise
*/
public boolean isUnit(double eps) {
return type.isUnit(this, eps);
}
/**
* Checks whether the instance is a pure quaternion within a given
* tolerance.
*
* @param eps Tolerance (absolute error).
* @return {@code true} if the scalar part of the quaternion is zero.
*/
public boolean isPure(double eps) {
return Math.abs(w) <= eps;
}
/**
* Returns the polar form of the quaternion.
*
* @return the unit quaternion with positive scalar part.
*/
public Quaternion positivePolarForm() {
switch (type) {
case POSITIVE_POLAR_FORM:
return this;
case NORMALIZED:
return w >= 0 ?
new Quaternion(Type.POSITIVE_POLAR_FORM, this) :
new Quaternion(Type.POSITIVE_POLAR_FORM, negate());
case DEFAULT:
return w >= 0 ?
normalize() :
// The quaternion of rotation (normalized quaternion) q and -q
// are equivalent (i.e. represent the same rotation).
negate().normalize();
default:
throw new IllegalStateException(); // Should never happen.
}
}
/**
* Returns the opposite of this instance.
*
* @return the quaternion for which all components have an opposite
* sign to this one.
*/
public Quaternion negate() {
switch (type) {
case POSITIVE_POLAR_FORM:
case NORMALIZED:
return new Quaternion(Type.NORMALIZED, -w, -x, -y, -z);
case DEFAULT:
return new Quaternion(Type.DEFAULT, -w, -x, -y, -z);
default:
throw new IllegalStateException(); // Should never happen.
}
}
/**
* Returns the inverse of this instance.
* The norm of the quaternion must not be zero.
*
* @return the inverse.
* @throws IllegalStateException if the norm (squared) of the quaternion is NaN,
* infinite, or near zero.
*/
public Quaternion inverse() {
switch (type) {
case POSITIVE_POLAR_FORM:
case NORMALIZED:
return new Quaternion(type, w, -x, -y, -z);
case DEFAULT:
final double squareNorm = normSq();
if (squareNorm < Precision.SAFE_MIN ||
!Double.isFinite(squareNorm)) {
throw new IllegalStateException(ILLEGAL_NORM_MSG + Math.sqrt(squareNorm));
}
return of(w / squareNorm,
-x / squareNorm,
-y / squareNorm,
-z / squareNorm);
default:
throw new IllegalStateException(); // Should never happen.
}
}
/**
* Gets the first component of the quaternion (scalar part).
*
* @return the scalar part.
*/
public double getW() {
return w;
}
/**
* Gets the second component of the quaternion (first component
* of the vector part).
*
* @return the first component of the vector part.
*/
public double getX() {
return x;
}
/**
* Gets the third component of the quaternion (second component
* of the vector part).
*
* @return the second component of the vector part.
*/
public double getY() {
return y;
}
/**
* Gets the fourth component of the quaternion (third component
* of the vector part).
*
* @return the third component of the vector part.
*/
public double getZ() {
return z;
}
/**
* Gets the scalar part of the quaternion.
*
* @return the scalar part.
* @see #getW()
*/
public double getScalarPart() {
return getW();
}
/**
* Gets the three components of the vector part of the quaternion.
*
* @return the vector part.
* @see #getX()
* @see #getY()
* @see #getZ()
*/
public double[] getVectorPart() {
return new double[] {x, y, z};
}
/**
* Multiplies the instance by a scalar.
*
* @param alpha Scalar factor.
* @return a scaled quaternion.
*/
public Quaternion multiply(final double alpha) {
return of(alpha * w,
alpha * x,
alpha * y,
alpha * z);
}
/**
* Divides the instance by a scalar.
*
* @param alpha Scalar factor.
* @return a scaled quaternion.
*/
public Quaternion divide(final double alpha) {
return of(w / alpha,
x / alpha,
y / alpha,
z / alpha);
}
/**
* Parses a string that would be produced by {@link #toString()}
* and instantiates the corresponding object.
*
* @param s String representation.
* @return an instance.
* @throws NumberFormatException if the string does not conform
* to the specification.
*/
public static Quaternion parse(String s) {
final int startBracket = s.indexOf(FORMAT_START);
if (startBracket != 0) {
throw new QuaternionParsingException("Expected start string: " + FORMAT_START);
}
final int len = s.length();
final int endBracket = s.indexOf(FORMAT_END);
if (endBracket != len - 1) {
throw new QuaternionParsingException("Expected end string: " + FORMAT_END);
}
final String[] elements = s.substring(1, s.length() - 1).split(FORMAT_SEP);
if (elements.length != NUMBER_OF_PARTS) {
throw new QuaternionParsingException("Incorrect number of parts: Expected 4 but was " +
elements.length +
" (separator is '" + FORMAT_SEP + "')");
}
final double a;
try {
a = Double.parseDouble(elements[0]);
} catch (NumberFormatException ex) {
throw new QuaternionParsingException("Could not parse scalar part" + elements[0], ex);
}
final double b;
try {
b = Double.parseDouble(elements[1]);
} catch (NumberFormatException ex) {
throw new QuaternionParsingException("Could not parse i part" + elements[1], ex);
}
final double c;
try {
c = Double.parseDouble(elements[2]);
} catch (NumberFormatException ex) {
throw new QuaternionParsingException("Could not parse j part" + elements[2], ex);
}
final double d;
try {
d = Double.parseDouble(elements[3]);
} catch (NumberFormatException ex) {
throw new QuaternionParsingException("Could not parse k part" + elements[3], ex);
}
return of(a, b, c, d);
}
/**
* {@inheritDoc}
*/
@Override
public String toString() {
final StringBuilder s = new StringBuilder();
s.append(FORMAT_START)
.append(w).append(FORMAT_SEP)
.append(x).append(FORMAT_SEP)
.append(y).append(FORMAT_SEP)
.append(z)
.append(FORMAT_END);
return s.toString();
}
/** See {@link #parse(String)}. */
private static class QuaternionParsingException extends NumberFormatException {
/** Serializable version identifier. */
private static final long serialVersionUID = 20181128L;
/**
* @param msg Error message.
*/
QuaternionParsingException(String msg) {
super(msg);
}
/**
* @param msg Error message.
* @param cause Cause of the exception.
*/
QuaternionParsingException(String msg, Throwable cause) {
super(msg);
initCause(cause);
}
}
}