org.biojava.nbio.structure.geometry.UnitQuaternions Maven / Gradle / Ivy
/*
* BioJava development code
*
* This code may be freely distributed and modified under the
* terms of the GNU Lesser General Public Licence. This should
* be distributed with the code. If you do not have a copy,
* see:
*
* http://www.gnu.org/copyleft/lesser.html
*
* Copyright for this code is held jointly by the individual
* authors. These should be listed in @author doc comments.
*
* For more information on the BioJava project and its aims,
* or to join the biojava-l mailing list, visit the home page
* at:
*
* http://www.biojava.org/
*
*/
package org.biojava.nbio.structure.geometry;
import javax.vecmath.AxisAngle4d;
import javax.vecmath.Point3d;
import javax.vecmath.Quat4d;
import org.biojava.nbio.structure.jama.EigenvalueDecomposition;
import org.biojava.nbio.structure.jama.Matrix;
/**
* UnitQuaternions is a static Class that contains methods for calculating and
* using unit quaternions. It assumes the use of {@link Quat4d} Class from
* vecmath to represent the unit quaternions, and it also implements some of the
* basic methods that the library is missing.
*
* A Unit Quaternion is a four-dimensional vector used to describe a
* three-dimensional attitude representation (axis and angle of rotation). By
* definition, unit quaternions are always normalized, so their length is always
* 1.
*
* @author Aleix Lafita
* @since 5.0.0
*
*/
public class UnitQuaternions {
/** Prevent instantiation */
private UnitQuaternions() {
}
/**
* The orientation metric is obtained by comparing the quaternion
* orientations of the principal axes of each set of points in 3D.
*
* First, the quaternion orientation of each set of points is calculated
* using their principal axes with {@link #orientation(Point3d[])}. Then,
* the two quaternions are compared using the method
* {@link #orientationMetric(Quat4d, Quat4d)}.
*
* A requisite for this method to work properly is that both sets of points
* have to define the same shape (or very low RMSD), otherwise some of the
* principal axes might change or be inverted, resulting in an unreliable
* metric. For shapes with some deviations in their shape, use the metric
* {@link #orientationAngle(Point3d[], Point3d[])}.
*
* @param a
* array of Point3d
* @param b
* array of Point3d
* @return the quaternion orientation metric
*/
public static double orientationMetric(Point3d[] a, Point3d[] b) {
Quat4d qa = orientation(a);
Quat4d qb = orientation(b);
return orientationMetric(qa, qb);
}
/**
* The orientation metric is obtained by comparing two unit quaternion
* orientations.
*
* The two quaternions are compared using the formula: d(q1,q2) =
* arccos(|q1*q2|). The range of the metric is [0, Pi/2], where 0 means the
* same orientation and Pi/2 means the opposite orientation.
*
* The formula is taken from: Huynh, D. Q. (2009). Metrics for 3D rotations:
* comparison and analysis. Journal of Mathematical Imaging and Vision,
* 35(2), 155–164. http://doi.org/10.1007/s10851-009-0161-2
*
* @param q1
* quaternion as Quat4d object
* @param q2
* quaternion as Quat4d object
* @return the quaternion orientation metric
*/
public static double orientationMetric(Quat4d q1, Quat4d q2) {
return Math.acos(Math.abs(dotProduct(q1, q2)));
}
/**
* The orientation represents the rotation of the principal axes with
* respect to the axes of the coordinate system (unit vectors [1,0,0],
* [0,1,0] and [0,0,1]).
*
* The orientation can be expressed as a unit quaternion.
*
* @param points
* array of Point3d
* @return the orientation of the point cloud as a unit quaternion
*/
public static Quat4d orientation(Point3d[] points) {
MomentsOfInertia moi = new MomentsOfInertia();
for (Point3d p : points)
moi.addPoint(p, 1.0);
// Convert rotation matrix to quaternion
Quat4d quat = new Quat4d();
quat.set(moi.getOrientationMatrix());
return quat;
}
/**
* Calculate the rotation angle component of the input unit quaternion.
*
* @param q
* unit quaternion Quat4d
* @return the angle in radians of the input quaternion
*/
public static double angle(Quat4d q) {
AxisAngle4d axis = new AxisAngle4d();
axis.set(q);
return axis.angle;
}
/**
* The angle of the relative orientation of the two sets of points in 3D.
* Equivalent to {@link #angle(Quat4d)} of the unit quaternion obtained by
* {@link #relativeOrientation(Point3d[], Point3d[])}.
*
* The arrays of points need to be centered at the origin. To center the
* points use {@link CalcPoint#center(Point3d[])}.
*
* @param fixed
* array of Point3d, centered at origin. Original coordinates
* will not be modified.
* @param moved
* array of Point3d, centered at origin. Original coordinates
* will not be modified.
* @return the angle in radians of the relative orientation of the points,
* angle to rotate moved to bring it to the same orientation as
* fixed.
*/
public static double orientationAngle(Point3d[] fixed, Point3d[] moved) {
Quat4d q = relativeOrientation(fixed, moved);
return angle(q);
}
/**
* The angle of the relative orientation of the two sets of points in 3D.
* Equivalent to {@link #angle(Quat4d)} of the unit quaternion obtained by
* {@link #relativeOrientation(Point3d[], Point3d[])}.
*
* @param fixed
* array of Point3d. Original coordinates will not be modified.
* @param moved
* array of Point3d. Original coordinates will not be modified.
* @param centered
* true if the points are already centered at the origin
* @return the angle in radians of the relative orientation of the points,
* angle to rotate moved to bring it to the same orientation as
* fixed.
*/
public static double orientationAngle(Point3d[] fixed, Point3d[] moved,
boolean centered) {
if (!centered) {
fixed = CalcPoint.clonePoint3dArray(fixed);
moved = CalcPoint.clonePoint3dArray(moved);
CalcPoint.center(fixed);
CalcPoint.center(moved);
}
return orientationAngle(fixed, moved);
}
/**
* Calculate the relative quaternion orientation of two arrays of points.
*
* @param fixed
* point array, coordinates will not be modified
* @param moved
* point array, coordinates will not be modified
* @return a unit quaternion representing the relative orientation, to
* rotate moved to bring it to the same orientation as fixed.
*/
public static Quat4d relativeOrientation(Point3d[] fixed, Point3d[] moved) {
Matrix m = CalcPoint.formMatrix(moved, fixed); // inverse
EigenvalueDecomposition eig = m.eig();
double[][] v = eig.getV().getArray();
Quat4d q = new Quat4d(v[1][3], v[2][3], v[3][3], v[0][3]);
q.normalize();
q.conjugate();
return q;
}
/**
* Compute the dot (inner) product of two quaternions.
*
* @param q1
* quaternion as Quat4d object
* @param q2
* quaternion as Quat4d object
* @return the value of the quaternion dot product
*/
public static double dotProduct(Quat4d q1, Quat4d q2) {
return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
}
}