Quaternion.cs source code in C# .NET

Source code for the .NET framework in C#

                        

Code:

/ Dotnetfx_Vista_SP2 / Dotnetfx_Vista_SP2 / 8.0.50727.4016 / DEVDIV / depot / DevDiv / releases / Orcas / QFE / wpf / src / Core / CSharp / System / Windows / Media3D / Quaternion.cs / 1 / Quaternion.cs

                            //---------------------------------------------------------------------------- 
//
// 
//    Copyright (C) Microsoft Corporation.  All rights reserved.
//  
//
// 
// Description: 3D quaternion implementation. 
//
//              See spec at http://avalon/medialayer/Specifications/Avalon3D%20API%20Spec.mht 
//
// History:
//  06/02/2003 : t-gregr - Created
// 
// NOTE:  The field _isNotDistinguishedIdentity is a work-around to the
//        problem that we can't define a default constructor that sets 
//        _w to 1.  So the default constructor sets all fields to 0 or 
//        false, and we interpret _isNotDistinguishedIdentity as follows
// 
//        If false, the quaternion is the identity 0,0,0,1 even though
//        the member fields are 0,0,0,0.
//
//        If true, the quaternion has the value given by its member fields. 
//
//        Don't mess it up! 
//--------------------------------------------------------------------------- 

using MS.Internal; 
using System;
using System.Collections;
using System.ComponentModel;
using System.Diagnostics; 
using System.Globalization;
using System.Reflection; 
using System.Runtime.InteropServices; 
using System.ComponentModel.Design.Serialization;
using System.Windows.Markup; 
using System.Windows.Media;
using System.Windows.Media.Animation;
using System.Windows.Media.Composition;
// These types are aliased to match the unamanaged names used in interop 
using BOOL = System.Boolean;
using WORD = System.UInt16; 
using Float = System.Single; 

using SR=MS.Internal.PresentationCore.SR; 
using SRID=MS.Internal.PresentationCore.SRID;

namespace System.Windows.Media.Media3D
{ 

    ///  
    /// Quaternions. 
    /// Quaternions are distinctly 3D entities that represent rotation in three dimensions.
    /// Their power comes in being able to interpolate (and thus animate) between 
    /// quaternions to achieve a smooth, reliable interpolation.
    /// The default quaternion is the identity.
    /// 
    public partial struct Quaternion : IFormattable 
    {
        //----------------------------------------------------- 
        // 
        //  Constructors
        // 
        //-----------------------------------------------------

        #region Constructors
 
        /// 
        /// Constructor that sets quaternion's initial values. 
        ///  
        /// Value of the X coordinate of the new quaternion.
        /// Value of the Y coordinate of the new quaternion. 
        /// Value of the Z coordinate of the new quaternion.
        /// Value of the W coordinate of the new quaternion.
        public Quaternion(double x, double y, double z, double w)
        { 
            _x = x;
            _y = y; 
            _z = z; 
            _w = w;
            _isNotDistinguishedIdentity = true; 
        }

        /// 
        /// Constructs a quaternion via specified axis of rotation and an angle. 
        /// Throws an InvalidOperationException if given (0,0,0) as axis vector.
        ///  
        /// Vector representing axis of rotation. 
        /// Angle to turn around the given axis (in degrees).
        public Quaternion(Vector3D axisOfRotation, double angleInDegrees) 
        {
            angleInDegrees %= 360.0; // Doing the modulo before converting to radians reduces total error
            double angleInRadians = angleInDegrees * (Math.PI / 180.0);
            double length = axisOfRotation.Length; 
            if (length == 0)
                throw new System.InvalidOperationException(SR.Get(SRID.Quaternion_ZeroAxisSpecified)); 
            Vector3D v = (axisOfRotation / length) * Math.Sin(0.5 * angleInRadians); 
            _x = v.X;
            _y = v.Y; 
            _z = v.Z;
            _w = Math.Cos(0.5 * angleInRadians);
            _isNotDistinguishedIdentity = true;
        } 

        #endregion Constructors 
 

        //------------------------------------------------------ 
        //
        //  Public Methods
        //
        //----------------------------------------------------- 

        #region Public Methods 
        ///  
        ///     Identity quaternion
        ///  
        public static Quaternion Identity
        {
            get
            { 
                return s_identity;
            } 
        } 

        ///  
        /// Retrieves quaternion's axis.
        /// 
        public Vector3D Axis
        { 
            // q = M [cos(Q/2), sin(Q /2)v]
            // axis = sin(Q/2)v 
            // angle = cos(Q/2) 
            // M is magnitude
            get 
            {
                // Handle identity (where axis is indeterminate) by
                // returning arbitrary axis.
                if (IsDistinguishedIdentity || (_x == 0 && _y == 0 && _z == 0)) 
                {
                    return new Vector3D(0,1,0); 
                } 
                else
                { 
                    Vector3D v = new Vector3D(_x, _y, _z);
                    v.Normalize();
                    return v;
                } 
            }
        } 
 
