Quaternion.cs

//-----------------------------------------------------------------------
// <copyright file="Quaternion.cs" company="Math.NET Project">
//    Copyright (c) 2002-2009, Christoph Rüegg.
//    All Right Reserved.
// </copyright>
// <author>
//    Christoph Rüegg, http://christoph.ruegg.name
// </author>
// <product>
//    Math.NET Iridium, part of the Math.NET Project.
//    http://mathnet.opensourcedotnet.info
// </product>
// <license type="opensource" name="LGPL" version="2 or later">
//    This program is free software; you can redistribute it and/or modify
//    it under the terms of the GNU Lesser General Public License as published 
//    by the Free Software Foundation; either version 2 of the License, or
//    any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU Lesser General Public License for more details.
//
//    You should have received a copy of the GNU Lesser General Public 
//    License along with this program; if not, write to the Free Software
//    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
// </license>
//-----------------------------------------------------------------------

using System;
using System.Text;

namespace MathNet.Numerics
{
    /// <summary>Quaternion Number.</summary>
    /// <remarks>
    /// http://en.wikipedia.org/wiki/Quaternion
    /// http://de.wikipedia.org/wiki/Quaternion
    /// </remarks>
    public struct Quaternion :
        IComparable,
        ICloneable
    {
        /* TODO: testing suite for Quaternion */

        readonly double qw; // real part
        readonly double qx, qy, qz; // imaginary part
        readonly double qabs, qnorm; // norm
        readonly double qarg; // polar notation

        /// <summary>
        /// Initializes a new instance of the Quaternion struct.
        /// </summary>
        public
        Quaternion(
            double real,
            double imagX,
            double imagY,
            double imagZ)
        {
            qx = imagX;
            qy = imagY;
            qz = imagZ;
            qw = real;
            qnorm = ToNorm(real, imagX, imagY, imagZ);
            qabs = Math.Sqrt(qnorm);
            qarg = Math.Acos(real / qabs);
        }

        /// <summary>
        /// Initializes a new instance of the Quaternion struct.
        /// </summary>
        internal
        Quaternion(
            double real,
            double imagX,
            double imagY,
            double imagZ,
            double abs,
            double norm,
            double arg)
        {
            qx = imagX;
            qy = imagY;
            qz = imagZ;
            qw = real;
            qnorm = norm;
            qabs = abs;
            qarg = arg;
        }

        static
        double
        ToNorm(
            double real,
            double imagX,
            double imagY,
            double imagZ)
        {
            return (imagX * imagX)
                + (imagY * imagY)
                + (imagZ * imagZ)
                + (real * real);
        }
        
        static
        Quaternion
        ToUnitQuaternion(
            double real,
            double imagX,
            double imagY, 
            double imagZ)
        {
            double abs = Math.Sqrt(ToNorm(real, imagX, imagY, imagZ));
            return new Quaternion(
                real / abs,
                imagX / abs,
                imagY / abs,
                imagZ / abs,
                1, // abs
                1, // norm
                Math.Acos(real / abs)); // arg
        }

        #region Accessors

        /// <summary>Gets the real part of the quaternion.</summary>
        public double Real
        {
            get { return qw; }
        }

        /// <summary>Gets the imaginary X part (coefficient of complex I) of the quaternion.</summary>
        public double ImagX
        {
            get { return qx; }
        }

        /// <summary>Gets the imaginary Y part (coefficient of complex J) of the quaternion.</summary>
        public double ImagY
        {
            get { return qy; }
        }

        /// <summary>Gets the imaginary Z part (coefficient of complex K) of the quaternion.</summary>
        public double ImagZ
        {
            get { return qz; }
        }

        /// <summary>G
        /// ets the standard euclidean length |q| = sqrt(||q||) of the quaternion q: the square root of the sum of the squares of the four components.
        /// Q may then be represented as q = r*(cos(phi) + u * sin(phi)) = r*exp(phi*u) where u is the unit vector and phi the argument of q.
        /// </summary>
        public double Abs
        {
            get { return qabs; }
        }

        /// <summary>Gets the norm ||q|| = |q|^2 of the quaternion q: the sum of the squares of the four components.</summary>
        public double Norm
        {
            get { return qnorm; }
        }

        /// <summary>Gets the argument phi = arg(q) of the quaternion q, such that q = r*(cos(phi) + u * sin(phi)) = r*exp(phi*u) where r is the absolute and u the unit vector of q.</summary>
        public double Arg
        {
            get { return qarg; }
        }

        /// <summary>True if the quaternion q is of lenght |q| = 1.</summary>
        /// <remarks>To normalize a quaternion to a length of 1, use the <see cref="Sign"/> method. All unit quaternions form a 3-sphere.</remarks>
        public bool IsUnitQuaternion
        {
            get { return Number.AlmostEqual(qabs, 1); }
        }

        /// <summary>
        /// Returns a new Quaternion q with the Scalar part only.
        /// If you need a Double, use the Real-Field instead.
        /// </summary>
        public
        Quaternion
        Scalar()
        {
            return new Quaternion(qw, 0, 0, 0);
        }

        /// <summary>
        /// Returns a new Quaternion q with the Vectorpart only.
        /// </summary>
        public
        Quaternion
        Vector()
        {
            return new Quaternion(0, qx, qy, qz);
        }

        /// <summary>
        /// Returns a new normalized Quaternion u with the Vectorpart only, such that ||u|| = 1.
        /// Q may then be represented as q = r*(cos(phi) + u * sin(phi)) = r*exp(phi*u) where r is the absolute and phi the argument of q.
        /// </summary>
        public
        Quaternion
        UnitVector()
        {
            return ToUnitQuaternion(0, qx, qy, qz);
        }

        /// <summary>
        /// Returns a new normalized Quaternion q with the direction of this quaternion.
        /// </summary>
        public
        Quaternion
        Sign()
        {
            return ToUnitQuaternion(qw, qx, qy, qz);
        }

        /////// <summary>
        /////// Returns a new Quaternion q with the Sign of the components.
        /////// </summary>
        /////// <returns>
        /////// <list type="bullet">
        /////// <item>1 if Positive</item>
        /////// <item>0 if Neutral</item>
        /////// <item>-1 if Negative</item>
        /////// </list>
        /////// </returns>
        ////public Quaternion ComponentSigns()
        ////{
        ////    return new Quaternion(
        ////        Math.Sign(qx),
        ////        Math.Sign(qy),
        ////        Math.Sign(qz),
        ////        Math.Sign(qw));
        ////}

        #endregion

        #region Operators

        /// <summary>
        /// (nop)
        /// </summary>
        public static
        Quaternion
        operator +(Quaternion q)
        {
            return q;
        }

        /// <summary>
        /// Negate a quaternion.
        /// </summary>
        public static
        Quaternion
        operator -(Quaternion q)
        {
            return q.Negate();
        }

        /// <summary>
        /// Add a quaternion to a quaternion.
        /// </summary>
        public static
        Quaternion
        operator +(
            Quaternion q1,
            Quaternion q2)
        {
            return q1.Add(q2);
        }

        /// <summary>
        /// Add a floating point number to a quaternion.
        /// </summary>
        public static
        Quaternion
        operator +(
            Quaternion q1,
            double d)
        {
            return q1.Add(d);
        }

        /// <summary>
        /// Subtract a quaternion from a quaternion.
        /// </summary>
        public static
        Quaternion
        operator -(
            Quaternion q1,
            Quaternion q2)
        {
            return q1.Subtract(q2);
        }

        /// <summary>
        /// Subtract a floating point number from a quaternion.
        /// </summary>
        public static
        Quaternion
        operator -(
            Quaternion q1,
            double d)
        {
            return q1.Subtract(d);
        }

        /// <summary>
        /// Multiplay a quaternion with a quaternion.
        /// </summary>
        public static
        Quaternion
        operator *(
            Quaternion q1,
            Quaternion q2)
        {
            return q1.Multiply(q2);
        }

        /// <summary>
        /// Multiplay a floating point number with a quaternion.
        /// </summary>
        public static
        Quaternion
        operator *(
            Quaternion q1,
            double d)
        {
            return q1.Multiply(d);
        }

        /// <summary>
        /// Divide a quaternion by a quaternion.
        /// </summary>
        public static
        Quaternion
        operator /(
            Quaternion q1,
            Quaternion q2)
        {
            return q1.Divide(q2);
        }

        /// <summary>
        /// Divide a quaternion by a floating point number.
        /// </summary>
        public static
        Quaternion
        operator /(
            Quaternion q1,
            double d)
        {
            return q1.Divide(d);
        }

        /// <summary>
        /// Raise a quaternion to a quaternion.
        /// </summary>
        public static
        Quaternion
        operator ^(
            Quaternion q1,
            Quaternion q2)
        {
            return q1.Pow(q2);
        }

        /// <summary>
        /// Raise a quaternion to a floating point number.
        /// </summary>
        public static
        Quaternion
        operator ^(
            Quaternion q1,
            double d)
        {
            return q1.Pow(d);
        }

        /// <summary>
        /// Convert a floating point number to a quaternion.
        /// </summary>
        public static implicit
        operator Quaternion(double d)
        {
            return new Quaternion(d, 0, 0, 0);
        }

        #endregion

        #region Arithmetic Methods
        
        /// <summary>
        /// Add a quaternion to this quaternion.
        /// </summary>
        public
        Quaternion
        Add(Quaternion q)
        {
            return new Quaternion(
                qw + q.qw,
                qx + q.qx,
                qy + q.qy,
                qz + q.qz);
        }

        /// <summary>
        /// Add a floating point number to this quaternion.
        /// </summary>
        public
        Quaternion
        Add(double r)
        {
            return new Quaternion(
                qw + r,
                qx,
                qy,
                qz);
        }

        /// <summary>
        /// SUbtract a quaternion from this quaternion.
        /// </summary>
        public
        Quaternion
        Subtract(Quaternion q)
        {
            return new Quaternion(
                qw - q.qw,
                qx - q.qx,
                qy - q.qy,
                qz - q.qz);
        }

        /// <summary>
        /// Subtract a floating point number from this quaternion.
        /// </summary>
        public
        Quaternion
        Subtract(double r)
        {
            return new Quaternion(
                qw - r,
                qx,
                qy,
                qz);
        }

        /// <summary>
        /// Negate this quaternion.
        /// </summary>
        public
        Quaternion
        Negate()
        {
            return new Quaternion(
                -qw,
                -qx,
                -qy,
                -qz,
                qabs, // abs
                qnorm, // norm
                Math.PI - qarg); // arg
        }

        /// <summary>
        /// Multiply a quaternion with this quaternion.
        /// </summary>
        public
        Quaternion
        Multiply(Quaternion q)
        {
            double ci = (+qx * q.qw) + (qy * q.qz) - (qz * q.qy) + (qw * q.qx);
            double cj = (-qx * q.qz) + (qy * q.qw) + (qz * q.qx) + (qw * q.qy);
            double ck = (+qx * q.qy) - (qy * q.qx) + (qz * q.qw) + (qw * q.qz);
            double cr = (-qx * q.qx) - (qy * q.qy) - (qz * q.qz) + (qw * q.qw);
            return new Quaternion(cr, ci, cj, ck);
        }

        /// <summary>
        /// Multiply a floating point number to this quaternion.
        /// </summary>
        public
        Quaternion
        Multiply(double d)
        {
            return new Quaternion(
                d * qw,
                d * qx,
                d * qy,
                d * qz);
        }

        /// <summary>
        /// Multiplies a Quaternion with the inverse of another
        /// Quaternion (q*q<sup>-1</sup>). Note that for Quaternions
        /// q*q<sup>-1</sup> is not the same then q<sup>-1</sup>*q,
        /// because this will lead to a rotation in the other direction.
        /// </summary>
        public
        Quaternion
        Divide(Quaternion q)
        {
            return Multiply(q.Inverse());
        }

        /// <summary>
        /// Multiplies a Quaterion with the inverse of a real number.
        /// </summary>
        /// <remarks>
        /// Its also Possible to cast a double to a Quaternion
        /// and make the division afterwards. But this is less
        /// performant.
        /// </remarks>
        public
        Quaternion
        Divide(double d)
        {
            return new Quaternion(
                qw / d,
                qx / d,
                qy / d,
                qz / d);
        }

        /// <summary>
        /// Inverts this quaternion.
        /// </summary>
        public
        Quaternion
        Inverse()
        {
            if(Number.AlmostEqual(qabs, 1))
            {
                return new Quaternion(
                    qw,
                    -qx,
                    -qy,
                    -qz);
            }
            
            return new Quaternion(
                qw / qnorm,
                -qx / qnorm,
                -qy / qnorm,
                -qz / qnorm);
        }

        /// <summary>
        /// Returns the distance |a-b| of two quaternions, forming a metric space.
        /// </summary>
        public static
        double
        Distance(
            Quaternion a,
            Quaternion b)
        {
            return a.Subtract(b).Abs;
        }

        /// <summary>
        /// Conjugate this quaternion.
        /// </summary>
        public
        Quaternion
        Conjugate()
        {
            return new Quaternion(
                qw,
                -qx,
                -qy,
                -qz,
                qabs,
                qnorm,
                qarg);
        }
        
        #endregion

        #region Exponential

        /// <summary>
        /// Logarithm to a given base.
        /// </summary>
        public
        Quaternion
        Log(double lbase)
        {
            return Ln().Divide(Math.Log(lbase));
        }

        /// <summary>
        /// Natural Logrithm to base E.
        /// </summary>
        public
        Quaternion
        Ln()
        {
            return UnitVector().Multiply(qarg).Add(Math.Log(qabs));
        }

        /// <summary>
        /// Common Logarithm to base 10.
        /// </summary>
        public
        Quaternion
        Lg()
        {
            return Ln().Divide(Math.Log(10));
        }

        /// <summary>
        /// Exponential Function.
        /// </summary>
        /// <returns></returns>
        public
        Quaternion
        Exp()
        {
            double vabs = Math.Sqrt(ToNorm(0, qx, qy, qz));
            return UnitVector().Multiply(Math.Sin(vabs)).Add(Math.Cos(vabs)).Multiply(Math.Exp(qw));
        }

        /// <summary>
        /// Raise the quaternion to a given power.
        /// </summary>
        public
        Quaternion
        Pow(double power)
        {
            double arg = power * qarg;
            return UnitVector().Multiply(Math.Sin(arg)).Add(Math.Cos(arg)).Multiply(Math.Pow(qw, power));
        }

        /// <summary>
        /// Raise the quaternion to a given power.
        /// </summary>
        public
        Quaternion
        Pow(Quaternion power)
        {
            return power.Multiply(Ln()).Exp();
        }

        /// <summary>
        /// Square of the Quaternion q: q^2.
        /// </summary>
        public
        Quaternion
        Sqr()
        {
            double arg = qarg * 2;
            return UnitVector().Multiply(Math.Sin(arg)).Add(Math.Cos(arg)).Multiply(qw * qw);
        }

        /// <summary>
        /// Square root of the Quaternion: q^(1/2).
        /// </summary>
        public
        Quaternion
        Sqrt()
        {
            double arg = qarg * 0.5;
            return UnitVector().Multiply(Math.Sin(arg)).Add(Math.Cos(arg)).Multiply(Math.Sqrt(qw));
        }

        #endregion

        #region Trigonometric

        /* TODO: Implement */

        #endregion

        #region String Formatting and Parsing

        /* TODO: Implement */

        #endregion

        #region .NET Integration: Hashing, Equality, Ordering, Cloning

        /// <summary>
        /// Compares this quaternion with another quaternion.
        /// </summary>
        public
        int
        CompareTo(object obj)
        {
            // TODO: Implement
            throw new NotImplementedException();
        }

        /// <summary>
        /// Creates a copy of this quaternion.
        /// </summary>
        public
        object
        Clone()
        {
            // TODO: Implement
            throw new NotImplementedException();
        }

        #endregion
    }
}