/**
* Returns a Quaternion created from three Euler angle rotations. The angles represent rotation
* about their respective unit-axes. The angles are applied in the order X, Y, Z. Angles can be
* extracted by calling {@link #getRotationX}, {@link #getRotationY}, {@link #getRotationZ}.
*
* @param x Angle rotation about unit-X axis.
* @param y Angle rotation about unit-Y axis.
* @param z Angle rotation about unit-Z axis.
* @return Quaternion representation of the combined X-Y-Z rotation.
*/
public static Quaternion fromRotationXYZ(Angle x, Angle y, Angle z) {
if (x == null || y == null || z == null) {
throw new IllegalArgumentException("Angle Is Null");
}
double cx = x.cosHalfAngle();
double cy = y.cosHalfAngle();
double cz = z.cosHalfAngle();
double sx = x.sinHalfAngle();
double sy = y.sinHalfAngle();
double sz = z.sinHalfAngle();
// The order in which the three Euler angles are applied is critical. This can be thought of as
// multiplying
// three quaternions together, one for each Euler angle (and corresponding unit axis). Like
// matrices,
// quaternions affect vectors in reverse order. For example, suppose we construct a quaternion
// Q = (QX * QX) * QZ
// then transform some vector V by Q. This can be thought of as first transforming V by QZ, then
// QY, and
// finally by QX. This means that the order of quaternion multiplication is the reverse of the
// order in which
// the Euler angles are applied.
//
// The ordering below refers to the order in which angles are applied.
//
// QX = (sx, 0, 0, cx)
// QY = (0, sy, 0, cy)
// QZ = (0, 0, sz, cz)
//
// 1. XYZ Ordering
// (QZ * QY * QX)
// qw = (cx * cy * cz) + (sx * sy * sz);
// qx = (sx * cy * cz) - (cx * sy * sz);
// qy = (cx * sy * cz) + (sx * cy * sz);
// qz = (cx * cy * sz) - (sx * sy * cz);
//
// 2. ZYX Ordering
// (QX * QY * QZ)
// qw = (cx * cy * cz) - (sx * sy * sz);
// qx = (sx * cy * cz) + (cx * sy * sz);
// qy = (cx * sy * cz) - (sx * cy * sz);
// qz = (cx * cy * sz) + (sx * sy * cz);
//
double qw = (cx * cy * cz) + (sx * sy * sz);
double qx = (sx * cy * cz) - (cx * sy * sz);
double qy = (cx * sy * cz) + (sx * cy * sz);
double qz = (cx * cy * sz) - (sx * sy * cz);
return new Quaternion(qx, qy, qz, qw);
}
/**
* Returns a Quaternion created from latitude and longitude rotations. Latitude and longitude can
* be extracted from a Quaternion by calling {@link #getLatLon}.
*
* @param latitude Angle rotation of latitude.
* @param longitude Angle rotation of longitude.
* @return Quaternion representing combined latitude and longitude rotation.
*/
public static Quaternion fromLatLon(Angle latitude, Angle longitude) {
if (latitude == null || longitude == null) {
throw new IllegalArgumentException("Angle Is Null");
}
double clat = latitude.cosHalfAngle();
double clon = longitude.cosHalfAngle();
double slat = latitude.sinHalfAngle();
double slon = longitude.sinHalfAngle();
// The order in which the lat/lon angles are applied is critical. This can be thought of as
// multiplying two
// quaternions together, one for each lat/lon angle. Like matrices, quaternions affect vectors
// in reverse
// order. For example, suppose we construct a quaternion
// Q = QLat * QLon
// then transform some vector V by Q. This can be thought of as first transforming V by QLat,
// then QLon. This
// means that the order of quaternion multiplication is the reverse of the order in which the
// lat/lon angles
// are applied.
//
// The ordering below refers to order in which angles are applied.
//
// QLat = (0, slat, 0, clat)
// QLon = (slon, 0, 0, clon)
//
// 1. LatLon Ordering
// (QLon * QLat)
// qw = clat * clon;
// qx = clat * slon;
// qy = slat * clon;
// qz = slat * slon;
//
// 2. LonLat Ordering
// (QLat * QLon)
// qw = clat * clon;
// qx = clat * slon;
// qy = slat * clon;
// qz = - slat * slon;
//
double qw = clat * clon;
double qx = clat * slon;
double qy = slat * clon;
double qz = 0.0 - slat * slon;
return new Quaternion(qx, qy, qz, qw);
}
private static Quaternion fromAxisAngle(
Angle angle, double axisX, double axisY, double axisZ, boolean normalize) {
if (angle == null) {
throw new IllegalArgumentException("Angle Is Null");
}
if (normalize) {
double length = Math.sqrt((axisX * axisX) + (axisY * axisY) + (axisZ * axisZ));
if (!isZero(length) && (length != 1.0)) {
axisX /= length;
axisY /= length;
axisZ /= length;
}
}
double s = angle.sinHalfAngle();
double c = angle.cosHalfAngle();
return new Quaternion(axisX * s, axisY * s, axisZ * s, c);
}
