Esempio n. 1
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;
        }
Esempio n. 2
0
        public static Matrix34 CreateFromVectors(Vec3 vx, Vec3 vy, Vec3 vz, Vec3 pos)
        {
            var matrix = new Matrix34();

            matrix.SetFromVectors(vx, vy, vz, pos);

            return(matrix);
        }
Esempio n. 3
0
        public static Matrix34 CreateTranslationMat(Vec3 v)
        {
            var matrix = new Matrix34();

            matrix.SetTranslationMat(v);

            return(matrix);
        }
Esempio n. 4
0
        public static Matrix34 CreateScale(Vec3 s, Vec3 t = default(Vec3))
        {
            var matrix = new Matrix34();

            matrix.SetScale(s, t);

            return(matrix);
        }
Esempio n. 5
0
        Matrix34 GetInverted()
        {
            Matrix34 dst = this;

            dst.Invert();

            return(dst);
        }
Esempio n. 6
0
        public static Matrix34 Create(Vec3 s, Quat q, Vec3 t = default(Vec3))
        {
            var matrix = new Matrix34();

            matrix.Set(s, q, t);

            return(matrix);
        }
Esempio n. 7
0
        public static Matrix34 CreateRotationAA(float c, float s, Vec3 axis, Vec3 t = default(Vec3))
        {
            var matrix = new Matrix34();

            matrix.SetRotationAA(c, s, axis, t);

            return(matrix);
        }
Esempio n. 8
0
        public static Matrix34 CreateSlerp(Matrix34 m, Matrix34 n, float t)
        {
            var matrix = new Matrix34();

            matrix.SetSlerp(m, n, t);

            return(matrix);
        }
Esempio n. 9
0
        public static Matrix34 CreateRotationXYZ(Vec3 rad, Vec3 t = default(Vec3))
        {
            var matrix = new Matrix34();

            matrix.SetRotationXYZ(rad, t);

            return(matrix);
        }
Esempio n. 10
0
        public static Matrix34 CreateRotationAA(Vec3 rot, Vec3 t = default(Vec3))
        {
            var matrix = new Matrix34();

            matrix.SetRotationAA(rot, t);

            return(matrix);
        }
Esempio n. 11
0
        public static Matrix34 CreateIdentity()
        {
            var matrix = new Matrix34();

            matrix.SetIdentity();

            return(matrix);
        }
Esempio n. 12
0
        public void SetFromVectors(Vec3 vx, Vec3 vy, Vec3 vz, Vec3 pos)
        {
            var m34 = new Matrix34();

            m34.M00 = vx.X; m34.M01 = vy.X; m34.M02 = vz.X; m34.M03 = pos.X;
            m34.M10 = vx.Y; m34.M11 = vy.Y; m34.M12 = vz.Y; m34.M13 = pos.Y;
            m34.M20 = vx.Z; m34.M21 = vy.Z; m34.M22 = vz.Z; m34.M23 = pos.Z;
            this    = new QuatT(m34);
        }
Esempio n. 13
0
        public Matrix34 GetInvertedFast()
        {
            var dst = new Matrix34();

            dst.M00 = M00; dst.M01 = M10; dst.M02 = M20; dst.M03 = -M03 * M00 - M13 * M10 - M23 * M20;
            dst.M10 = M01; dst.M11 = M11; dst.M12 = M21; dst.M13 = -M03 * M01 - M13 * M11 - M23 * M21;
            dst.M20 = M02; dst.M21 = M12; dst.M22 = M22; dst.M23 = -M03 * M02 - M13 * M12 - M23 * M22;
            return(dst);
        }
Esempio n. 14
0
        public Matrix33(Matrix34 m)
        {
            M00 = m.M00;
            M01 = m.M01;
            M02 = m.M02;

            M10 = m.M10;
            M11 = m.M11;
            M12 = m.M12;

            M20 = m.M20;
            M21 = m.M21;
            M22 = m.M22;
        }
Esempio n. 15
0
        public Matrix33(Matrix34 m)
        {
            M00 = m.M00;
            M01 = m.M01;
            M02 = m.M02;

            M10 = m.M10;
            M11 = m.M11;
            M12 = m.M12;

            M20 = m.M20;
            M21 = m.M21;
            M22 = m.M22;
        }
