예제 #1
0
        /// <summary>
        /// Performs spherical interpolation between two quaternions. Spherical interpolation neatly interpolates between
        /// two rotations without modifying the size of the vector it is applied to (unlike linear interpolation).
        /// </summary>
        /// <param name="from">Start quaternion.</param>
        /// <param name="to">End quaternion.</param>
        /// <param name="t">Interpolation factor in range [0, 1] that determines how much to interpolate between
        /// <paramref name="from"/> and <paramref name="to"/>.</param>
        /// <param name="shortestPath">Should the interpolation be performed between the shortest or longest path between
        ///                            the two quaternions.</param>
        /// <returns>Interpolated quaternion representing a rotation between <paramref name="from"/> and
        /// <paramref name="to"/>.</returns>
        public static Quaternion Slerp(Quaternion from, Quaternion to, float t, bool shortestPath = true)
        {
            float      dot = Dot(from, to);
            Quaternion quat;

            if (dot < 0.0f && shortestPath)
            {
                dot  = -dot;
                quat = -to;
            }
            else
            {
                quat = to;
            }

            if (MathEx.Abs(dot) < (1 - epsilon))
            {
                float  sin    = MathEx.Sqrt(1 - (dot * dot));
                Radian angle  = MathEx.Atan2(sin, dot);
                float  invSin = 1.0f / sin;
                float  a      = MathEx.Sin((1.0f - t) * angle) * invSin;
                float  b      = MathEx.Sin(t * angle) * invSin;

                return(a * from + b * quat);
            }
            else
            {
                Quaternion ret = (1.0f - t) * from + t * quat;

                ret.Normalize();
                return(ret);
            }
        }
예제 #2
0
        /// <summary>
        /// Converts an orthonormal matrix to euler angle (pitch/yaw/roll) representation.
        /// </summary>
        /// <returns>Euler angles in degrees representing the rotation in this matrix.</returns>
        public Vector3 ToEulerAngles()
        {
            Radian xAngle = -MathEx.Asin(this[1, 2]);

            if (xAngle < MathEx.HalfPi)
            {
                if (xAngle > -MathEx.HalfPi)
                {
                    Radian yAngle = MathEx.Atan2(this[0, 2], this[2, 2]);
                    Radian zAngle = MathEx.Atan2(this[1, 0], this[1, 1]);

                    return(new Vector3(xAngle.Degrees, yAngle.Degrees, zAngle.Degrees));
                }
                else
                {
                    // Note: Not an unique solution.
                    xAngle = -MathEx.HalfPi;
                    Radian yAngle = MathEx.Atan2(-this[0, 1], this[0, 0]);
                    Radian zAngle = (Radian)0.0f;

                    return(new Vector3(xAngle.Degrees, yAngle.Degrees, zAngle.Degrees));
                }
            }
            else
            {
                // Note: Not an unique solution.
                xAngle = MathEx.HalfPi;
                Radian yAngle = MathEx.Atan2(this[0, 1], this[0, 0]);
                Radian zAngle = (Radian)0.0f;

                return(new Vector3(xAngle.Degrees, yAngle.Degrees, zAngle.Degrees));
            }
        }
예제 #3
0
        /// <summary>
        /// Converts an orthonormal matrix to euler angle (pitch/yaw/roll) representation.
        /// </summary>
        /// <returns>Euler angles in degrees representing the rotation in this matrix.</returns>
        public Vector3 ToEulerAngles()
        {
            float xAngle = -MathEx.Asin(this[1, 2]);

            if (xAngle < MathEx.HalfPi)
            {
                if (xAngle > -MathEx.HalfPi)
                {
                    float yAngle = MathEx.Atan2(this[0, 2], this[2, 2]);
                    float zAngle = MathEx.Atan2(this[1, 0], this[1, 1]);

                    return(new Vector3(xAngle * MathEx.Rad2Deg, yAngle * MathEx.Rad2Deg, zAngle * MathEx.Rad2Deg));
                }
                else
                {
                    // Note: Not an unique solution.
                    xAngle = -MathEx.HalfPi;
                    float yAngle = MathEx.Atan2(-this[0, 1], this[0, 0]);
                    float zAngle = 0.0f;

                    return(new Vector3(xAngle * MathEx.Rad2Deg, yAngle * MathEx.Rad2Deg, zAngle * MathEx.Rad2Deg));
                }
            }
            else
            {
                // Note: Not an unique solution.
                xAngle = MathEx.HalfPi;
                float yAngle = MathEx.Atan2(this[0, 1], this[0, 0]);
                float zAngle = 0.0f;

                return(new Vector3(xAngle * MathEx.Rad2Deg, yAngle * MathEx.Rad2Deg, zAngle * MathEx.Rad2Deg));
            }
        }