        /// 
        /// Retrieves quaternion's angle. 
        /// 
        public double Angle
        {
            get 
            {
                if (IsDistinguishedIdentity) 
                { 
                    return 0;
                } 

                // Magnitude of quaternion times sine and cosine
                double msin = Math.Sqrt(_x*_x + _y*_y + _z*_z);
                double mcos = _w; 

                if (!(msin <= Double.MaxValue)) 
                { 
                    // Overflowed probably in squaring, so let's scale
                    // the values.  We don't need to include _w in the 
                    // scale factor because we're not going to square
                    // it.
                    double maxcoeff = Math.Max(Math.Abs(_x),Math.Max(Math.Abs(_y),Math.Abs(_z)));
                    double x = _x/maxcoeff; 
                    double y = _y/maxcoeff;
                    double z = _z/maxcoeff; 
                    msin = Math.Sqrt(x*x + y*y + z*z); 
                    // Scale mcos too.
                    mcos = _w/maxcoeff; 
                }

                // Atan2 is better than acos.  (More precise and more efficient.)
                return Math.Atan2(msin,mcos) * (360.0 / Math.PI); 
            }
        } 
 
        /// 
        /// Returns whether the quaternion is normalized (i.e. has a magnitude of 1). 
        /// 
        public bool IsNormalized
        {
            get 
            {
                if (IsDistinguishedIdentity) 
                { 
                    return true;
                } 
                double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
                return DoubleUtil.IsOne(norm2);
            }
        } 

        ///  
        /// Tests whether or not a given quaternion is an identity quaternion. 
        /// 
        public bool IsIdentity 
        {
            get
            {
                return IsDistinguishedIdentity || (_x == 0 && _y == 0 && _z == 0 && _w == 1); 
            }
        } 
 
        /// 
        /// Relaces quaternion with its conjugate 
        /// 
        public void Conjugate()
        {
            if (IsDistinguishedIdentity) 
            {
                return; 
            } 

            // Conjugate([x,y,z,w]) = [-x,-y,-z,w] 
            _x = -_x;
            _y = -_y;
            _z = -_z;
        } 

        ///  
        /// Replaces quaternion with its inverse 
        /// 
        public void Invert() 
        {
            if (IsDistinguishedIdentity)
            {
                return; 
            }
 
            // Inverse = Conjugate / Norm Squared 
            Conjugate();
            double norm2 = _x*_x + _y*_y + _z*_z + _w*_w; 
            _x /= norm2;
            _y /= norm2;
            _z /= norm2;
            _w /= norm2; 
        }
 
        ///  
        /// Normalizes this quaternion.
        ///  
        public void Normalize()
        {
            if (IsDistinguishedIdentity)
            { 
                return;
            } 
 
            double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
            if (norm2 > Double.MaxValue) 
            {
                // Handle overflow in computation of norm2
                double rmax = 1.0/Max(Math.Abs(_x),
                                      Math.Abs(_y), 
                                      Math.Abs(_z),
                                      Math.Abs(_w)); 
 
                _x *= rmax;
                _y *= rmax; 
                _z *= rmax;
                _w *= rmax;
                norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
            } 
            double normInverse = 1.0 / Math.Sqrt(norm2);
            _x *= normInverse; 
            _y *= normInverse; 
            _z *= normInverse;
            _w *= normInverse; 
        }

        /// 
        /// Quaternion addition. 
        /// 
        /// First quaternion being added. 
        /// Second quaternion being added. 
        /// Result of addition.
        public static Quaternion operator +(Quaternion left, Quaternion right) 
        {
            if (right.IsDistinguishedIdentity)
            {
                if (left.IsDistinguishedIdentity) 
                {
                    return new Quaternion(0,0,0,2); 
                } 
                else
                { 
                    // We know left is not distinguished identity here.
                    left._w += 1;
                    return left;
                } 
            }
            else if (left.IsDistinguishedIdentity) 
            { 
                // We know right is not distinguished identity here.
                right._w += 1; 
                return right;
            }
            else
            { 
                return new Quaternion(left._x + right._x,
                                      left._y + right._y, 
                                      left._z + right._z, 
                                      left._w + right._w);
            } 
        }

        /// 
        /// Quaternion addition. 
        /// 
        /// First quaternion being added. 
        /// Second quaternion being added. 
        /// Result of addition.
        public static Quaternion Add(Quaternion left, Quaternion right) 
        {
            return (left + right);
        }
 
        /// 
        /// Quaternion subtraction. 
        ///  
        /// Quaternion to subtract from.
        /// Quaternion to subtract from the first quaternion. 
        /// Result of subtraction.
        public static Quaternion operator -(Quaternion left, Quaternion right)
        {
            if (right.IsDistinguishedIdentity) 
            {
                if (left.IsDistinguishedIdentity) 
                { 
                    return new Quaternion(0,0,0,0);
                } 
                else
                {
                    // We know left is not distinguished identity here.
                    left._w -= 1; 
                    return left;
                } 
            } 
            else if (left.IsDistinguishedIdentity)
            { 
                // We know right is not distinguished identity here.
                return new Quaternion(-right._x, -right._y, -right._z, 1 - right._w);
            }
            else 
            {
                return new Quaternion(left._x - right._x, 
                                      left._y - right._y, 
                                      left._z - right._z,
                                      left._w - right._w); 
            }
        }

        ///  
        /// Quaternion subtraction.
        ///  
        /// Quaternion to subtract from. 
        /// Quaternion to subtract from the first quaternion.
        /// Result of subtraction. 
        public static Quaternion Subtract(Quaternion left, Quaternion right)
        {
            return (left - right);
        } 

