quaternion.java

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

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

    // ============== Arithmetic Functions ======================= //
    // ============== Arithmetic Functions ======================= //
    // ============== Arithmetic Functions ======================= //

    public final Quaternion add(Quaternion quaternion)
    {
        if (quaternion == null)
        {
            String msg = Logging.getMessage("nullValue.QuaternionIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        return new Quaternion(
            this.x + quaternion.x,
            this.y + quaternion.y,
            this.z + quaternion.z,
            this.w + quaternion.w);
    }

    public final Quaternion subtract(Quaternion quaternion)
    {
        if (quaternion == null)
        {
            String msg = Logging.getMessage("nullValue.QuaternionIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        return new Quaternion(
            this.x - quaternion.x,
            this.y - quaternion.y,
            this.z - quaternion.z,
            this.w - quaternion.w);
    }

    public final Quaternion multiplyComponents(double value)
    {
        return new Quaternion(
            this.x * value,
            this.y * value,
            this.z * value,
            this.w * value);
    }

    public final Quaternion multiply(Quaternion quaternion)
    {
        if (quaternion == null)
        {
            String msg = Logging.getMessage("nullValue.QuaternionIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        return new Quaternion(
            (this.w * quaternion.x) + (this.x * quaternion.w) + (this.y * quaternion.z) - (this.z * quaternion.y),
            (this.w * quaternion.y) + (this.y * quaternion.w) + (this.z * quaternion.x) - (this.x * quaternion.z),
            (this.w * quaternion.z) + (this.z * quaternion.w) + (this.x * quaternion.y) - (this.y * quaternion.x),
            (this.w * quaternion.w) - (this.x * quaternion.x) - (this.y * quaternion.y) - (this.z * quaternion.z));
    }

    public final Quaternion divideComponents(double value)
    {
        if (isZero(value))
        {
            String msg = Logging.getMessage("generic.ArgumentOutOfRange", value);
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        return new Quaternion(
            this.x / value,
            this.y / value,
            this.z / value,
            this.w / value);
    }

    public final Quaternion divideComponents(Quaternion quaternion)
    {
        if (quaternion == null)
        {
            String msg = Logging.getMessage("nullValue.QuaternionIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        return new Quaternion(
            this.x / quaternion.x,
            this.y / quaternion.y,
            this.z / quaternion.z,
            this.w / quaternion.w);
    }

    public final Quaternion getConjugate()
    {
        return new Quaternion(
            0.0 - this.x,
            0.0 - this.y,
            0.0 - this.z,
            this.w);
    }

    public final Quaternion getNegative()
    {
        return new Quaternion(
            0.0 - this.x,
            0.0 - this.y,
            0.0 - this.z,
            0.0 - this.w);   
    }

    // ============== Geometric Functions ======================= //
    // ============== Geometric Functions ======================= //
    // ============== Geometric Functions ======================= //

    public final double getLength()
    {
        return Math.sqrt(this.getLengthSquared());
    }

    public final double getLengthSquared()
    {
        return (this.x * this.x)
             + (this.y * this.y)
             + (this.z * this.z)
             + (this.w * this.w);
    }

    public final Quaternion normalize()
    {
        double length = this.getLength();
        // Vector has zero length.
        if (isZero(length))
        {
            return this;
        }
        else
        {
            return new Quaternion(
                this.x / length,
                this.y / length,
                this.z / length,
                this.w / length);
        }
    }

    public final double dot(Quaternion quaternion)
    {
        if (quaternion == null)
        {
            String msg = Logging.getMessage("nullValue.QuaternionIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        return (this.x * quaternion.x) + (this.y * quaternion.y) + (this.z * quaternion.z) + (this.w * quaternion.w);
    }

    public final Quaternion getInverse()
    {
        double length = this.getLength();
        // Vector has zero length.
        if (isZero(length))
        {
            return this;
        }
        else
        {
            return new Quaternion(
                (0.0 - this.x) / length,
                (0.0 - this.y) / length,
                (0.0 - this.z) / length,
                this.w / length);
        }
    }

    // ============== Mixing Functions ======================= //
    // ============== Mixing Functions ======================= //
    // ============== Mixing Functions ======================= //

    public static Quaternion mix(double amount, Quaternion value1, Quaternion value2)
    {
        if ((value1 == null) || (value2 == null))
        {
            String msg = Logging.getMessage("nullValue.QuaternionIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        if (amount < 0.0)
            return value1;
        else if (amount > 1.0)
            return value2;

        double t1 = 1.0 - amount;
        return new Quaternion(
            (value1.x * t1) + (value2.x * amount),
            (value1.y * t1) + (value2.y * amount),
            (value1.z * t1) + (value2.z * amount),
            (value1.w * t1) + (value2.w * amount));
    }

    public static Quaternion slerp(double amount, Quaternion value1, Quaternion value2)
    {
        if ((value1 == null) || (value2 == null))
        {
            String msg = Logging.getMessage("nullValue.QuaternionIsNull");
            Logging.logger().severe(msg);
            throw new IllegalArgumentException(msg);
        }

        if (amount < 0.0)
            return value1;
        else if (amount > 1.0)
            return value2;

        double dot = value1.dot(value2);
        double x2, y2, z2, w2;
        if (dot < 0.0)
        {
            dot = 0.0 - dot;
            x2 = 0.0 - value2.x;
            y2 = 0.0 - value2.y;
            z2 = 0.0 - value2.z;
            w2 = 0.0 - value2.w;
        }
        else
        {
            x2 = value2.x;
            y2 = value2.y;
            z2 = value2.z;
            w2 = value2.w;
        }

        double t1, t2;

        final double EPSILON = 0.0001;
        if ((1.0 - dot) > EPSILON) // standard case (slerp)
        {
            double angle = Math.acos(dot);
            double sinAngle = Math.sin(angle);
            t1 = Math.sin((1.0 - amount) * angle) / sinAngle;
            t2 = Math.sin(amount * angle) / sinAngle;
        }
        else // just lerp
        {
            t1 = 1.0 - amount;
            t2 = amount;
        }

        return new Quaternion(
            (value1.x * t1) + (x2 * t2),
            (value1.y * t1) + (y2 * t2),
            (value1.z * t1) + (z2 * t2),
            (value1.w * t1) + (w2 * t2));
    }

    // ============== Accessor Functions ======================= //
    // ============== Accessor Functions ======================= //
    // ============== Accessor Functions ======================= //

    public final Angle getAngle()
    {
        double w = this.w;

        double length = this.getLength();
        if (!isZero(length) && (length != 1.0))
            w /= length;

        double radians = 2.0 * Math.acos(w);
        if (Double.isNaN(radians))
            return null;

        return Angle.fromRadians(radians);
    }

    public final Vec4 getAxis()
    {
        double x = this.x;
        double y = this.y;
        double z = this.z;

        double length = this.getLength();
        if (!isZero(length) && (length != 1.0))
        {
            x /= length;
            y /= length;
            z /= length;
        }

        double vecLength = Math.sqrt((x * x) + (y * y) + (z * z));
        if (!isZero(vecLength) && (vecLength != 1.0))
        {
            x /= vecLength;
            y /= vecLength;
            z /= vecLength;
        }

        return new Vec4(x, y, z);
    }

    public final Angle getRotationX()
    {
        double radians = Math.atan2((2.0 * this.x * this.w) - (2.0 * this.y * this.z),
                                    1.0 - 2.0 * (this.x * this.x) - 2.0 * (this.z * this.z));
        if (Double.isNaN(radians))
            return null;

        return Angle.fromRadians(radians);
    }

    public final Angle getRotationY()
    {
        double radians = Math.atan2((2.0 * this.y * this.w) - (2.0 * this.x * this.z),
                                    1.0 - (2.0 * this.y * this.y) - (2.0 * this.z * this.z));
        if (Double.isNaN(radians))
            return null;

        return Angle.fromRadians(radians);
    }

    public final Angle getRotationZ()
    {
        double radians = Math.asin((2.0 * this.x * this.y) + (2.0 * this.z * this.w));
        if (Double.isNaN(radians))
            return null;

        return Angle.fromRadians(radians);
    }

    public final LatLon getLatLon()
    {
        double latRadians = Math.asin((2.0 * this.y * this.w) - (2.0 * this.x * this.z));
        double lonRadians = Math.atan2((2.0 * this.y * this.z) + (2.0 * this.x * this.w),
                                       (this.w * this.w) - (this.x * this.x) - (this.y * this.y) + (this.z * this.z));
        if (Double.isNaN(latRadians) || Double.isNaN(lonRadians))
            return null;

        return LatLon.fromRadians(latRadians, lonRadians);
    }

    // ============== Helper Functions ======================= //
    // ============== Helper Functions ======================= //
    // ============== Helper Functions ======================= //

    private static final Double PositiveZero = +0.0d;

    private static final Double NegativeZero = -0.0d;

    private static boolean isZero(double value)
    {
        return (PositiveZero.compareTo(value) == 0)
            || (NegativeZero.compareTo(value) == 0);
    }
}