Exemple #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);
            }
        }
Exemple #2
0
        /// <summary>
        /// Calculates an inverse of the matrix if it exists.
        /// </summary>
        public void Invert()
        {
            float[,] invVals = new float[3, 3];

            invVals[0, 0] = m11 * m22 - m12 * m21;
            invVals[1, 0] = m12 * m20 - m10 * m22;
            invVals[2, 0] = m10 * m21 - m11 * m20;

            float det = m00 * invVals[0, 0] + m01 * invVals[1, 0] + m02 * invVals[2, 0];

            if (MathEx.Abs(det) <= 1e-06f)
            {
                throw new DivideByZeroException("Matrix determinant is zero. Cannot invert.");
            }

            invVals[0, 1] = m02 * m21 - m01 * m22;
            invVals[0, 2] = m01 * m12 - m02 * m11;

            invVals[1, 1] = m00 * m22 - m02 * m20;
            invVals[1, 2] = m02 * m10 - m00 * m12;

            invVals[2, 1] = m01 * m20 - m00 * m21;
            invVals[2, 2] = m00 * m11 - m01 * m10;

            float invDet = 1.0f / det;

            for (int row = 0; row < 3; row++)
            {
                for (int col = 0; col < 3; col++)
                {
                    invVals[row, col] *= invDet;
                }
            }
        }
Exemple #3
0
        /// <summary>
        /// Transforms the bounding box by the given matrix.
        ///
        /// As the resulting box will no longer be axis aligned, an axis align box is instead created by encompassing the
        /// transformed oriented bounding box. Retrieving the value as an actual OBB would provide a tighter fit.
        /// </summary>
        /// <param name="tfrm">Affine matrix to transform the box with.</param>
        public void TransformAffine(Matrix4 tfrm)
        {
            Vector3 center   = Center;
            Vector3 halfSize = Size * 0.5f;

            Vector3 newCenter   = tfrm.MultiplyAffine(center);
            Vector3 newHalfSize = new Vector3(
                MathEx.Abs(tfrm.m00) * halfSize.x + MathEx.Abs(tfrm.m01) * halfSize.y + MathEx.Abs(tfrm.m02) * halfSize.z,
                MathEx.Abs(tfrm.m10) * halfSize.x + MathEx.Abs(tfrm.m11) * halfSize.y + MathEx.Abs(tfrm.m12) * halfSize.z,
                MathEx.Abs(tfrm.m20) * halfSize.x + MathEx.Abs(tfrm.m21) * halfSize.y + MathEx.Abs(tfrm.m22) * halfSize.z);

            minimum = newCenter - newHalfSize;
            maximum = newCenter + newHalfSize;
        }
Exemple #4
0
        /// <summary>
        /// Calculates two vectors orthonormal to the first vector.
        /// </summary>
        /// <param name="x">Normalized vector to calculate orthonormal vectors for.</param>
        /// <param name="y">First orthonormal vector.</param>
        /// <param name="z">Second orthonormal vector.</param>
        public static void OrthogonalComplement(Vector3 x, out Vector3 y, out Vector3 z)
        {
            if (MathEx.Abs(x.x) > MathEx.Abs(x.y))
            {
                y = new Vector3(-x.z, 0, x.x);
            }
            else
            {
                y = new Vector3(0, x.z, -x.y);
            }

            z = Cross(x, y);

            Orthonormalize(ref x, ref y, ref z);
        }
Exemple #5
0
        /// <summary>
        /// Wraps an angle in [0, 360) range. Values lower than zero, or higher or equal to 360
        /// will get wrapped around back into [0, 360) range.
        /// </summary>
        /// <param name="angle">Angle to wrap.</param>
        /// <returns>Angle in [0, 360) range.</returns>
        public static Degree WrapAngle(Degree angle)
        {
            const float inv360    = 1.0f / 360.0f;
            float       angleVal  = angle.Degrees;
            float       wrapCount = (float)MathEx.Floor(MathEx.Abs(angleVal) * inv360);

            if (angleVal > 0.0f)
            {
                angleVal -= 360.0f * wrapCount;
            }
            else if (angleVal < 0.0f)
            {
                angleVal += 360.0f * (wrapCount + 1);
            }

            return(new Degree(angleVal));
        }
Exemple #6
0
 /// <summary>
 /// Calculates an angle between two rotations.
 /// </summary>
 /// <param name="a">First rotation.</param>
 /// <param name="b">Second rotation.</param>
 /// <returns>Angle between the rotations, in degrees.</returns>
 public static Degree Angle(Quaternion a, Quaternion b)
 {
     return(MathEx.Acos(MathEx.Min(MathEx.Abs(Dot(a, b)), 1.0f)) * 2.0f);
 }