        ///  
        /// Quaternion multiplication. 
        /// 
        /// First quaternion. 
        /// Second quaternion.
        /// Result of multiplication.
        public static Quaternion operator *(Quaternion left, Quaternion right)
        { 
            if (left.IsDistinguishedIdentity)
            { 
                return right; 
            }
            if (right.IsDistinguishedIdentity) 
            {
                return left;
            }
 
            double x = left._w * right._x + left._x * right._w + left._y * right._z - left._z * right._y;
            double y = left._w * right._y + left._y * right._w + left._z * right._x - left._x * right._z; 
            double z = left._w * right._z + left._z * right._w + left._x * right._y - left._y * right._x; 
            double w = left._w * right._w - left._x * right._x - left._y * right._y - left._z * right._z;
            Quaternion result = new Quaternion(x,y,z,w); 
            return result;

        }
 
        /// 
        /// Quaternion multiplication. 
        ///  
        /// First quaternion.
        /// Second quaternion. 
        /// Result of multiplication.
        public static Quaternion Multiply(Quaternion left, Quaternion right)
        {
            return left * right; 
        }
 
        ///  
        /// Scale this quaternion by a scalar.
        ///  
        /// Value to scale by.
        private void Scale( double scale )
        {
            if (IsDistinguishedIdentity) 
            {
                _w = scale; 
                IsDistinguishedIdentity = false; 
                return;
            } 
            _x *= scale;
            _y *= scale;
            _z *= scale;
            _w *= scale; 
        }
 
        ///  
        /// Return length of quaternion.
        ///  
        private double Length()
        {
            if (IsDistinguishedIdentity)
            { 
                return 1;
            } 
 
            double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
            if (!(norm2 <= Double.MaxValue)) 
            {
                // Do this the slow way to avoid squaring large
                // numbers since the length of many quaternions is
                // representable even if the squared length isn't.  Of 
                // course some lengths aren't representable because
                // the length can be up to twice as big as the largest 
                // coefficient. 

                double max = Math.Max(Math.Max(Math.Abs(_x),Math.Abs(_y)), 
                                      Math.Max(Math.Abs(_z),Math.Abs(_w)));

                double x = _x/max;
                double y = _y/max; 
                double z = _z/max;
                double w = _w/max; 
 
                double smallLength = Math.Sqrt(x*x+y*y+z*z+w*w);
                // Return length of this smaller vector times the scale we applied originally. 
                return smallLength * max;
            }
            return Math.Sqrt(norm2);
        } 

        ///  
        /// Smoothly interpolate between the two given quaternions using Spherical 
        /// Linear Interpolation (SLERP).
        ///  
        /// First quaternion for interpolation.
        /// Second quaternion for interpolation.
        /// Interpolation coefficient.
        /// SLERP-interpolated quaternion between the two given quaternions. 
        public static Quaternion Slerp(Quaternion from, Quaternion to, double t)
        { 
            return Slerp(from, to, t, /* useShortestPath = */ true); 
        }
 
        /// 
        /// Smoothly interpolate between the two given quaternions using Spherical
        /// Linear Interpolation (SLERP).
        ///  
        /// First quaternion for interpolation.
        /// Second quaternion for interpolation. 
        /// Interpolation coefficient. 
        /// If true, Slerp will automatically flip the sign of
        ///     the destination Quaternion to ensure the shortest path is taken. 
        /// SLERP-interpolated quaternion between the two given quaternions.
        public static Quaternion Slerp(Quaternion from, Quaternion to, double t, bool useShortestPath)
        {
            if (from.IsDistinguishedIdentity) 
            {
                from._w = 1; 
            } 
            if (to.IsDistinguishedIdentity)
            { 
                to._w = 1;
            }

            double cosOmega; 
            double scaleFrom, scaleTo;
 
            // Normalize inputs and stash their lengths 
            double lengthFrom = from.Length();
            double lengthTo = to.Length(); 
            from.Scale(1/lengthFrom);
            to.Scale(1/lengthTo);

            // Calculate cos of omega. 
            cosOmega = from._x*to._x + from._y*to._y + from._z*to._z + from._w*to._w;
 
            if (useShortestPath) 
            {
                // If we are taking the shortest path we flip the signs to ensure that 
                // cosOmega will be positive.
                if (cosOmega < 0.0)
                {
                    cosOmega = -cosOmega; 
                    to._x = -to._x;
                    to._y = -to._y; 
                    to._z = -to._z; 
                    to._w = -to._w;
                } 
            }
            else
            {
                // If we are not taking the UseShortestPath we clamp cosOmega to 
                // -1 to stay in the domain of Math.Acos below.
                if (cosOmega < -1.0) 
                { 
                    cosOmega = -1.0;
                } 
            }

            // Clamp cosOmega to [-1,1] to stay in the domain of Math.Acos below.
            // The logic above has either flipped the sign of cosOmega to ensure it 
            // is positive or clamped to -1 aready.  We only need to worry about the
            // upper limit here. 
            if (cosOmega > 1.0) 
            {
                cosOmega = 1.0; 
            }

            Debug.Assert(!(cosOmega < -1.0) && !(cosOmega > 1.0),
                "cosOmega should be clamped to [-1,1]"); 

            // The mainline algorithm doesn't work for extreme 
            // cosine values.  For large cosine we have a better 
            // fallback hence the asymmetric limits.
            const double maxCosine = 1.0 - 1e-6; 
            const double minCosine = 1e-10 - 1.0;

            // Calculate scaling coefficients.
            if (cosOmega > maxCosine) 
            {
                // Quaternions are too close - use linear interpolation. 
                scaleFrom = 1.0 - t; 
                scaleTo = t;
            } 
            else if (cosOmega < minCosine)
            {
                // Quaternions are nearly opposite, so we will pretend to
                // is exactly -from. 
                // First assign arbitrary perpendicular to "to".
                to = new Quaternion(-from.Y, from.X, -from.W, from.Z); 
 
                double theta = t * Math.PI;
 
                scaleFrom = Math.Cos(theta);
                scaleTo = Math.Sin(theta);
            }
            else 
            {
                // Standard case - use SLERP interpolation. 
                double omega = Math.Acos(cosOmega); 
                double sinOmega = Math.Sqrt(1.0 - cosOmega*cosOmega);
                scaleFrom = Math.Sin((1.0 - t) * omega) / sinOmega; 
                scaleTo = Math.Sin(t * omega) / sinOmega;
            }

            // We want the magnitude of the output quaternion to be 
            // multiplicatively interpolated between the input
            // magnitudes, i.e. lengthOut = lengthFrom * (lengthTo/lengthFrom)^t 
            //                            = lengthFrom ^ (1-t) * lengthTo ^ t 

            double lengthOut = lengthFrom * Math.Pow(lengthTo/lengthFrom, t); 
            scaleFrom *= lengthOut;
            scaleTo *= lengthOut;

            return new Quaternion(scaleFrom*from._x + scaleTo*to._x, 
                                  scaleFrom*from._y + scaleTo*to._y,
                                  scaleFrom*from._z + scaleTo*to._z, 
                                  scaleFrom*from._w + scaleTo*to._w); 
        }
 
