예제 #1
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        /// <summary>
        ///  Direct-Matrix-Slerp: for the sake of completeness, I have included the following expression
        ///  for Spherical-Linear-Interpolation without using quaternions. This is much faster then converting
        ///  both matrices into quaternions in order to do a quaternion slerp and then converting the slerped
        ///  quaternion back into a matrix.
        ///  This is a high-precision calculation. Given two orthonormal 3x3 matrices this function calculates
        ///  the shortest possible interpolation-path between the two rotations. The interpolation curve forms
        ///  a great arc on the rotation sphere (geodesic). Not only does Slerp follow a great arc it follows
        ///  the shortest great arc.    Furthermore Slerp has constant angular velocity. All in all Slerp is the
        ///  optimal interpolation curve between two rotations.
        ///  STABILITY PROBLEM: There are two singularities at angle=0 and angle=PI. At 0 the interpolation-axis
        ///  is arbitrary, which means any axis will produce the same result because we have no rotation. Thats
        ///  why I'm using (1,0,0). At PI the rotations point away from each other and the interpolation-axis
        ///  is unpredictable. In this case I'm also using the axis (1,0,0). If the angle is ~0 or ~PI, then we
        ///  have to normalize a very small vector and this can cause numerical instability. The quaternion-slerp
        ///  has exactly the same problems.                                                                    Ivo
        /// </summary>
        /// <param name="m"></param>
        /// <param name="n"></param>
        /// <param name="t"></param>
        /// <example>Matrix33 slerp=Matrix33::CreateSlerp( m,n,0.333f );</example>
        public void SetSlerp(Matrix34 m, Matrix34 n, float t)
        {
            // calculate delta-rotation between m and n (=39 flops)
            Matrix33 d = new Matrix33(), i = new Matrix33();

            d.M00 = m.M00 * n.M00 + m.M10 * n.M10 + m.M20 * n.M20; d.M01 = m.M00 * n.M01 + m.M10 * n.M11 + m.M20 * n.M21; d.M02 = m.M00 * n.M02 + m.M10 * n.M12 + m.M20 * n.M22;
            d.M10 = m.M01 * n.M00 + m.M11 * n.M10 + m.M21 * n.M20; d.M11 = m.M01 * n.M01 + m.M11 * n.M11 + m.M21 * n.M21; d.M12 = m.M01 * n.M02 + m.M11 * n.M12 + m.M21 * n.M22;
            d.M20 = d.M01 * d.M12 - d.M02 * d.M11; d.M21 = d.M02 * d.M10 - d.M00 * d.M12; d.M22 = d.M00 * d.M11 - d.M01 * d.M10;

            // extract angle and axis
            double cosine = MathHelpers.Clamp((d.M00 + d.M11 + d.M22 - 1.0) * 0.5, -1.0, +1.0);
            double angle  = Math.Atan2(Math.Sqrt(1.0 - cosine * cosine), cosine);
            var    axis   = new Vec3(d.M21 - d.M12, d.M02 - d.M20, d.M10 - d.M01);
            double l      = Math.Sqrt(axis | axis); if (l > 0.00001)

            {
                axis /= (float)l;
            }
            else
            {
                axis = new Vec3(1, 0, 0);
            }

            i.SetRotationAA((float)angle * t, axis); // angle interpolation and calculation of new delta-matrix (=26 flops)

            // final concatenation (=39 flops)
            M00 = m.M00 * i.M00 + m.M01 * i.M10 + m.M02 * i.M20; M01 = m.M00 * i.M01 + m.M01 * i.M11 + m.M02 * i.M21; M02 = m.M00 * i.M02 + m.M01 * i.M12 + m.M02 * i.M22;
            M10 = m.M10 * i.M00 + m.M11 * i.M10 + m.M12 * i.M20; M11 = m.M10 * i.M01 + m.M11 * i.M11 + m.M12 * i.M21; M12 = m.M10 * i.M02 + m.M11 * i.M12 + m.M12 * i.M22;
            M20 = M01 * M12 - M02 * M11; M21 = M02 * M10 - M00 * M12; M22 = M00 * M11 - M01 * M10;

            M03 = m.M03 * (1 - t) + n.M03 * t;
            M13 = m.M13 * (1 - t) + n.M13 * t;
            M23 = m.M23 * (1 - t) + n.M23 * t;
        }
