예제 #1
0
        /// <summary>
        /// Converts this Rotation to the axis angle representation.
        /// </summary>
        /// <param name="axis">Output of normalized axis of rotation.</param>
        /// <param name="angleInRadians">Output of angle of rotation about axis (Right Hand Rule).</param>
        public void ToAxisAngle(ref __v3t__ axis, ref __ft__ angleInRadians)
        {
            if (!Fun.ApproximateEquals(NormSquared, 1, 0.001))
            {
                throw new ArgumentException("Quaternion needs to be normalized to represent a rotation.");
            }
            angleInRadians = 2 * (__ft__)System.Math.Acos(W);
            var s = (__ft__)System.Math.Sqrt(1 - W * W); // assuming quaternion normalised then w is less than 1, so term always positive.

            if (s < 0.001)
            {               // test to avoid divide by zero, s is always positive due to sqrt
                // if s close to zero then direction of axis not important
                axis.X = X; // if it is important that axis is normalised then replace with x=1; y=z=0;
                axis.Y = Y;
                axis.Z = Z;
            }
            else
            {
                axis.X = X / s; // normalise axis
                axis.Y = Y / s;
                axis.Z = Z / s;
            }
        }