Example #1
0
        /// <summary>
        /// BonsaiQuaternion linear interpolation
        /// </summary>
        /// <param name="source"></param>
        /// <param name="dest"></param>
        /// <param name="time"></param>
        /// <returns></returns>
        public static BonsaiQuaternion Slerp(BonsaiQuaternion source, BonsaiQuaternion dest, float time)
        {
            // valid
            BonsaiQuaternion res;

            float[] to1 = { 0, 0, 0, 0 };
            float   omega, cosom, sinom, scale0, scale1;


            // calc cosine
            cosom = source.X * dest.X + source.Y * dest.Y + source.Z * dest.Z
                    + source.W * dest.W;


            // adjust signs (if necessary)
            if (cosom < 0.0)
            {
                cosom  = -cosom; to1[0] = -dest.X;
                to1[1] = -dest.Y;
                to1[2] = -dest.Z;
                to1[3] = -dest.W;
            }
            else
            {
                to1[0] = dest.X;
                to1[1] = dest.Y;
                to1[2] = dest.Z;
                to1[3] = dest.W;
            }


            // calculate coefficients


            if ((1.0 - cosom) > EPSILON)
            {
                // standard case (slerp)
                omega  = (float)Math.Acos(cosom);
                sinom  = (float)Math.Sin(omega);
                scale0 = (float)Math.Sin((1.0f - time) * omega) / sinom;
                scale1 = (float)Math.Sin(time * omega) / sinom;
            }
            else
            {
                // "from" and "to" BonsaiQuaternions are very close
                //  ... so we can do a linear interpolation
                scale0 = 1.0f - time;
                scale1 = time;
            }
            // calculate final values
            res.X = scale0 * source.X + scale1 * to1[0];
            res.Y = scale0 * source.Y + scale1 * to1[1];
            res.Z = scale0 * source.Z + scale1 * to1[2];
            res.W = scale0 * source.W + scale1 * to1[3];

            return(res);
        }
Example #2
0
        /// <summary>
        /// BonsaiQuaternion multiplication
        /// </summary>
        /// <param name="left"></param>
        /// <param name="right"></param>
        /// <returns></returns>
        public static BonsaiQuaternion operator *(BonsaiQuaternion left, BonsaiQuaternion right)
        {
            // to maintain DirectX compatibility the factor is inversed (res = right*left)...
            // valid
            BonsaiQuaternion res = Zero;

            res.W = right.W * left.W - right.X * left.X - right.Y * left.Y - right.Z * left.Z;
            res.X = right.W * left.X + right.X * left.W + right.Y * left.Z - right.Z * left.Y;
            res.Y = right.W * left.Y + right.Y * left.W + right.Z * left.X - right.X * left.Z;
            res.Z = right.W * left.Z + right.Z * left.W + right.X * left.Y - right.Y * left.X;


            return(res);
        }
Example #3
0
        /// <summary>
        /// return a BonsaiQuaternion from axis and angle
        /// </summary>
        /// <param name="axis"></param>
        /// <param name="angle"></param>
        /// <returns></returns>
        public static BonsaiQuaternion RotationAxis(Vector3 axis, float angle)
        {
            // valid
            BonsaiQuaternion res      = BonsaiQuaternion.Zero;
            Vector3          normAxis = Vector3.Normalize(axis);


            float sin = (float)Math.Sin(angle / 2.0f);

            res.X = sin * normAxis.X;
            res.Y = sin * normAxis.Y;
            res.Z = sin * normAxis.Z;
            res.W = (float)Math.Cos(angle / 2.0f);

            return(res);
        }
Example #4
0
        public static BonsaiQuaternion RotationYawPitchRoll(float yaw, float pitch, float roll)
        {
            BonsaiQuaternion res = Zero;


            float fSinPitch       = (float)Math.Sin(pitch * 0.5F);
            float fCosPitch       = (float)Math.Cos(pitch * 0.5F);
            float fSinYaw         = (float)Math.Sin(yaw * 0.5F);
            float fCosYaw         = (float)Math.Cos(yaw * 0.5F);
            float fSinRoll        = (float)Math.Sin(roll * 0.5F);
            float fCosRoll        = (float)Math.Cos(roll * 0.5F);
            float fCosPitchCosYaw = fCosPitch * fCosYaw;
            float fSinPitchSinYaw = fSinPitch * fSinYaw;

            res.X = fSinRoll * fCosPitchCosYaw - fCosRoll * fSinPitchSinYaw;
            res.Y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw;
            res.Z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw;
            res.W = fCosRoll * fCosPitchCosYaw + fSinRoll * fSinPitchSinYaw;
            return(res);
        }
Example #5
0
        public override bool Equals(object obj)
        {
            BonsaiQuaternion quat = (BonsaiQuaternion)obj;

            return(quat == this);
        }
Example #6
0
        /// <summary>
        /// returns a BonsaiQuaternion from matrix
        /// </summary>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public static BonsaiQuaternion RotationMatrix(Matrix matrix)
        {
            // valid
            BonsaiQuaternion res = Zero;

            float [,] m = get44(matrix);
            float tr, s;

            float[] q = { 0, 0, 0, 0 };
            int     i, j, k;


            int [] nxt = { 1, 2, 0 };


            tr = m[0, 0] + m[1, 1] + m[2, 2];


            // check the diagonal
            if (tr > 0.0)
            {
                s     = (float)Math.Sqrt(tr + 1.0);
                res.W = s / 2.0f;
                s     = 0.5f / s;
                res.X = (m[1, 2] - m[2, 1]) * s;
                res.Y = (m[2, 0] - m[0, 2]) * s;
                res.Z = (m[0, 1] - m[1, 0]) * s;
            }
            else
            {
                // diagonal is negative
                i = 0;
                if (m[1, 1] > m[0, 0])
                {
                    i = 1;
                }
                if (m[2, 2] > m[i, i])
                {
                    i = 2;
                }
                j = nxt[i];
                k = nxt[j];


                s = (float)Math.Sqrt((m[i, i] - (m[j, j] + m[k, k])) + 1.0);

                q[i] = s * 0.5f;

                if (s != 0.0)
                {
                    s = 0.5f / s;
                }


                q[3] = (m[j, k] - m[k, j]) * s;
                q[j] = (m[i, j] + m[j, i]) * s;
                q[k] = (m[i, k] + m[k, i]) * s;


                res.X = q[0];
                res.Y = q[1];
                res.Z = q[2];
                res.W = q[3];
            }
            return(res);
        }
Example #7
0
 public static float Dot(BonsaiQuaternion left, BonsaiQuaternion right)
 {
     return(left.X * right.X + left.Y * right.Y + left.Z * right.Z + left.W * right.W);
 }