        #endregion Public Methods

        #region Private Methods
 
        static private double Max(double a, double b, double c, double d)
        { 
            if (b > a) 
                a = b;
            if (c > a) 
                a = c;
            if (d > a)
                a = d;
            return a; 
        }
 
        #endregion Private Methods 

        //------------------------------------------------------ 
        //
        //  Public Properties
        //
        //------------------------------------------------------ 

        #region Public Properties 
 
        /// 
        /// X - Default value is 0. 
        /// 
        public double X
        {
            get 
            {
                return _x; 
            } 

            set 
            {
                if (IsDistinguishedIdentity)
                {
                    this = s_identity; 
                    IsDistinguishedIdentity = false;
                } 
                _x = value; 
            }
        } 

        /// 
        /// Y - Default value is 0.
        ///  
        public double Y
        { 
            get 
            {
                return _y; 
            }

            set
            { 
                if (IsDistinguishedIdentity)
                { 
                    this = s_identity; 
                    IsDistinguishedIdentity = false;
                } 
                _y = value;
            }
        }
 
        /// 
        /// Z - Default value is 0. 
        ///  
        public double Z
        { 
            get
            {
                return _z;
            } 

            set 
            { 
                if (IsDistinguishedIdentity)
                { 
                    this = s_identity;
                    IsDistinguishedIdentity = false;
                }
                _z = value; 
            }
        } 
 
        /// 
        /// W - Default value is 1. 
        /// 
        public double W
        {
            get 
            {
                if (IsDistinguishedIdentity) 
                { 
                    return 1.0;
                } 
                else
                {
                    return _w;
                } 
            }
 
            set 
            {
                if (IsDistinguishedIdentity) 
                {
                    this = s_identity;
                    IsDistinguishedIdentity = false;
                } 
                _w = value;
            } 
        } 

        #endregion Public Properties 

        //-----------------------------------------------------
        //
        //  Internal Fields 
        //
        //------------------------------------------------------ 
 
        #region Internal Fields
 
        internal double _x;
        internal double _y;
        internal double _z;
        internal double _w; 

        #endregion Internal Fields 
 
        #region Private Fields and Properties
 
        // If this bool is false then we are a default quaternion with
        // all doubles equal to zero, but should be treated as
        // identity.
        private bool _isNotDistinguishedIdentity; 

        private bool IsDistinguishedIdentity 
        { 
            get
            { 
                return !_isNotDistinguishedIdentity;
            }
            set
            { 
                _isNotDistinguishedIdentity = !value;
            } 
        } 

        private static int GetIdentityHashCode() 
        {
            // This code is called only once.
            double zero = 0;
            double one = 1; 
            // return zero.GetHashCode() ^ zero.GetHashCode() ^ zero.GetHashCode() ^ one.GetHashCode();
            // But this expression can be simplified because the first two hash codes cancel. 
            return zero.GetHashCode() ^ one.GetHashCode(); 
        }
 
        private static Quaternion GetIdentity()
        {
            // This code is called only once.
            Quaternion q = new Quaternion(0,0,0,1); 
            q.IsDistinguishedIdentity = true;
            return q; 
        } 

 
        // Hash code for identity.
        private static int c_identityHashCode = GetIdentityHashCode();

        // Default identity 
        private static Quaternion s_identity = GetIdentity();
 
        #endregion Private Fields and Properties 
    }
} 