Esempio n. 16
0
        public static Matrix34 operator *(Matrix34 l, Matrix34 r)
        {
            var m = new Matrix34();

            m.M00 = l.M00 * r.M00 + l.M01 * r.M10 + l.M02 * r.M20;
            m.M10 = l.M10 * r.M00 + l.M11 * r.M10 + l.M12 * r.M20;
            m.M20 = l.M20 * r.M00 + l.M21 * r.M10 + l.M22 * r.M20;
            m.M01 = l.M00 * r.M01 + l.M01 * r.M11 + l.M02 * r.M21;
            m.M11 = l.M10 * r.M01 + l.M11 * r.M11 + l.M12 * r.M21;
            m.M21 = l.M20 * r.M01 + l.M21 * r.M11 + l.M22 * r.M21;
            m.M02 = l.M00 * r.M02 + l.M01 * r.M12 + l.M02 * r.M22;
            m.M12 = l.M10 * r.M02 + l.M11 * r.M12 + l.M12 * r.M22;
            m.M22 = l.M20 * r.M02 + l.M21 * r.M12 + l.M22 * r.M22;
            m.M03 = l.M00 * r.M03 + l.M01 * r.M13 + l.M02 * r.M23 + l.M03;
            m.M13 = l.M10 * r.M03 + l.M11 * r.M13 + l.M12 * r.M23 + l.M13;
            m.M23 = l.M20 * r.M03 + l.M21 * r.M13 + l.M22 * r.M23 + l.M23;

            return(m);
        }
Esempio n. 17
0
        public static Matrix34 CreateRotationAA(Vec3 rot, Vec3 t = default(Vec3))
        {
            var matrix = new Matrix34();
            matrix.SetRotationAA(rot, t);

            return matrix;
        }
Esempio n. 18
0
        public static Matrix34 CreateFromVectors(Vec3 vx, Vec3 vy, Vec3 vz, Vec3 pos)
        {
            var matrix = new Matrix34();
            matrix.SetFromVectors(vx, vy, vz, pos);

            return matrix;
        }
Esempio n. 19
0
        public static Matrix34 CreateIdentity()
        {
            var matrix = new Matrix34();

            matrix.SetIdentity();

            return matrix;
        }
Esempio n. 20
0
        public static Matrix34 operator *(Matrix34 l, Matrix34 r)
        {
            var m = new Matrix34();
            m.M00 = l.M00 * r.M00 + l.M01 * r.M10 + l.M02 * r.M20;
            m.M10 = l.M10 * r.M00 + l.M11 * r.M10 + l.M12 * r.M20;
            m.M20 = l.M20 * r.M00 + l.M21 * r.M10 + l.M22 * r.M20;
            m.M01 = l.M00 * r.M01 + l.M01 * r.M11 + l.M02 * r.M21;
            m.M11 = l.M10 * r.M01 + l.M11 * r.M11 + l.M12 * r.M21;
            m.M21 = l.M20 * r.M01 + l.M21 * r.M11 + l.M22 * r.M21;
            m.M02 = l.M00 * r.M02 + l.M01 * r.M12 + l.M02 * r.M22;
            m.M12 = l.M10 * r.M02 + l.M11 * r.M12 + l.M12 * r.M22;
            m.M22 = l.M20 * r.M02 + l.M21 * r.M12 + l.M22 * r.M22;
            m.M03 = l.M00 * r.M03 + l.M01 * r.M13 + l.M02 * r.M23 + l.M03;
            m.M13 = l.M10 * r.M03 + l.M11 * r.M13 + l.M12 * r.M23 + l.M13;
            m.M23 = l.M20 * r.M03 + l.M21 * r.M13 + l.M22 * r.M23 + l.M23;

            return m;
        }
Esempio n. 21
0
        public static Matrix34 Create(Vec3 s, Quat q, Vec3 t = default(Vec3))
        {
            var matrix = new Matrix34();
            matrix.Set(s, q, t);

            return matrix;
        }
Esempio n. 22
0
        public static Matrix34 CreateScale(Vec3 s, Vec3 t = default(Vec3))
        {
            var matrix = new Matrix34();
            matrix.SetScale(s, t);

            return matrix;
        }
Esempio n. 23
0
 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;
 }
Esempio n. 24
0
        /*!
         *
         * 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);
        }
Esempio n. 25
0
        public void SetRotationAA(Vec3 rot, Vec3 t = default(Vec3))
        {
            this = new Matrix34(Matrix33.CreateRotationAA(rot));

            SetTranslation(t);
        }
Esempio n. 26
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;
        }
Esempio n. 27
0
 public void SetFromVectors(Vec3 vx, Vec3 vy, Vec3 vz, Vec3 pos)
 {
     var m34 = new Matrix34();
     m34.M00 = vx.X; m34.M01 = vy.X; m34.M02 = vz.X; m34.M03 = pos.X;
     m34.M10 = vx.Y; m34.M11 = vy.Y; m34.M12 = vz.Y; m34.M13 = pos.Y;
     m34.M20 = vx.Z; m34.M21 = vy.Z; m34.M22 = vz.Z; m34.M23 = pos.Z;
     this = new QuatT(m34);
 }
Esempio n. 28
0
        /*!
        * Create rotation-matrix about Z axis using an angle.
        * The angle is assumed to be in radians.
        * The translation-vector is set to zero.
        *
        *  Example:
        *        Matrix34 m34;
        *        m34.SetRotationZ(0.5f);
        */
        public void SetRotationZ(float rad, Vec3 t = default(Vec3))
        {
            this = new Matrix34(Matrix33.CreateRotationZ(rad));

            SetTranslation(t);
        }
