001/*
002 *                    BioJava development code
003 *
004 * This code may be freely distributed and modified under the
005 * terms of the GNU Lesser General Public Licence.  This should
006 * be distributed with the code.  If you do not have a copy,
007 * see:
008 *
009 *      http://www.gnu.org/copyleft/lesser.html
010 *
011 * Copyright for this code is held jointly by the individual
012 * authors.  These should be listed in @author doc comments.
013 *
014 * For more information on the BioJava project and its aims,
015 * or to join the biojava-l mailing list, visit the home page
016 * at:
017 *
018 *      http://www.biojava.org/
019 *
020 */
021package org.biojava.nbio.structure.geometry;
022
023import javax.vecmath.AxisAngle4d;
024import javax.vecmath.Point3d;
025import javax.vecmath.Quat4d;
026
027import org.biojava.nbio.structure.jama.EigenvalueDecomposition;
028import org.biojava.nbio.structure.jama.Matrix;
029
030/**
031 * UnitQuaternions is a static Class that contains methods for calculating and
032 * using unit quaternions. It assumes the use of {@link Quat4d} Class from
033 * vecmath to represent the unit quaternions, and it also implements some of the
034 * basic methods that the library is missing.
035 * <p>
036 * A Unit Quaternion is a four-dimensional vector used to describe a
037 * three-dimensional attitude representation (axis and angle of rotation). By
038 * definition, unit quaternions are always normalized, so their length is always
039 * 1.
040 * 
041 * @author Aleix Lafita
042 * @since 5.0.0
043 *
044 */
045public class UnitQuaternions {
046
047        /** Prevent instantiation */
048        private UnitQuaternions() {
049        }
050
051        /**
052         * The orientation metric is obtained by comparing the quaternion
053         * orientations of the principal axes of each set of points in 3D.
054         * <p>
055         * First, the quaternion orientation of each set of points is calculated
056         * using their principal axes with {@link #orientation(Point3d[])}. Then,
057         * the two quaternions are compared using the method
058         * {@link #orientationMetric(Quat4d, Quat4d)}.
059         * <p>
060         * A requisite for this method to work properly is that both sets of points
061         * have to define the same shape (or very low RMSD), otherwise some of the
062         * principal axes might change or be inverted, resulting in an unreliable
063         * metric. For shapes with some deviations in their shape, use the metric
064         * {@link #orientationAngle(Point3d[], Point3d[])}.
065         * 
066         * @param a
067         *            array of Point3d
068         * @param b
069         *            array of Point3d
070         * @return the quaternion orientation metric
071         */
072        public static double orientationMetric(Point3d[] a, Point3d[] b) {
073
074                Quat4d qa = orientation(a);
075                Quat4d qb = orientation(b);
076
077                return orientationMetric(qa, qb);
078        }
079
080        /**
081         * The orientation metric is obtained by comparing two unit quaternion
082         * orientations.
083         * <p>
084         * The two quaternions are compared using the formula: d(q1,q2) =
085         * arccos(|q1*q2|). The range of the metric is [0, Pi/2], where 0 means the
086         * same orientation and Pi/2 means the opposite orientation.
087         * <p>
088         * The formula is taken from: Huynh, D. Q. (2009). Metrics for 3D rotations:
089         * comparison and analysis. Journal of Mathematical Imaging and Vision,
090         * 35(2), 155–164. http://doi.org/10.1007/s10851-009-0161-2
091         * 
092         * @param q1
093         *            quaternion as Quat4d object
094         * @param q2
095         *            quaternion as Quat4d object
096         * @return the quaternion orientation metric
097         */
098        public static double orientationMetric(Quat4d q1, Quat4d q2) {
099                return Math.acos(Math.abs(dotProduct(q1, q2)));
100        }
101
102        /**
103         * The orientation represents the rotation of the principal axes with
104         * respect to the axes of the coordinate system (unit vectors [1,0,0],
105         * [0,1,0] and [0,0,1]).
106         * <p>
107         * The orientation can be expressed as a unit quaternion.
108         * 
109         * @param points
110         *            array of Point3d
111         * @return the orientation of the point cloud as a unit quaternion
112         */
113        public static Quat4d orientation(Point3d[] points) {
114
115                MomentsOfInertia moi = new MomentsOfInertia();
116
117                for (Point3d p : points)
118                        moi.addPoint(p, 1.0);
119
120                // Convert rotation matrix to quaternion
121                Quat4d quat = new Quat4d();
122                quat.set(moi.getOrientationMatrix());
123
124                return quat;
125        }
126
127        /**
128         * Calculate the rotation angle component of the input unit quaternion.
129         * 
130         * @param q
131         *            unit quaternion Quat4d
132         * @return the angle in radians of the input quaternion
133         */
134        public static double angle(Quat4d q) {
135                AxisAngle4d axis = new AxisAngle4d();
136                axis.set(q);
137                return axis.angle;
138        }
139
140        /**
141         * The angle of the relative orientation of the two sets of points in 3D.
142         * Equivalent to {@link #angle(Quat4d)} of the unit quaternion obtained by
143         * {@link #relativeOrientation(Point3d[], Point3d[])}.
144         * <p>
145         * The arrays of points need to be centered at the origin. To center the
146         * points use {@link CalcPoint#center(Point3d[])}.
147         * 
148         * @param fixed
149         *            array of Point3d, centered at origin. Original coordinates
150         *            will not be modified.
151         * @param moved
152         *            array of Point3d, centered at origin. Original coordinates
153         *            will not be modified.
154         * @return the angle in radians of the relative orientation of the points,
155         *         angle to rotate moved to bring it to the same orientation as
156         *         fixed.
157         */
158        public static double orientationAngle(Point3d[] fixed, Point3d[] moved) {
159                Quat4d q = relativeOrientation(fixed, moved);
160                return angle(q);
161        }
162
163        /**
164         * The angle of the relative orientation of the two sets of points in 3D.
165         * Equivalent to {@link #angle(Quat4d)} of the unit quaternion obtained by
166         * {@link #relativeOrientation(Point3d[], Point3d[])}.
167         * 
168         * @param fixed
169         *            array of Point3d. Original coordinates will not be modified.
170         * @param moved
171         *            array of Point3d. Original coordinates will not be modified.
172         * @param centered
173         *            true if the points are already centered at the origin
174         * @return the angle in radians of the relative orientation of the points,
175         *         angle to rotate moved to bring it to the same orientation as
176         *         fixed.
177         */
178        public static double orientationAngle(Point3d[] fixed, Point3d[] moved,
179                        boolean centered) {
180                if (!centered) {
181                        fixed = CalcPoint.clonePoint3dArray(fixed);
182                        moved = CalcPoint.clonePoint3dArray(moved);
183                        CalcPoint.center(fixed);
184                        CalcPoint.center(moved);
185                }
186                return orientationAngle(fixed, moved);
187        }
188
189        /**
190         * Calculate the relative quaternion orientation of two arrays of points.
191         * 
192         * @param fixed
193         *            point array, coordinates will not be modified
194         * @param moved
195         *            point array, coordinates will not be modified
196         * @return a unit quaternion representing the relative orientation, to
197         *         rotate moved to bring it to the same orientation as fixed.
198         */
199        public static Quat4d relativeOrientation(Point3d[] fixed, Point3d[] moved) {
200                Matrix m = CalcPoint.formMatrix(moved, fixed); // inverse
201                EigenvalueDecomposition eig = m.eig();
202                double[][] v = eig.getV().getArray();
203                Quat4d q = new Quat4d(v[1][3], v[2][3], v[3][3], v[0][3]);
204                q.normalize();
205                q.conjugate();
206                return q;
207        }
208
209        /**
210         * Compute the dot (inner) product of two quaternions.
211         * 
212         * @param q1
213         *            quaternion as Quat4d object
214         * @param q2
215         *            quaternion as Quat4d object
216         * @return the value of the quaternion dot product
217         */
218        public static double dotProduct(Quat4d q1, Quat4d q2) {
219                return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
220        }
221
222}