// File provided for Reference Use Only by Microsoft Corporation (c) 2007.
// Copyright (c) Microsoft Corporation. All rights reserved.
//---------------------------------------------------------------------------- 
//
// 
//    Copyright (C) Microsoft Corporation.  All rights reserved.
//  
//
// 
// Description: 3D quaternion implementation. 
//
//              See spec at http://avalon/medialayer/Specifications/Avalon3D%20API%20Spec.mht 
//
// History:
//  06/02/2003 : t-gregr - Created
// 
// NOTE:  The field _isNotDistinguishedIdentity is a work-around to the
//        problem that we can't define a default constructor that sets 
//        _w to 1.  So the default constructor sets all fields to 0 or 
//        false, and we interpret _isNotDistinguishedIdentity as follows
// 
//        If false, the quaternion is the identity 0,0,0,1 even though
//        the member fields are 0,0,0,0.
//
//        If true, the quaternion has the value given by its member fields. 
//
//        Don't mess it up! 
//--------------------------------------------------------------------------- 

using MS.Internal; 
using System;
using System.Collections;
using System.ComponentModel;
using System.Diagnostics; 
using System.Globalization;
using System.Reflection; 
using System.Runtime.InteropServices; 
using System.ComponentModel.Design.Serialization;
using System.Windows.Markup; 
using System.Windows.Media;
using System.Windows.Media.Animation;
using System.Windows.Media.Composition;
// These types are aliased to match the unamanaged names used in interop 
using BOOL = System.Boolean;
using WORD = System.UInt16; 
using Float = System.Single; 

using SR=MS.Internal.PresentationCore.SR; 
using SRID=MS.Internal.PresentationCore.SRID;

namespace System.Windows.Media.Media3D
{ 

    ///  
    /// Quaternions. 
    /// Quaternions are distinctly 3D entities that represent rotation in three dimensions.
    /// Their power comes in being able to interpolate (and thus animate) between 
    /// quaternions to achieve a smooth, reliable interpolation.
    /// The default quaternion is the identity.
    /// 
    public partial struct Quaternion : IFormattable 
    {
        //----------------------------------------------------- 
        // 
        //  Constructors
        // 
        //-----------------------------------------------------

        #region Constructors
 
        /// 
        /// Constructor that sets quaternion's initial values. 
        ///  
        /// Value of the X coordinate of the new quaternion.
        /// Value of the Y coordinate of the new quaternion. 
        /// Value of the Z coordinate of the new quaternion.
        /// Value of the W coordinate of the new quaternion.
        public Quaternion(double x, double y, double z, double w)
        { 
            _x = x;
            _y = y; 
            _z = z; 
            _w = w;
            _isNotDistinguishedIdentity = true; 
        }

        /// 
        /// Constructs a quaternion via specified axis of rotation and an angle. 
        /// Throws an InvalidOperationException if given (0,0,0) as axis vector.
        ///  
        /// Vector representing axis of rotation. 
        /// Angle to turn around the given axis (in degrees).
        public Quaternion(Vector3D axisOfRotation, double angleInDegrees) 
        {
            angleInDegrees %= 360.0; // Doing the modulo before converting to radians reduces total error
            double angleInRadians = angleInDegrees * (Math.PI / 180.0);
            double length = axisOfRotation.Length; 
            if (length == 0)
                throw new System.InvalidOperationException(SR.Get(SRID.Quaternion_ZeroAxisSpecified)); 
            Vector3D v = (axisOfRotation / length) * Math.Sin(0.5 * angleInRadians); 
            _x = v.X;
            _y = v.Y; 
            _z = v.Z;
            _w = Math.Cos(0.5 * angleInRadians);
            _isNotDistinguishedIdentity = true;
        } 

        #endregion Constructors 
 

        //------------------------------------------------------ 
        //
        //  Public Methods
        //
        //----------------------------------------------------- 

        #region Public Methods 
        ///  
        ///     Identity quaternion
        ///  
        public static Quaternion Identity
        {
            get
            { 
                return s_identity;
            } 
        } 

        ///  
        /// Retrieves quaternion's axis.
        /// 
        public Vector3D Axis
        { 
            // q = M [cos(Q/2), sin(Q /2)v]
            // axis = sin(Q/2)v 
            // angle = cos(Q/2) 
            // M is magnitude
            get 
            {
                // Handle identity (where axis is indeterminate) by
                // returning arbitrary axis.
                if (IsDistinguishedIdentity || (_x == 0 && _y == 0 && _z == 0)) 
                {
                    return new Vector3D(0,1,0); 
                } 
                else
                { 
                    Vector3D v = new Vector3D(_x, _y, _z);
                    v.Normalize();
                    return v;
                } 
            }
        } 
 
        /// 
        /// Retrieves quaternion's angle. 
        /// 
        public double Angle
        {
            get 
            {
                if (IsDistinguishedIdentity) 
                { 
                    return 0;
                } 

                // Magnitude of quaternion times sine and cosine
                double msin = Math.Sqrt(_x*_x + _y*_y + _z*_z);
                double mcos = _w; 

                if (!(msin <= Double.MaxValue)) 
                { 
                    // Overflowed probably in squaring, so let's scale
                    // the values.  We don't need to include _w in the 
                    // scale factor because we're not going to square
                    // it.
                    double maxcoeff = Math.Max(Math.Abs(_x),Math.Max(Math.Abs(_y),Math.Abs(_z)));
                    double x = _x/maxcoeff; 
                    double y = _y/maxcoeff;
                    double z = _z/maxcoeff; 
                    msin = Math.Sqrt(x*x + y*y + z*z); 
                    // Scale mcos too.
                    mcos = _w/maxcoeff; 
                }

                // Atan2 is better than acos.  (More precise and more efficient.)
                return Math.Atan2(msin,mcos) * (360.0 / Math.PI); 
            }
        } 
 
