quaternion.java

来自「world wind java sdk 源码」· Java 代码 · 共 752 行 · 第 1/2 页

/*
Copyright (C) 2001, 2006 United States Government
as represented by the Administrator of the
National Aeronautics and Space Administration.
All Rights Reserved.
*/
package gov.nasa.worldwind.geom;

import gov.nasa.worldwind.util.Logging;

/**
 * @author Chris Maxwell
 * @version $Id: Quaternion.java 5250 2008-05-01 15:33:31Z dcollins $
 */
public class Quaternion
{
    // Multiplicative identity quaternion.
    public static final Quaternion IDENTITY = new Quaternion(0, 0, 0, 1);

    public final double x;
    public final double y;
    public final double z;
    public final double w;

    // 4 values in a quaternion.
    private static final int NUM_ELEMENTS = 4;
    // Cached computations.
    private int hashCode;

    public Quaternion(double x, double y, double z, double w)
    {
        this.x = x;
        this.y = y;
        this.z = z;
        this.w = w;
    }

    public final boolean equals(Object obj)
    {
        if (this == obj)
            return true;
        if (obj == null || obj.getClass() != this.getClass())
            return false;

        Quaternion that = (Quaternion) obj;
        return (this.x == that.x)
            && (this.y == that.y)
            && (this.z == that.z)
            && (this.w == that.w);
    }

    public final int hashCode()
    {
        if (this.hashCode == 0)
        {
            int result;
            long tmp;
            tmp = Double.doubleToLongBits(this.x);
            result = (int) (tmp ^ (tmp >>> 32));
            tmp = Double.doubleToLongBits(this.y);
            result = 31 * result + (int) (tmp ^ (tmp >>> 32));
            tmp = Double.doubleToLongBits(this.z);
            result = 31 * result + (int) (tmp ^ (tmp >>> 32));
            tmp = Double.doubleToLongBits(this.w);
            result = 31 * result + (int) (tmp ^ (tmp >>> 32));
            this.hashCode = result;
        }
        return this.hashCode;
    }

    public static Quaternion fromArray(double[] compArray, int offset)
    {
        if (compArray == null)
        {
            String msg = Logging.getMessage("nullValue.ArrayIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }
        if ((compArray.length - offset) < NUM_ELEMENTS)
        {
            String msg = Logging.getMessage("generic.ArrayInvalidLength", compArray.length);
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        //noinspection PointlessArithmeticExpression                
        return new Quaternion(
            compArray[0 + offset],
            compArray[1 + offset],
            compArray[2 + offset],
            compArray[3 + offset]);
    }

    public final double[] toArray(double[] compArray, int offset)
    {
        if (compArray == null)
        {
            String msg = Logging.getMessage("nullValue.ArrayIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }
        if ((compArray.length - offset) < NUM_ELEMENTS)
        {
            String msg = Logging.getMessage("generic.ArrayInvalidLength", compArray.length);
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }
        
        //noinspection PointlessArithmeticExpression
        compArray[0 + offset] = this.x;
        compArray[1 + offset] = this.y;
        compArray[2 + offset] = this.z;
        compArray[3 + offset] = this.w;
        return compArray;
    }

    public final String toString()
    {
        StringBuilder sb = new StringBuilder();
        sb.append("(");
        sb.append(this.x).append(", ");
        sb.append(this.y).append(", ");
        sb.append(this.z).append(", ");
        sb.append(this.w);
        sb.append(")");
        return sb.toString();
    }

    public final double getX()
    {
        return this.x;
    }

    public final double getY()
    {
        return this.y;
    }

    public final double getZ()
    {
        return this.z;
    }

    public final double getW()
    {
        return this.w;
    }

    public final double x()
    {
        return this.x;
    }

    public final double y()
    {
        return this.y;
    }

    public final double z()
    {
        return this.z;
    }

    public final double w()
    {
        return this.w;
    }

    // ============== Factory Functions ======================= //
    // ============== Factory Functions ======================= //
    // ============== Factory Functions ======================= //

    public static Quaternion fromAxisAngle(Angle angle, Vec4 axis)
    {
        if (angle == null)
        {
            String msg = Logging.getMessage("nullValue.AngleIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }
        if (axis == null)
        {
            String msg = Logging.getMessage("nullValue.Vec4IsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        return fromAxisAngle(angle, axis.x, axis.y, axis.z, true);
    }

    public static Quaternion fromAxisAngle(Angle angle, double axisX, double axisY, double axisZ)
    {
        if (angle == null)
        {
            String msg = Logging.getMessage("nullValue.AngleIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }
        return fromAxisAngle(angle, axisX, axisY, axisZ, true);
    }

    private static Quaternion fromAxisAngle(Angle angle, double axisX, double axisY, double axisZ, boolean normalize)
    {
        if (angle == null)
        {
            String msg = Logging.getMessage("nullValue.AngleIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        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);
    }

    public static Quaternion fromMatrix(Matrix matrix)
    {
        if (matrix == null)
        {
            String msg = Logging.getMessage("nullValue.MatrixIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }
        
        double t = 1.0 + matrix.m11 + matrix.m22 + matrix.m33;
        double x, y, z, w;
        double s;
        final double EPSILON = 0.00000001;
        if (t > EPSILON)
        {
			s = 2.0 * Math.sqrt(t);
			x = (matrix.m32 - matrix.m23) / s;
            y = (matrix.m13 - matrix.m31) / s;
            z = (matrix.m21 - matrix.m12) / s;
            w = s / 4.0;
        }
        else if ((matrix.m11 > matrix.m22) && (matrix.m11 > matrix.m33))
        {
			s = 2.0 * Math.sqrt(1.0 + matrix.m11 - matrix.m22 - matrix.m33);
			x = s / 4.0;
			y = (matrix.m21 + matrix.m12) / s;
			z = (matrix.m13 + matrix.m31) / s;
			w = (matrix.m32 - matrix.m23) / s;
		}
        else if (matrix.m22 > matrix.m33)
        {
			s = 2.0 * Math.sqrt(1.0 + matrix.m22 - matrix.m11 - matrix.m33);
			x = (matrix.m21 + matrix.m12) / s;
			y = s / 4.0;
			z = (matrix.m32 + matrix.m23) / s;
			w = (matrix.m13 - matrix.m31) / s;
		}
        else
        {
			s = 2.0 * Math.sqrt(1.0 + matrix.m33 - matrix.m11 - matrix.m22);
			x = (matrix.m13 + matrix.m31) / s;
			y = (matrix.m32 + matrix.m23) / s;
			z = s / 4.0;
			w = (matrix.m21 - matrix.m12) / s;
		}
        return new Quaternion(x, y, z, w);
    }

    /**
     * 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)
        {
            String msg = Logging.getMessage("nullValue.AngleIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        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)
        {
            String msg = Logging.getMessage("nullValue.AngleIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        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;