예제 #2
0
        public static Quat FromMatrix33(Matrix33 m)
        {
            float s, p, tr = m.M00 + m.M11 + m.M22;

            //check the diagonal
            if (tr > (float)0.0)
            {
                s = (float)Math.Sqrt(tr + 1.0f); p = 0.5f / s;
                return(new Quat(s * 0.5f, (m.M21 - m.M12) * p, (m.M02 - m.M20) * p, (m.M10 - m.M01) * p));
            }
            //diagonal is negative. now we have to find the biggest element on the diagonal
            //check if "M00" is the biggest element
            if ((m.M00 >= m.M11) && (m.M00 >= m.M22))
            {
                s = (float)Math.Sqrt(m.M00 - m.M11 - m.M22 + 1.0f); p = 0.5f / s;
                return(new Quat((m.M21 - m.M12) * p, s * 0.5f, (m.M10 + m.M01) * p, (m.M20 + m.M02) * p));
            }
            //check if "M11" is the biggest element
            if ((m.M11 >= m.M00) && (m.M11 >= m.M22))
            {
                s = (float)Math.Sqrt(m.M11 - m.M22 - m.M00 + 1.0f); p = 0.5f / s;
                return(new Quat((m.M02 - m.M20) * p, (m.M01 + m.M10) * p, s * 0.5f, (m.M21 + m.M12) * p));
            }
            //check if "M22" is the biggest element
            if ((m.M22 >= m.M00) && (m.M22 >= m.M11))
            {
                s = (float)Math.Sqrt(m.M22 - m.M00 - m.M11 + 1.0f); p = 0.5f / s;
                return(new Quat((m.M10 - m.M01) * p, (m.M02 + m.M20) * p, (m.M12 + m.M21) * p, s * 0.5f));
            }

            return(Quat.Identity); // if it ends here, then we have no valid rotation matrix
        }
예제 #3
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        public static Matrix33 CreateScale(Vec3 s)
        {
            var matrix = new Matrix33();

            matrix.SetScale(s);

            return(matrix);
        }
예제 #4
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        public static Matrix33 CreateFromVectors(Vec3 vx, Vec3 vy, Vec3 vz)
        {
            var matrix = new Matrix33();

            matrix.SetFromVectors(vx, vy, vz);

            return(matrix);
        }
예제 #5
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        public static Matrix33 CreateRotationXYZ(Vec3 rad)
        {
            var matrix = new Matrix33();

            matrix.SetRotationXYZ(rad);

            return(matrix);
        }
예제 #6
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        public static Matrix33 CreateRotationZ(float rad)
        {
            var matrix = new Matrix33();

            matrix.SetRotationZ(rad);

            return(matrix);
        }
예제 #7
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        public static Matrix33 CreateRotationAA(float c, float s, Vec3 axis)
        {
            var matrix = new Matrix33();

            matrix.SetRotationAA(c, s, axis);

            return(matrix);
        }
예제 #8
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        public static Matrix33 CreateIdentity()
        {
            var matrix = new Matrix33();

            matrix.SetIdentity();

            return(matrix);
        }
예제 #9
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        public static Matrix33 CreateRotationAA(Vec3 rot)
        {
            var matrix = new Matrix33();

            matrix.SetRotationAA(rot);

            return(matrix);
        }
예제 #10
0
 public static Matrix33 operator *(Matrix33 left, Matrix33 right)
 {
     var m = new Matrix33();
     m.M00 = left.M00 * right.M00 + left.M01 * right.M10 + left.M02 * right.M20;
     m.M01 = left.M00 * right.M01 + left.M01 * right.M11 + left.M02 * right.M21;
     m.M02 = left.M00 * right.M02 + left.M01 * right.M12 + left.M02 * right.M22;
     m.M10 = left.M10 * right.M00 + left.M11 * right.M10 + left.M12 * right.M20;
     m.M11 = left.M10 * right.M01 + left.M11 * right.M11 + left.M12 * right.M21;
     m.M12 = left.M10 * right.M02 + left.M11 * right.M12 + left.M12 * right.M22;
     m.M20 = left.M20 * right.M00 + left.M21 * right.M10 + left.M22 * right.M20;
     m.M21 = left.M20 * right.M01 + left.M21 * right.M11 + left.M22 * right.M21;
     m.M22 = left.M20 * right.M02 + left.M21 * right.M12 + left.M22 * right.M22;
     return m;
 }