        /// 
        /// Returns whether the quaternion is normalized (i.e. has a magnitude of 1). 
        /// 
        public bool IsNormalized
        {
            get 
            {
                if (IsDistinguishedIdentity) 
                { 
                    return true;
                } 
                double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
                return DoubleUtil.IsOne(norm2);
            }
        } 

        ///  
        /// Tests whether or not a given quaternion is an identity quaternion. 
        /// 
        public bool IsIdentity 
        {
            get
            {
                return IsDistinguishedIdentity || (_x == 0 && _y == 0 && _z == 0 && _w == 1); 
            }
        } 
 
        /// 
        /// Relaces quaternion with its conjugate 
        /// 
        public void Conjugate()
        {
            if (IsDistinguishedIdentity) 
            {
                return; 
            } 

            // Conjugate([x,y,z,w]) = [-x,-y,-z,w] 
            _x = -_x;
            _y = -_y;
            _z = -_z;
        } 

        ///  
        /// Replaces quaternion with its inverse 
        /// 
        public void Invert() 
        {
            if (IsDistinguishedIdentity)
            {
                return; 
            }
 
            // Inverse = Conjugate / Norm Squared 
            Conjugate();
            double norm2 = _x*_x + _y*_y + _z*_z + _w*_w; 
            _x /= norm2;
            _y /= norm2;
            _z /= norm2;
            _w /= norm2; 
        }
 
        ///  
        /// Normalizes this quaternion.
        ///  
        public void Normalize()
        {
            if (IsDistinguishedIdentity)
            { 
                return;
            } 
 
            double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
            if (norm2 > Double.MaxValue) 
            {
                // Handle overflow in computation of norm2
                double rmax = 1.0/Max(Math.Abs(_x),
                                      Math.Abs(_y), 
                                      Math.Abs(_z),
                                      Math.Abs(_w)); 
 
                _x *= rmax;
                _y *= rmax; 
                _z *= rmax;
                _w *= rmax;
                norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
            } 
            double normInverse = 1.0 / Math.Sqrt(norm2);
            _x *= normInverse; 
            _y *= normInverse; 
            _z *= normInverse;
            _w *= normInverse; 
        }

        /// 
        /// Quaternion addition. 
        /// 
        /// First quaternion being added. 
        /// Second quaternion being added. 
        /// Result of addition.
        public static Quaternion operator +(Quaternion left, Quaternion right) 
        {
            if (right.IsDistinguishedIdentity)
            {
                if (left.IsDistinguishedIdentity) 
                {
                    return new Quaternion(0,0,0,2); 
                } 
                else
                { 
                    // We know left is not distinguished identity here.
                    left._w += 1;
                    return left;
                } 
            }
            else if (left.IsDistinguishedIdentity) 
            { 
                // We know right is not distinguished identity here.
                right._w += 1; 
                return right;
            }
            else
            { 
                return new Quaternion(left._x + right._x,
                                      left._y + right._y, 
                                      left._z + right._z, 
                                      left._w + right._w);
            } 
        }

        /// 
        /// Quaternion addition. 
        /// 
        /// First quaternion being added. 
        /// Second quaternion being added. 
        /// Result of addition.
        public static Quaternion Add(Quaternion left, Quaternion right) 
        {
            return (left + right);
        }
 
        /// 
        /// Quaternion subtraction. 
        ///  
        /// Quaternion to subtract from.
        /// Quaternion to subtract from the first quaternion. 
        /// Result of subtraction.
        public static Quaternion operator -(Quaternion left, Quaternion right)
        {
            if (right.IsDistinguishedIdentity) 
            {
                if (left.IsDistinguishedIdentity) 
                { 
                    return new Quaternion(0,0,0,0);
                } 
                else
                {
                    // We know left is not distinguished identity here.
                    left._w -= 1; 
                    return left;
                } 
            } 
            else if (left.IsDistinguishedIdentity)
            { 
                // We know right is not distinguished identity here.
                return new Quaternion(-right._x, -right._y, -right._z, 1 - right._w);
            }
            else 
            {
                return new Quaternion(left._x - right._x, 
                                      left._y - right._y, 
                                      left._z - right._z,
                                      left._w - right._w); 
            }
        }

        ///  
        /// Quaternion subtraction.
        ///  
        /// Quaternion to subtract from. 
        /// Quaternion to subtract from the first quaternion.
        /// Result of subtraction. 
        public static Quaternion Subtract(Quaternion left, Quaternion right)
        {
            return (left - right);
        } 