Esempio n. 29
0
        public void SetRotationAA(Vec3 rot, Vec3 t = default(Vec3))
        {
            this = new Matrix34(Matrix33.CreateRotationAA(rot));

            SetTranslation(t);
        }
Esempio n. 30
0
        public static Matrix34 CreateTranslationMat(Vec3 v)
        {
            var matrix = new Matrix34();
            matrix.SetTranslationMat(v);

            return matrix;
        }
Esempio n. 31
0
        public static Matrix34 CreateRotationAA(float c, float s, Vec3 axis, Vec3 t = default(Vec3))
        {
            var matrix = new Matrix34();
            matrix.SetRotationAA(c, s, axis, t);

            return matrix;
        }
Esempio n. 32
0
 public Quat(Matrix34 matrix)
     : this(new Matrix33(matrix))
 {
 }
Esempio n. 33
0
        public static Matrix34 CreateRotationZ(float rad, Vec3 t = default(Vec3))
        {
            var matrix = new Matrix34();
            matrix.SetRotationZ(rad, t);

            return matrix;
        }
Esempio n. 34
0
        /*!
         *  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);
        }
Esempio n. 35
0
        public static Matrix34 CreateSlerp(Matrix34 m, Matrix34 n, float t)
        {
            var matrix = new Matrix34();
            matrix.SetSlerp(m, n, t);

            return matrix;
        }
Esempio n. 36
0
        public void SetScale(Vec3 s, Vec3 t = default(Vec3))
        {
            this = new Matrix34(Matrix33.CreateScale(s));

            SetTranslation(t);
        }
Esempio n. 37
0
 public bool IsEquivalent(Matrix34 m, float e = 0.05f)
 {
     return ((Math.Abs(M00 - m.M00) <= e) && (Math.Abs(M01 - m.M01) <= e) && (Math.Abs(M02 - m.M02) <= e) && (Math.Abs(M03 - m.M03) <= e) &&
     (Math.Abs(M10 - m.M10) <= e) && (Math.Abs(M11 - m.M11) <= e) && (Math.Abs(M12 - m.M12) <= e) && (Math.Abs(M13 - m.M13) <= e) &&
     (Math.Abs(M20 - m.M20) <= e) && (Math.Abs(M21 - m.M21) <= e) && (Math.Abs(M22 - m.M22) <= e) && (Math.Abs(M23 - m.M23) <= e));
 }
Esempio n. 38
0
 public bool IsEquivalent(Matrix34 m, float e = 0.05f)
 {
     return((Math.Abs(M00 - m.M00) <= e) && (Math.Abs(M01 - m.M01) <= e) && (Math.Abs(M02 - m.M02) <= e) && (Math.Abs(M03 - m.M03) <= e) &&
            (Math.Abs(M10 - m.M10) <= e) && (Math.Abs(M11 - m.M11) <= e) && (Math.Abs(M12 - m.M12) <= e) && (Math.Abs(M13 - m.M13) <= e) &&
            (Math.Abs(M20 - m.M20) <= e) && (Math.Abs(M21 - m.M21) <= e) && (Math.Abs(M22 - m.M22) <= e) && (Math.Abs(M23 - m.M23) <= e));
 }
Esempio n. 39
0
 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;
 }
Esempio n. 40
0
 public Matrix34 GetInvertedFast()
 {
     var dst = new Matrix34();
     dst.M00 = M00; dst.M01 = M10; dst.M02 = M20; dst.M03 = -M03 * M00 - M13 * M10 - M23 * M20;
     dst.M10 = M01; dst.M11 = M11; dst.M12 = M21; dst.M13 = -M03 * M01 - M13 * M11 - M23 * M21;
     dst.M20 = M02; dst.M21 = M12; dst.M22 = M22; dst.M23 = -M03 * M02 - M13 * M12 - M23 * M22;
     return dst;
 }
Esempio n. 41
0
        public void SetScale(Vec3 s, Vec3 t = default(Vec3))
        {
            this = new Matrix34(Matrix33.CreateScale(s));

            SetTranslation(t);
        }
Esempio n. 42
0
 public QuatT(Matrix34 m)
 {
     Q = new Quat(m);
     T = m.Translation;
 }
Esempio n. 43
0
 public Quat(Matrix34 matrix)
     : this(new Matrix33(matrix))
 {
 }
Esempio n. 44
0
 public QuatT(Matrix34 m)
 {
     Q = new Quat(m);
     T = m.Translation;
 }
Esempio n. 45
0
 public QuatT(Matrix34 m)
 {
     Q = new Quat(m);
     T = m.GetTranslation();
 }
Esempio n. 46
0
 public QuatT(Matrix34 m)
 {
     Q = new Quat(m);
     T = m.GetTranslation();
 }