예제 #11
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        public static Matrix33 operator *(Matrix33 left, Matrix33 right)
        {
            var m = new Matrix33();

            m.M00 = left.M00 * right.M00 + left.M01 * right.M10 + left.M02 * right.M20;
            m.M01 = left.M00 * right.M01 + left.M01 * right.M11 + left.M02 * right.M21;
            m.M02 = left.M00 * right.M02 + left.M01 * right.M12 + left.M02 * right.M22;
            m.M10 = left.M10 * right.M00 + left.M11 * right.M10 + left.M12 * right.M20;
            m.M11 = left.M10 * right.M01 + left.M11 * right.M11 + left.M12 * right.M21;
            m.M12 = left.M10 * right.M02 + left.M11 * right.M12 + left.M12 * right.M22;
            m.M20 = left.M20 * right.M00 + left.M21 * right.M10 + left.M22 * right.M20;
            m.M21 = left.M20 * right.M01 + left.M21 * right.M11 + left.M22 * right.M21;
            m.M22 = left.M20 * right.M02 + left.M21 * right.M12 + left.M22 * right.M22;
            return(m);
        }
예제 #12
0
        public static Matrix33 CreateScale(Vec3 s)
        {
            var matrix = new Matrix33();
            matrix.SetScale(s);

            return matrix;
        }
예제 #13
0
        public static Matrix33 CreateRotationZ(float rad)
        {
            var matrix = new Matrix33();
            matrix.SetRotationZ(rad);

            return matrix;
        }
예제 #14
0
        public static Matrix33 CreateRotationXYZ(Vec3 rad)
        {
            var matrix = new Matrix33();
            matrix.SetRotationXYZ(rad);

            return matrix;
        }
예제 #15
0
        public static Matrix33 CreateRotationAA(Vec3 rot)
        {
            var matrix = new Matrix33();
            matrix.SetRotationAA(rot);

            return matrix;
        }
예제 #16
0
        public static Matrix33 CreateRotationAA(float c, float s, Vec3 axis)
        {
            var matrix = new Matrix33();
            matrix.SetRotationAA(c, s, axis);

            return matrix;
        }
예제 #17
0
        public static Matrix33 CreateIdentity()
        {
            var matrix = new Matrix33();
            matrix.SetIdentity();

            return matrix;
        }
예제 #18
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        /*!
         *  Create a rotation matrix around an arbitrary axis (Eulers Theorem).
         *  The axis is specified as an normalized Vector3. The angle is assumed to be in radians.
         *  This function also assumes a translation-vector and stores it in the right column.
         *
         *  Example:
         *        Matrix34 m34;
         *        Vector3 axis=GetNormalized( Vector3(-1.0f,-0.3f,0.0f) );
         *        m34.SetRotationAA( 3.14314f, axis, Vector3(5,5,5) );
         */
        public void SetRotationAA(float rad, Vec3 axis, Vec3 t = default(Vec3))
        {
            this = new Matrix34(Matrix33.CreateRotationAA(rad, axis));

            SetTranslation(t);
        }
예제 #19
0
 public Quat(Matrix33 matrix)
 {
     this = FromMatrix33(matrix);
 }
