Source/OpenTK/Math/Quaterniond.cs

#region --- License ---
/*
Copyright (c) 2006 - 2008 The Open Toolkit library.

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is furnished to do
so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#endregion

using System;
using System.Runtime.InteropServices;
using System.ComponentModel;
using System.Xml.Serialization;

namespace OpenTK
{
    [Serializable]
    [StructLayout(LayoutKind.Sequential)]
    public struct Quaterniond : IEquatable<Quaterniond>
    {
        #region Fields

        Vector3d xyz;
        double w;

        #endregion

        #region Constructors

        public Quaterniond(Vector3d v, double w)
        {
            this.xyz = v;
            this.w = w;
        }

        public Quaterniond(double x, double y, double z, double w)
            : this(new Vector3d(x, y, z), w)
        { }

        #endregion

        #region Public Members

        #region Properties

        [Obsolete("Use Xyz property instead.")]
        [CLSCompliant(false)]
        [EditorBrowsable(EditorBrowsableState.Never)]
        [XmlIgnore]
        public Vector3d XYZ { get { return Xyz; } set { Xyz = value; } }

        public Vector3d Xyz { get { return xyz; } set { xyz = value; } }

        [XmlIgnore]
        public double X { get { return xyz.X; } set { xyz.X = value; } }

        [XmlIgnore]
        public double Y { get { return xyz.Y; } set { xyz.Y = value; } }

        [XmlIgnore]
        public double Z { get { return xyz.Z; } set { xyz.Z = value; } }

        public double W { get { return w; } set { w = value; } }
        
        #endregion

        #region Instance

        #region ToAxisAngle

        public void ToAxisAngle(out Vector3d axis, out double angle)
        {
            Vector4d result = ToAxisAngle();
            axis = result.Xyz;
            angle = result.W;
        }

        public Vector4d ToAxisAngle()
        {
            Quaterniond q = this;
            if (q.W > 1.0f)
                q.Normalize();

            Vector4d result = new Vector4d();

            result.W = 2.0f * (float)System.Math.Acos(q.W); // angle
            float den = (float)System.Math.Sqrt(1.0 - q.W * q.W);
            if (den > 0.0001f)
            {
                result.Xyz = q.Xyz / den;
            }
            else
            {
                // This occurs when the angle is zero. 
                // Not a problem: just set an arbitrary normalized axis.
                result.Xyz = Vector3d.UnitX;
            }

            return result;
        }

        #endregion

        #region public double Length

        public double Length
        {
            get
            {
                return (double)System.Math.Sqrt(W * W + Xyz.LengthSquared);
            }
        }

        #endregion

        #region public double LengthSquared

        public double LengthSquared
        {
            get
            {
                return W * W + Xyz.LengthSquared;
            }
        }

        #endregion

        #region public void Normalize()

        public void Normalize()
        {
            double scale = 1.0f / this.Length;
            Xyz *= scale;
            W *= scale;
        }

        #endregion

        #region public void Conjugate()

        public void Conjugate()
        {
            Xyz = -Xyz;
        }

        #endregion

        #endregion

        #region Static

        #region Fields

        public readonly static Quaterniond Identity = new Quaterniond(0, 0, 0, 1);

        #endregion

        #region Add

        public static Quaterniond Add(Quaterniond left, Quaterniond right)
        {
            return new Quaterniond(
                left.Xyz + right.Xyz,
                left.W + right.W);
        }

        public static void Add(ref Quaterniond left, ref Quaterniond right, out Quaterniond result)
        {
            result = new Quaterniond(
                left.Xyz + right.Xyz,
                left.W + right.W);
        }

        #endregion

        #region Sub

        public static Quaterniond Sub(Quaterniond left, Quaterniond right)
        {
            return new Quaterniond(
                left.Xyz - right.Xyz,
                left.W - right.W);
        }

        public static void Sub(ref Quaterniond left, ref Quaterniond right, out Quaterniond result)
        {
            result = new Quaterniond(
                left.Xyz - right.Xyz,
                left.W - right.W);
        }

        #endregion

        #region Mult

        [Obsolete("Use Multiply instead.")]
        public static Quaterniond Mult(Quaterniond left, Quaterniond right)
        {
            return new Quaterniond(
                right.W * left.Xyz + left.W * right.Xyz + Vector3d.Cross(left.Xyz, right.Xyz),
                left.W * right.W - Vector3d.Dot(left.Xyz, right.Xyz));
        }

        [Obsolete("Use Multiply instead.")]
        public static void Mult(ref Quaterniond left, ref Quaterniond right, out Quaterniond result)
        {
            result = new Quaterniond(
                right.W * left.Xyz + left.W * right.Xyz + Vector3d.Cross(left.Xyz, right.Xyz),
                left.W * right.W - Vector3d.Dot(left.Xyz, right.Xyz));
        }

        public static Quaterniond Multiply(Quaterniond left, Quaterniond right)
        {
            Quaterniond result;
            Multiply(ref left, ref right, out result);
            return result;
        }

        public static void Multiply(ref Quaterniond left, ref Quaterniond right, out Quaterniond result)
        {
            result = new Quaterniond(
                right.W * left.Xyz + left.W * right.Xyz + Vector3d.Cross(left.Xyz, right.Xyz),
                left.W * right.W - Vector3d.Dot(left.Xyz, right.Xyz));
        }

        public static void Multiply(ref Quaterniond quaternion, double scale, out Quaterniond result)
        {
            result = new Quaterniond(quaternion.X * scale, quaternion.Y * scale, quaternion.Z * scale, quaternion.W * scale);
        }

        public static Quaterniond Multiply(Quaterniond quaternion, double scale)
        {
            return new Quaterniond(quaternion.X * scale, quaternion.Y * scale, quaternion.Z * scale, quaternion.W * scale);
        }

        #endregion

        #region Conjugate

        public static Quaterniond Conjugate(Quaterniond q)
        {
            return new Quaterniond(-q.Xyz, q.W);
        }

        public static void Conjugate(ref Quaterniond q, out Quaterniond result)
        {
            result = new Quaterniond(-q.Xyz, q.W);
        }

        #endregion

        #region Invert

        public static Quaterniond Invert(Quaterniond q)
        {
            Quaterniond result;
            Invert(ref q, out result);
            return result;
        }

        public static void Invert(ref Quaterniond q, out Quaterniond result)
        {
            double lengthSq = q.LengthSquared;
            if (lengthSq != 0.0)
            {
                double i = 1.0f / lengthSq;
                result = new Quaterniond(q.Xyz * -i, q.W * i);
            }
            else
            {
                result = q;
            }
        }

        #endregion

        #region Normalize

        public static Quaterniond Normalize(Quaterniond q)
        {
            Quaterniond result;
            Normalize(ref q, out result);
            return result;
        }

        public static void Normalize(ref Quaterniond q, out Quaterniond result)
        {
            double scale = 1.0f / q.Length;
            result = new Quaterniond(q.Xyz * scale, q.W * scale);
        }

        #endregion

        #region FromAxisAngle

        public static Quaterniond FromAxisAngle(Vector3d axis, double angle)
        {
            if (axis.LengthSquared == 0.0f)
                return Identity;

            Quaterniond result = Identity;

            angle *= 0.5f;
            axis.Normalize();
            result.Xyz = axis * (double)System.Math.Sin(angle);
            result.W = (double)System.Math.Cos(angle);

            return Normalize(result);
        }

        #endregion

        #region Slerp

        public static Quaterniond Slerp(Quaterniond q1, Quaterniond q2, double blend)
        {
            // if either input is zero, return the other.
            if (q1.LengthSquared == 0.0f)
            {
                if (q2.LengthSquared == 0.0f)
                {
                    return Identity;
                }
                return q2;
            }
            else if (q2.LengthSquared == 0.0f)
            {
                return q1;
            }


            double cosHalfAngle = q1.W * q2.W + Vector3d.Dot(q1.Xyz, q2.Xyz);

            if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
            {
                // angle = 0.0f, so just return one input.
                return q1;
            }
            else if (cosHalfAngle < 0.0f)
            {
                q2.Xyz = -q2.Xyz;
                q2.W = -q2.W;
                cosHalfAngle = -cosHalfAngle;
            }

            double blendA;
            double blendB;
            if (cosHalfAngle < 0.99f)
            {
                // do proper slerp for big angles
                double halfAngle = (double)System.Math.Acos(cosHalfAngle);
                double sinHalfAngle = (double)System.Math.Sin(halfAngle);
                double oneOverSinHalfAngle = 1.0f / sinHalfAngle;
                blendA = (double)System.Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle;
                blendB = (double)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
            }
            else
            {
                // do lerp if angle is really small.
                blendA = 1.0f - blend;
                blendB = blend;
            }

            Quaterniond result = new Quaterniond(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);
            if (result.LengthSquared > 0.0f)
                return Normalize(result);
            else
                return Identity;
        }

        #endregion

        #endregion
                
        #region Operators

        public static Quaterniond operator +(Quaterniond left, Quaterniond right)
        {
            left.Xyz += right.Xyz;
            left.W += right.W;
            return left;
        }

        public static Quaterniond operator -(Quaterniond left, Quaterniond right)
        {
            left.Xyz -= right.Xyz;
            left.W -= right.W;
            return left;
        }

        public static Quaterniond operator *(Quaterniond left, Quaterniond right)
        {
            Multiply(ref left, ref right, out left);
            return left;
        }

        public static Quaterniond operator *(Quaterniond quaternion, double scale)
        {
            Multiply(ref quaternion, scale, out quaternion);
            return quaternion;
        }

        public static Quaterniond operator *(double scale, Quaterniond quaternion)
        {
            return new Quaterniond(quaternion.X * scale, quaternion.Y * scale, quaternion.Z * scale, quaternion.W * scale);
        }

        public static bool operator ==(Quaterniond left, Quaterniond right)
        {
            return left.Equals(right);
        }

        public static bool operator !=(Quaterniond left, Quaterniond right)
        {
            return !left.Equals(right);
        }

        #endregion

        #region Overrides

        #region public override string ToString()

        public override string ToString()
        {
            return String.Format("V: {0}, W: {1}", Xyz, W);
        }

        #endregion

        #region public override bool Equals (object o)

        public override bool Equals(object other)
        {
            if (other is Quaterniond == false) return false;
            return this == (Quaterniond)other;
        }

        #endregion

        #region public override int GetHashCode ()

        public override int GetHashCode()
        {
            return Xyz.GetHashCode() ^ W.GetHashCode();
        }

        #endregion

        #endregion

        #endregion

#if false

        #region Fields

        public double W;

        public double X;

        public double Y;

        public double Z;

        #endregion
        
        #region Constructors

        public Quaterniond(ref Quaterniond Quaterniond) : this(Quaterniond.W, Quaterniond.X, Quaterniond.Y, Quaterniond.Z) { }

        public Quaterniond(double w, ref Vector3d vector3d) : this(w, vector3d.X, vector3d.Y, vector3d.Z) { }

        public Quaterniond(ref Vector3d axis, double angle)
        {
            double halfAngle = Functions.DTOR * angle / 2;

            this.W = System.Math.Cos(halfAngle);

            double sin = System.Math.Sin(halfAngle);
            Vector3d axisNormalized;
            Vector3d.Normalize(ref axis, out axisNormalized);
            this.X = axisNormalized.X * sin;
            this.Y = axisNormalized.Y * sin;
            this.Z = axisNormalized.Z * sin;
        }

        public Quaterniond(double w, double x, double y, double z)
        {
            this.W = w;
            this.X = x;
            this.Y = y;
            this.Z = z;
        }

        public Quaterniond(double[] doubleArray)
        {
            if (doubleArray == null || doubleArray.GetLength(0) < 4) throw new MissingFieldException();

            this.W = doubleArray[0];
            this.X = doubleArray[1];
            this.Y = doubleArray[2];
            this.Z = doubleArray[3];
        }

        public Quaterniond(ref Matrix4d matrix)
        {
            double scale = System.Math.Pow(matrix.Determinant, 1.0d/3.0d);
 
            W = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] + matrix[1, 1] + matrix[2, 2])) / 2;
            X = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] - matrix[1, 1] - matrix[2, 2])) / 2;
            Y = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] + matrix[1, 1] - matrix[2, 2])) / 2;
            Z = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] - matrix[1, 1] + matrix[2, 2])) / 2; 
            if( matrix[2,1] - matrix[1,2] < 0 ) X = -X;
            if( matrix[0,2] - matrix[2,0] < 0 ) Y = -Y;
            if( matrix[1,0] - matrix[0,1] < 0 ) Z = -Z;
        }

        public Quaterniond(ref Matrix3d matrix)
        {
            double scale = System.Math.Pow(matrix.Determinant, 1.0d / 3.0d);

            W = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] + matrix[1, 1] + matrix[2, 2])) / 2;
            X = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] - matrix[1, 1] - matrix[2, 2])) / 2;
            Y = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] + matrix[1, 1] - matrix[2, 2])) / 2;
            Z = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] - matrix[1, 1] + matrix[2, 2])) / 2;
            if (matrix[2, 1] - matrix[1, 2] < 0) X = -X;
            if (matrix[0, 2] - matrix[2, 0] < 0) Y = -Y;
            if (matrix[1, 0] - matrix[0, 1] < 0) Z = -Z;
        }

        #endregion

        #region Arithmetic Operators

        public void Add(ref Quaterniond Quaterniond)
        {
            W = W + Quaterniond.W;
            X = X + Quaterniond.X;
            Y = Y + Quaterniond.Y;
            Z = Z + Quaterniond.Z;
        }
        public void Add(ref Quaterniond Quaterniond, out Quaterniond result)
        {
            result.W = W + Quaterniond.W;
            result.X = X + Quaterniond.X;
            result.Y = Y + Quaterniond.Y;
            result.Z = Z + Quaterniond.Z;
        }
        public static void Add(ref Quaterniond left, ref Quaterniond right, out Quaterniond result)
        {
            result.W = left.W + right.W;
            result.X = left.X + right.X;
            result.Y = left.Y + right.Y;
            result.Z = left.Z + right.Z;
        }

        public void Subtract(ref Quaterniond Quaterniond)
        {
            W = W - Quaterniond.W;
            X = X - Quaterniond.X;
            Y = Y - Quaterniond.Y;
            Z = Z - Quaterniond.Z;
        }
        public void Subtract(ref Quaterniond Quaterniond, out Quaterniond result)
        {
            result.W = W - Quaterniond.W;
            result.X = X - Quaterniond.X;
            result.Y = Y - Quaterniond.Y;
            result.Z = Z - Quaterniond.Z;
        }
        public static void Subtract(ref Quaterniond left, ref Quaterniond right, out Quaterniond result)
        {
            result.W = left.W - right.W;
            result.X = left.X - right.X;
            result.Y = left.Y - right.Y;
            result.Z = left.Z - right.Z;
        }

        public void Multiply(ref Quaterniond Quaterniond)
        {
            double w = W * Quaterniond.W - X * Quaterniond.X - Y * Quaterniond.Y - Z * Quaterniond.Z;
            double x = W * Quaterniond.X + X * Quaterniond.W + Y * Quaterniond.Z - Z * Quaterniond.Y;
            double y = W * Quaterniond.Y + Y * Quaterniond.W + Z * Quaterniond.X - X * Quaterniond.Z;
            Z = W * Quaterniond.Z + Z * Quaterniond.W + X * Quaterniond.Y - Y * Quaterniond.X;
            W = w;
            X = x;
            Y = y;
        }
        public void Multiply(ref Quaterniond Quaterniond, out Quaterniond result)
        {
            result.W = W * Quaterniond.W - X * Quaterniond.X - Y * Quaterniond.Y - Z * Quaterniond.Z;
            result.X = W * Quaterniond.X + X * Quaterniond.W + Y * Quaterniond.Z - Z * Quaterniond.Y;
            result.Y = W * Quaterniond.Y + Y * Quaterniond.W + Z * Quaterniond.X - X * Quaterniond.Z;
            result.Z = W * Quaterniond.Z + Z * Quaterniond.W + X * Quaterniond.Y - Y * Quaterniond.X;
        }
        public static void Multiply(ref Quaterniond left, ref Quaterniond right, out Quaterniond result)
        {
            result.W = left.W * right.W - left.X * right.X - left.Y * right.Y - left.Z * right.Z;
            result.X = left.W * right.X + left.X * right.W + left.Y * right.Z - left.Z * right.Y;
            result.Y = left.W * right.Y + left.Y * right.W + left.Z * right.X - left.X * right.Z;
            result.Z = left.W * right.Z + left.Z * right.W + left.X * right.Y - left.Y * right.X;
        }

        public void Multiply(double scalar)
        {
            W = W * scalar;
            X = X * scalar;
            Y = Y * scalar;
            Z = Z * scalar;
        }
        public void Multiply(double scalar, out Quaterniond result)
        {
            result.W = W * scalar;
            result.X = X * scalar;
            result.Y = Y * scalar;
            result.Z = Z * scalar;
        }
        public static void Multiply(ref Quaterniond Quaterniond, double scalar, out Quaterniond result)
        {
            result.W = Quaterniond.W * scalar;
            result.X = Quaterniond.X * scalar;
            result.Y = Quaterniond.Y * scalar;
            result.Z = Quaterniond.Z * scalar;
        }

        public void Divide(double scalar)
        {
            if (scalar == 0) throw new DivideByZeroException();
            W = W / scalar;
            X = X / scalar;
            Y = Y / scalar;
            Z = Z / scalar;
        }
        public void Divide(double scalar, out Quaterniond result)
        {
            if (scalar == 0) throw new DivideByZeroException();
            result.W = W / scalar;
            result.X = X / scalar;
            result.Y = Y / scalar;
            result.Z = Z / scalar;
        }
        public static void Divide(ref Quaterniond Quaterniond, double scalar, out Quaterniond result)
        {
            if (scalar == 0) throw new DivideByZeroException();
            result.W = Quaterniond.W / scalar;
            result.X = Quaterniond.X / scalar;
            result.Y = Quaterniond.Y / scalar;
            result.Z = Quaterniond.Z / scalar;
        }

        #endregion
        
        #region Functions

        public double Modulus
        {
            get
            {
                return System.Math.Sqrt(W * W + X * X + Y * Y + Z * Z);
            }
        }
        public double ModulusSquared
        {
            get
            {
                return W * W + X * X + Y * Y + Z * Z;
            }
        }

        public static double DotProduct(Quaterniond left, Quaterniond right)
        {
            return left.W * right.W + left.X * right.X + left.Y * right.Y + left.Z * right.Z;
        }

        public void Normalize()
        {
            double modulus = System.Math.Sqrt(W * W + X * X + Y * Y + Z * Z);
            if (modulus == 0) throw new DivideByZeroException();
            W = W / modulus;
            X = X / modulus;
            Y = Y / modulus;
            Z = Z / modulus;
        }
        public void Normalize( out Quaterniond result )
        {
            double modulus = System.Math.Sqrt(W * W + X * X + Y * Y + Z * Z);
            if (modulus == 0) throw new DivideByZeroException();
            result.W = W / modulus;
            result.X = X / modulus;
            result.Y = Y / modulus;
            result.Z = Z / modulus;
        }
        public static void Normalize(ref Quaterniond Quaterniond, out Quaterniond result)
        {
            double modulus = System.Math.Sqrt(Quaterniond.W * Quaterniond.W + Quaterniond.X * Quaterniond.X + Quaterniond.Y * Quaterniond.Y + Quaterniond.Z * Quaterniond.Z);
            if (modulus == 0) throw new DivideByZeroException();
            result.W = Quaterniond.W / modulus;
            result.X = Quaterniond.X / modulus;
            result.Y = Quaterniond.Y / modulus;
            result.Z = Quaterniond.Z / modulus;
        }

        public void Conjugate()
        {
            X = -X;
            Y = -Y;
            Z = -Z;
        }
        public void Conjugate( out Quaterniond result )
        {
            result.W = W;
            result.X = -X;
            result.Y = -Y;
            result.Z = -Z;
        }
        public static void Conjugate(ref Quaterniond Quaterniond, out Quaterniond result)
        {
            result.W = Quaterniond.W;
            result.X = -Quaterniond.X;
            result.Y = -Quaterniond.Y;
            result.Z = -Quaterniond.Z;
        }

        public void Inverse()
        {
            double modulusSquared = W * W + X * X + Y * Y + Z * Z;
            if (modulusSquared <= 0) throw new InvalidOperationException();
            double inverseModulusSquared = 1.0 / modulusSquared;
            W = W * inverseModulusSquared;
            X = X * -inverseModulusSquared;
            Y = Y * -inverseModulusSquared;
            Z = Z * -inverseModulusSquared;
        }
        public void Inverse( out Quaterniond result )
        {
            double modulusSquared = W * W + X * X + Y * Y + Z * Z;
            if (modulusSquared <= 0) throw new InvalidOperationException();
            double inverseModulusSquared = 1.0 / modulusSquared;
            result.W = W * inverseModulusSquared;
            result.X = X * -inverseModulusSquared;
            result.Y = Y * -inverseModulusSquared;
            result.Z = Z * -inverseModulusSquared;
        }
        public static void Inverse(ref Quaterniond Quaterniond, out Quaterniond result)
        {
            double modulusSquared = Quaterniond.W * Quaterniond.W + Quaterniond.X * Quaterniond.X + Quaterniond.Y * Quaterniond.Y + Quaterniond.Z * Quaterniond.Z;
            if (modulusSquared <= 0) throw new InvalidOperationException();
            double inverseModulusSquared = 1.0 / modulusSquared;
            result.W = Quaterniond.W * inverseModulusSquared;
            result.X = Quaterniond.X * -inverseModulusSquared;
            result.Y = Quaterniond.Y * -inverseModulusSquared;
            result.Z = Quaterniond.Z * -inverseModulusSquared;
        }

        public void Log()
        {
            if (System.Math.Abs(W) < 1.0)
            {
                double angle = System.Math.Acos(W);
                double sin = System.Math.Sin(angle);

                if (System.Math.Abs(sin) >= 0)
                {
                    double coefficient = angle / sin;
                    X = X * coefficient;
                    Y = Y * coefficient;
                    Z = Z * coefficient;
                }
            }
            else
            {
                X = 0;
                Y = 0;
                Z = 0;
            }

            W = 0;
        }
        public void Log( out Quaterniond result )
        {
            if (System.Math.Abs(W) < 1.0)
            {
                double angle = System.Math.Acos(W);
                double sin = System.Math.Sin(angle);

                if (System.Math.Abs(sin) >= 0)
                {
                    double coefficient = angle / sin;
                    result.X = X * coefficient;
                    result.Y = Y * coefficient;
                    result.Z = Z * coefficient;
                }
                else
                {
                    result.X = X;
                    result.Y = Y;
                    result.Z = Z;
                }
            }
            else
            {
                result.X = 0;
                result.Y = 0;
                result.Z = 0;
            }

            result.W = 0;
        }
        public static void Log(ref Quaterniond Quaterniond, out Quaterniond result)
        {
            if (System.Math.Abs(Quaterniond.W) < 1.0)
            {
                double angle = System.Math.Acos(Quaterniond.W);
                double sin = System.Math.Sin(angle);

                if (System.Math.Abs(sin) >= 0)
                {
                    double coefficient = angle / sin;
                    result.X = Quaterniond.X * coefficient;
                    result.Y = Quaterniond.Y * coefficient;
                    result.Z = Quaterniond.Z * coefficient;
                }
                else
                {
                    result.X = Quaterniond.X;
                    result.Y = Quaterniond.Y;
                    result.Z = Quaterniond.Z;
                }
            }
            else
            {
                result.X = 0;
                result.Y = 0;
                result.Z = 0;
            }

            result.W = 0;
        }

        public void Exp()
        {
            double angle = System.Math.Sqrt(X * X + Y * Y + Z * Z);
            double sin = System.Math.Sin(angle);

            if (System.Math.Abs(sin) > 0)
            {
                double coefficient = angle / sin;
                W = 0;
                X = X * coefficient;
                Y = Y * coefficient;
                Z = Z * coefficient;
            }
            else
            {
                W = 0;
            }
        }
        public void Exp(out Quaterniond result)
        {
            double angle = System.Math.Sqrt(X * X + Y * Y + Z * Z);
            double sin = System.Math.Sin(angle);

            if (System.Math.Abs(sin) > 0)
            {
                double coefficient = angle / sin;
                result.W = 0;
                result.X = X * coefficient;
                result.Y = Y * coefficient;
                result.Z = Z * coefficient;
            }
            else
            {
                result.W = 0;
                result.X = X;
                result.Y = Y;
                result.Z = Z;
            }
        }
        public static void Exp(ref Quaterniond Quaterniond, out Quaterniond result)
        {
            double angle = System.Math.Sqrt(Quaterniond.X * Quaterniond.X + Quaterniond.Y * Quaterniond.Y + Quaterniond.Z * Quaterniond.Z);
            double sin = System.Math.Sin(angle);

            if (System.Math.Abs(sin) > 0)
            {
                double coefficient = angle / sin;
                result.W = 0;
                result.X = Quaterniond.X * coefficient;
                result.Y = Quaterniond.Y * coefficient;
                result.Z = Quaterniond.Z * coefficient;
            }
            else
            {
                result.W = 0;
                result.X = Quaterniond.X;
                result.Y = Quaterniond.Y;
                result.Z = Quaterniond.Z;
            }
        }

        public void Matrix4d(out Matrix4d result)
        {
            // TODO Expand
            result = new Matrix4d(ref this);
        }

        public void GetAxisAndAngle(out Vector3d axis, out double angle)
        {
            Quaterniond Quaterniond;
            Normalize(out Quaterniond);
            double cos = Quaterniond.W;
            angle = System.Math.Acos(cos) * 2 * Functions.RTOD;
            double sin = System.Math.Sqrt( 1.0d - cos * cos );
            if ( System.Math.Abs( sin ) < 0.0001 ) sin = 1;
            axis = new Vector3d(X / sin, Y / sin, Z / sin);
        }

        public static void Slerp(ref Quaterniond start, ref Quaterniond end, double blend, out Quaterniond result)
        {
            if (start.W == 0 && start.X == 0 && start.Y == 0 && start.Z == 0)
            {
                if (end.W == 0 && end.X == 0 && end.Y == 0 && end.Z == 0)
                {
                    result.W = 1;
                    result.X = 0;
                    result.Y = 0;
                    result.Z = 0;
                }
                else
                {
                    result = end;
                }
            }
            else if (end.W == 0 && end.X == 0 && end.Y == 0 && end.Z == 0)
            {
                result = start;
            }

            Vector3d startVector = new Vector3d(start.X, start.Y, start.Z);
            Vector3d endVector = new Vector3d(end.X, end.Y, end.Z);
            double cosHalfAngle = start.W * end.W + Vector3d.Dot(startVector, endVector);

            if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
            {
                // angle = 0.0f, so just return one input.
                result = start;
            }
            else if (cosHalfAngle < 0.0f)
            {
                end.W = -end.W;
                end.X = -end.X;
                end.Y = -end.Y;
                end.Z = -end.Z;
                cosHalfAngle = -cosHalfAngle;
            }

            double blendA;
            double blendB;
            if (cosHalfAngle < 0.99f)
            {
                // do proper slerp for big angles
                double halfAngle = (double)System.Math.Acos(cosHalfAngle);
                double sinHalfAngle = (double)System.Math.Sin(halfAngle);
                double oneOverSinHalfAngle = 1.0f / sinHalfAngle;
                blendA = (double)System.Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle;
                blendB = (double)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
            }
            else
            {
                // do lerp if angle is really small.
                blendA = 1.0f - blend;
                blendB = blend;
            }

            result.W = blendA * start.W + blendB * end.W;
            result.X = blendA * start.X + blendB * end.X;
            result.Y = blendA * start.Y + blendB * end.Y;
            result.Z = blendA * start.Z + blendB * end.Z;

            if (result.W != 0 || result.X != 0 || result.Y != 0 || result.Z != 0)
            {
                result.Normalize();
            }
            else
            {
                result.W = 1;
                result.X = 0;
                result.Y = 0;
                result.Z = 0;
            }
        }

        #endregion
        
        #region HashCode

        public override int GetHashCode()
        {
            base.GetHashCode();
            return W.GetHashCode() ^ X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode();
        }

        #endregion
        
        #region String and Parse

        public override string ToString()
        {
            return string.Format("({0}, {1}, {2}, {3})", W, X, Y, Z);
        }

        public static void Parse(string str, out Quaterniond result)
        {
            Match match = new Regex(@"\((?<w>.*),(?<x>.*),(?<y>.*),(?<z>.*)\)", RegexOptions.None).Match(str);
            if (!match.Success) throw new Exception("Parse failed!");

            result.W = double.Parse(match.Result("${w}"));
            result.X = double.Parse(match.Result("${x}"));
            result.Y = double.Parse(match.Result("${y}"));
            result.Z = double.Parse(match.Result("${z}"));
        }

        #endregion
        
        #region Constants

        public static readonly Quaterniond Zero = new Quaterniond(0, 0, 0, 0);

        public static readonly Quaterniond Identity = new Quaterniond(1, 0, 0, 0);

        public static readonly Quaterniond WAxis = new Quaterniond(1, 0, 0, 0);

        public static readonly Quaterniond XAxis = new Quaterniond(0, 1, 0, 0);

        public static readonly Quaterniond YAxis = new Quaterniond(0, 0, 1, 0);

        public static readonly Quaterniond ZAxis = new Quaterniond(0, 0, 0, 1);

        #endregion

#endif

        #region IEquatable<Quaterniond> Members

        public bool Equals(Quaterniond other)
        {
            return Xyz == other.Xyz && W == other.W;
        }

        #endregion
    }
}

---