        ///  
        /// Quaternion multiplication. 
        /// 
        /// First quaternion. 
        /// Second quaternion.
        /// Result of multiplication.
        public static Quaternion operator *(Quaternion left, Quaternion right)
        { 
            if (left.IsDistinguishedIdentity)
            { 
                return right; 
            }
            if (right.IsDistinguishedIdentity) 
            {
                return left;
            }
 
            double x = left._w * right._x + left._x * right._w + left._y * right._z - left._z * right._y;
            double y = left._w * right._y + left._y * right._w + left._z * right._x - left._x * right._z; 
            double z = left._w * right._z + left._z * right._w + left._x * right._y - left._y * right._x; 
            double w = left._w * right._w - left._x * right._x - left._y * right._y - left._z * right._z;
            Quaternion result = new Quaternion(x,y,z,w); 
            return result;

        }
 
        /// 
        /// Quaternion multiplication. 
        ///  
        /// First quaternion.
        /// Second quaternion. 
        /// Result of multiplication.
        public static Quaternion Multiply(Quaternion left, Quaternion right)
        {
            return left * right; 
        }
 
        ///  
        /// Scale this quaternion by a scalar.
        ///  
        /// Value to scale by.
        private void Scale( double scale )
        {
            if (IsDistinguishedIdentity) 
            {
                _w = scale; 
                IsDistinguishedIdentity = false; 
                return;
            } 
            _x *= scale;
            _y *= scale;
            _z *= scale;
            _w *= scale; 
        }
 
        ///  
        /// Return length of quaternion.
        ///  
        private double Length()
        {
            if (IsDistinguishedIdentity)
            { 
                return 1;
            } 
 
            double norm2 = _x*_x + _y*_y + _z*_z + _w*_w;
            if (!(norm2 <= Double.MaxValue)) 
            {
                // Do this the slow way to avoid squaring large
                // numbers since the length of many quaternions is
                // representable even if the squared length isn't.  Of 
                // course some lengths aren't representable because
                // the length can be up to twice as big as the largest 
                // coefficient. 

                double max = Math.Max(Math.Max(Math.Abs(_x),Math.Abs(_y)), 
                                      Math.Max(Math.Abs(_z),Math.Abs(_w)));

                double x = _x/max;
                double y = _y/max; 
                double z = _z/max;
                double w = _w/max; 
 
                double smallLength = Math.Sqrt(x*x+y*y+z*z+w*w);
                // Return length of this smaller vector times the scale we applied originally. 
                return smallLength * max;
            }
            return Math.Sqrt(norm2);
        } 

        ///  
        /// Smoothly interpolate between the two given quaternions using Spherical 
        /// Linear Interpolation (SLERP).
        ///  
        /// First quaternion for interpolation.
        /// Second quaternion for interpolation.
        /// Interpolation coefficient.
        /// SLERP-interpolated quaternion between the two given quaternions. 
        public static Quaternion Slerp(Quaternion from, Quaternion to, double t)
        { 
            return Slerp(from, to, t, /* useShortestPath = */ true); 
        }
 
        /// 
        /// Smoothly interpolate between the two given quaternions using Spherical
        /// Linear Interpolation (SLERP).
        ///  
        /// First quaternion for interpolation.
        /// Second quaternion for interpolation. 
        /// Interpolation coefficient. 
        /// If true, Slerp will automatically flip the sign of
        ///     the destination Quaternion to ensure the shortest path is taken. 
        /// SLERP-interpolated quaternion between the two given quaternions.
        public static Quaternion Slerp(Quaternion from, Quaternion to, double t, bool useShortestPath)
        {
            if (from.IsDistinguishedIdentity) 
            {
                from._w = 1; 
            } 
            if (to.IsDistinguishedIdentity)
            { 
                to._w = 1;
            }

            double cosOmega; 
            double scaleFrom, scaleTo;
 
            // Normalize inputs and stash their lengths 
            double lengthFrom = from.Length();
            double lengthTo = to.Length(); 
            from.Scale(1/lengthFrom);
            to.Scale(1/lengthTo);

            // Calculate cos of omega. 
            cosOmega = from._x*to._x + from._y*to._y + from._z*to._z + from._w*to._w;
 
            if (useShortestPath) 
            {
                // If we are taking the shortest path we flip the signs to ensure that 
                // cosOmega will be positive.
                if (cosOmega < 0.0)
                {
                    cosOmega = -cosOmega; 
                    to._x = -to._x;
                    to._y = -to._y; 
                    to._z = -to._z; 
                    to._w = -to._w;
                } 
            }
            else
            {
                // If we are not taking the UseShortestPath we clamp cosOmega to 
                // -1 to stay in the domain of Math.Acos below.
                if (cosOmega < -1.0) 
                { 
                    cosOmega = -1.0;
                } 
            }

            // Clamp cosOmega to [-1,1] to stay in the domain of Math.Acos below.
            // The logic above has either flipped the sign of cosOmega to ensure it 
            // is positive or clamped to -1 aready.  We only need to worry about the
            // upper limit here. 
            if (cosOmega > 1.0) 
            {
                cosOmega = 1.0; 
            }

            Debug.Assert(!(cosOmega < -1.0) && !(cosOmega > 1.0),
                "cosOmega should be clamped to [-1,1]"); 

            // The mainline algorithm doesn't work for extreme 
            // cosine values.  For large cosine we have a better 
            // fallback hence the asymmetric limits.
            const double maxCosine = 1.0 - 1e-6; 
            const double minCosine = 1e-10 - 1.0;

            // Calculate scaling coefficients.
            if (cosOmega > maxCosine) 
            {
                // Quaternions are too close - use linear interpolation. 
                scaleFrom = 1.0 - t; 
                scaleTo = t;
            } 
            else if (cosOmega < minCosine)
            {
                // Quaternions are nearly opposite, so we will pretend to
                // is exactly -from. 
                // First assign arbitrary perpendicular to "to".
                to = new Quaternion(-from.Y, from.X, -from.W, from.Z); 
 
                double theta = t * Math.PI;
 
                scaleFrom = Math.Cos(theta);
                scaleTo = Math.Sin(theta);
            }
            else 
            {
                // Standard case - use SLERP interpolation. 
                double omega = Math.Acos(cosOmega); 
                double sinOmega = Math.Sqrt(1.0 - cosOmega*cosOmega);
                scaleFrom = Math.Sin((1.0 - t) * omega) / sinOmega; 
                scaleTo = Math.Sin(t * omega) / sinOmega;
            }

            // We want the magnitude of the output quaternion to be 
            // multiplicatively interpolated between the input
            // magnitudes, i.e. lengthOut = lengthFrom * (lengthTo/lengthFrom)^t 
            //                            = lengthFrom ^ (1-t) * lengthTo ^ t 

            double lengthOut = lengthFrom * Math.Pow(lengthTo/lengthFrom, t); 
            scaleFrom *= lengthOut;
            scaleTo *= lengthOut;

            return new Quaternion(scaleFrom*from._x + scaleTo*to._x, 
                                  scaleFrom*from._y + scaleTo*to._y,
                                  scaleFrom*from._z + scaleTo*to._z, 
                                  scaleFrom*from._w + scaleTo*to._w); 
        }
 