예제 #20
0
        /// <summary>
        ///  Direct-Matrix-Slerp: for the sake of completeness, I have included the following expression 
        ///  for Spherical-Linear-Interpolation without using quaternions. This is much faster then converting 
        ///  both matrices into quaternions in order to do a quaternion slerp and then converting the slerped 
        ///  quaternion back into a matrix.
        ///  This is a high-precision calculation. Given two orthonormal 3x3 matrices this function calculates 
        ///  the shortest possible interpolation-path between the two rotations. The interpolation curve forms 
        ///  a great arc on the rotation sphere (geodesic). Not only does Slerp follow a great arc it follows 
        ///  the shortest great arc.    Furthermore Slerp has constant angular velocity. All in all Slerp is the 
        ///  optimal interpolation curve between two rotations. 
        ///  STABILITY PROBLEM: There are two singularities at angle=0 and angle=PI. At 0 the interpolation-axis 
        ///  is arbitrary, which means any axis will produce the same result because we have no rotation. Thats 
        ///  why I'm using (1,0,0). At PI the rotations point away from each other and the interpolation-axis 
        ///  is unpredictable. In this case I'm also using the axis (1,0,0). If the angle is ~0 or ~PI, then we 
        ///  have to normalize a very small vector and this can cause numerical instability. The quaternion-slerp 
        ///  has exactly the same problems.                                                                    Ivo
        /// </summary>
        /// <param name="m"></param>
        /// <param name="n"></param>
        /// <param name="t"></param>
        /// <example>Matrix33 slerp=Matrix33::CreateSlerp( m,n,0.333f );</example>
        public void SetSlerp(Matrix34 m, Matrix34 n, float t)
        {
            // calculate delta-rotation between m and n (=39 flops)
            Matrix33 d = new Matrix33(), i = new Matrix33();
            d.M00 = m.M00 * n.M00 + m.M10 * n.M10 + m.M20 * n.M20; d.M01 = m.M00 * n.M01 + m.M10 * n.M11 + m.M20 * n.M21; d.M02 = m.M00 * n.M02 + m.M10 * n.M12 + m.M20 * n.M22;
            d.M10 = m.M01 * n.M00 + m.M11 * n.M10 + m.M21 * n.M20; d.M11 = m.M01 * n.M01 + m.M11 * n.M11 + m.M21 * n.M21; d.M12 = m.M01 * n.M02 + m.M11 * n.M12 + m.M21 * n.M22;
            d.M20 = d.M01 * d.M12 - d.M02 * d.M11; d.M21 = d.M02 * d.M10 - d.M00 * d.M12; d.M22 = d.M00 * d.M11 - d.M01 * d.M10;

            // extract angle and axis
            double cosine = MathHelpers.Clamp((d.M00 + d.M11 + d.M22 - 1.0) * 0.5, -1.0, +1.0);
            double angle = Math.Atan2(Math.Sqrt(1.0 - cosine * cosine), cosine);
            var axis = new Vec3(d.M21 - d.M12, d.M02 - d.M20, d.M10 - d.M01);
            double l = Math.Sqrt(axis | axis); if (l > 0.00001) axis /= (float)l; else axis = new Vec3(1, 0, 0);
            i.SetRotationAA((float)angle * t, axis); // angle interpolation and calculation of new delta-matrix (=26 flops)

            // final concatenation (=39 flops)
            M00 = m.M00 * i.M00 + m.M01 * i.M10 + m.M02 * i.M20; M01 = m.M00 * i.M01 + m.M01 * i.M11 + m.M02 * i.M21; M02 = m.M00 * i.M02 + m.M01 * i.M12 + m.M02 * i.M22;
            M10 = m.M10 * i.M00 + m.M11 * i.M10 + m.M12 * i.M20; M11 = m.M10 * i.M01 + m.M11 * i.M11 + m.M12 * i.M21; M12 = m.M10 * i.M02 + m.M11 * i.M12 + m.M12 * i.M22;
            M20 = M01 * M12 - M02 * M11; M21 = M02 * M10 - M00 * M12; M22 = M00 * M11 - M01 * M10;

            M03 = m.M03 * (1 - t) + n.M03 * t;
            M13 = m.M13 * (1 - t) + n.M13 * t;
            M23 = m.M23 * (1 - t) + n.M23 * t;
        }
예제 #21
0
 public Matrix34(Matrix33 m33)
     : this()
 {
 }
예제 #22
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
 public void SetRotation33(Matrix33 m33)
 {
     M00 = m33.M00; M01 = m33.M01; M02 = m33.M02;
     M10 = m33.M10; M11 = m33.M11; M12 = m33.M12;
     M20 = m33.M20; M21 = m33.M21; M22 = m33.M22;
 }
예제 #23
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        public void SetRotationAA(Vec3 rot, Vec3 t = default(Vec3))
        {
            this = new Matrix34(Matrix33.CreateRotationAA(rot));

            SetTranslation(t);
        }
예제 #24
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        public void SetScale(Vec3 s, Vec3 t = default(Vec3))
        {
            this = new Matrix34(Matrix33.CreateScale(s));

            SetTranslation(t);
        }