        #endregion Public Methods

        #region Private Methods
 
        static private double Max(double a, double b, double c, double d)
        { 
            if (b > a) 
                a = b;
            if (c > a) 
                a = c;
            if (d > a)
                a = d;
            return a; 
        }
 
        #endregion Private Methods 

        //------------------------------------------------------ 
        //
        //  Public Properties
        //
        //------------------------------------------------------ 

        #region Public Properties 
 
        /// 
        /// X - Default value is 0. 
        /// 
        public double X
        {
            get 
            {
                return _x; 
            } 

            set 
            {
                if (IsDistinguishedIdentity)
                {
                    this = s_identity; 
                    IsDistinguishedIdentity = false;
                } 
                _x = value; 
            }
        } 

        /// 
        /// Y - Default value is 0.
        ///  
        public double Y
        { 
            get 
            {
                return _y; 
            }

            set
            { 
                if (IsDistinguishedIdentity)
                { 
                    this = s_identity; 
                    IsDistinguishedIdentity = false;
                } 
                _y = value;
            }
        }
 
        /// 
        /// Z - Default value is 0. 
        ///  
        public double Z
        { 
            get
            {
                return _z;
            } 

            set 
            { 
                if (IsDistinguishedIdentity)
                { 
                    this = s_identity;
                    IsDistinguishedIdentity = false;
                }
                _z = value; 
            }
        } 
 
        /// 
        /// W - Default value is 1. 
        /// 
        public double W
        {
            get 
            {
                if (IsDistinguishedIdentity) 
                { 
                    return 1.0;
                } 
                else
                {
                    return _w;
                } 
            }
 
            set 
            {
                if (IsDistinguishedIdentity) 
                {
                    this = s_identity;
                    IsDistinguishedIdentity = false;
                } 
                _w = value;
            } 
        } 

        #endregion Public Properties 

        //-----------------------------------------------------
        //
        //  Internal Fields 
        //
        //------------------------------------------------------ 
 
        #region Internal Fields
 
        internal double _x;
        internal double _y;
        internal double _z;
        internal double _w; 

        #endregion Internal Fields 
 
        #region Private Fields and Properties
 
        // If this bool is false then we are a default quaternion with
        // all doubles equal to zero, but should be treated as
        // identity.
        private bool _isNotDistinguishedIdentity; 

        private bool IsDistinguishedIdentity 
        { 
            get
            { 
                return !_isNotDistinguishedIdentity;
            }
            set
            { 
                _isNotDistinguishedIdentity = !value;
            } 
        } 

        private static int GetIdentityHashCode() 
        {
            // This code is called only once.
            double zero = 0;
            double one = 1; 
            // return zero.GetHashCode() ^ zero.GetHashCode() ^ zero.GetHashCode() ^ one.GetHashCode();
            // But this expression can be simplified because the first two hash codes cancel. 
            return zero.GetHashCode() ^ one.GetHashCode(); 
        }
 
        private static Quaternion GetIdentity()
        {
            // This code is called only once.
            Quaternion q = new Quaternion(0,0,0,1); 
            q.IsDistinguishedIdentity = true;
            return q; 
        } 

 
        // Hash code for identity.
        private static int c_identityHashCode = GetIdentityHashCode();

        // Default identity 
        private static Quaternion s_identity = GetIdentity();
 
        #endregion Private Fields and Properties 
    }
} 

// File provided for Reference Use Only by Microsoft Corporation (c) 2007.
// Copyright (c) Microsoft Corporation. All rights reserved.

                        

Link Menu

Network programming in C#, Network Programming in VB.NET, Network Programming in .NET
This book is available now!
Buy at Amazon US or
Buy at Amazon UK