예제 #25
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
 public void SetRotationAA(float c, float s, Vec3 axis, Vec3 t = default(Vec3))
 {
     this = new Matrix34(Matrix33.CreateRotationAA(c, s, axis));
     M03  = t.X; M13 = t.Y; M23 = t.Z;
 }
예제 #26
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
        /*!
         *
         * Convert three Euler angle to mat33 (rotation order:XYZ)
         * The Euler angles are assumed to be in radians.
         * The translation-vector is set to zero.
         *
         *  Example 1:
         *        Matrix34 m34;
         *        m34.SetRotationXYZ( Ang3(0.5f,0.2f,0.9f), translation );
         *
         *  Example 2:
         *        Matrix34 m34=Matrix34::CreateRotationXYZ( Ang3(0.5f,0.2f,0.9f), translation );
         */
        public void SetRotationXYZ(Vec3 rad, Vec3 t = default(Vec3))
        {
            this = new Matrix34(Matrix33.CreateRotationXYZ(rad));

            SetTranslation(t);
        }
예제 #27
0
 public void SetRotation33(Matrix33 m33)
 {
     M00 = m33.M00; M01 = m33.M01; M02 = m33.M02;
     M10 = m33.M10; M11 = m33.M11; M12 = m33.M12;
     M20 = m33.M20; M21 = m33.M21; M22 = m33.M22;
 }
예제 #28
0
        public static Quat FromMatrix33(Matrix33 m)
        {
            float s, p, tr = m.M00 + m.M11 + m.M22;

            //check the diagonal
            if (tr > (float)0.0)
            {
                s = (float)Math.Sqrt(tr + 1.0f); p = 0.5f / s;
                return new Quat(s * 0.5f, (m.M21 - m.M12) * p, (m.M02 - m.M20) * p, (m.M10 - m.M01) * p);
            }
            //diagonal is negative. now we have to find the biggest element on the diagonal
            //check if "M00" is the biggest element
            if ((m.M00 >= m.M11) && (m.M00 >= m.M22))
            {
                s = (float)Math.Sqrt(m.M00 - m.M11 - m.M22 + 1.0f); p = 0.5f / s;
                return new Quat((m.M21 - m.M12) * p, s * 0.5f, (m.M10 + m.M01) * p, (m.M20 + m.M02) * p);
            }
            //check if "M11" is the biggest element
            if ((m.M11 >= m.M00) && (m.M11 >= m.M22))
            {
                s = (float)Math.Sqrt(m.M11 - m.M22 - m.M00 + 1.0f); p = 0.5f / s;
                return new Quat((m.M02 - m.M20) * p, (m.M01 + m.M10) * p, s * 0.5f, (m.M21 + m.M12) * p);
            }
            //check if "M22" is the biggest element
            if ((m.M22 >= m.M00) && (m.M22 >= m.M11))
            {
                s = (float)Math.Sqrt(m.M22 - m.M00 - m.M11 + 1.0f); p = 0.5f / s;
                return new Quat((m.M10 - m.M01) * p, (m.M02 + m.M20) * p, (m.M12 + m.M21) * p, s * 0.5f);
            }

            return Quat.Identity; // if it ends here, then we have no valid rotation matrix
        }
예제 #29
0
        /// <summary>
        /// x-YAW
        /// y-PITCH (negative=looking down / positive=looking up)
        /// z-ROLL
        /// Note: If we are looking along the z-axis, its not possible to specify the x and z-angle.
        /// </summary>
        /// <param name="m"></param>
        /// <returns></returns>
        public static Angles3 CreateAnglesYPR(Matrix3x4 m)
        {
            Matrix33 m33 = new Matrix33(m);

            return(CCamera.CreateAnglesYPR(m33));
        }
예제 #30
0
 public Quat(Matrix33 matrix)
 {
     this = FromMatrix33(matrix);
 }
예제 #31
0
        public static Matrix33 CreateFromVectors(Vec3 vx, Vec3 vy, Vec3 vz)
        {
            var matrix = new Matrix33();
            matrix.SetFromVectors(vx, vy, vz);

            return matrix;
        }
예제 #32
0
파일: Matrix.cs 프로젝트: BenChung/CryMono
 public Matrix34(Matrix33 m33)
     : this()
 {
 }