Ejemplo n.º 1
0
        /// <summary>
        /// Creates a matrix that represents a spherical linear interpolation from one
        /// matrix to another.
        /// </summary>
        /// <remarks>
        /// <para>This is an implementation of interpolation without quaternions.</para>
        /// <para>
        /// Given two orthonormal 3x3 matrices this function calculates the shortest
        /// possible interpolation-path between the two rotations. The interpolation curve
        /// forms the shortest great arc on the rotation sphere (geodesic).
        /// </para>
        /// <para>Angular velocity of the interpolation is constant.</para>
        /// <para>Possible stability problems:</para>
        /// <para>
        /// There are two singularities at angle = 0 and angle = <see cref="Math.PI"/> .
        /// At 0 the interpolation-axis is arbitrary, which means any axis will produce
        /// the same result because we have no rotation. (1,0,0) axis is used in this
        /// case. At <see cref="Math.PI"/> the rotations point away from each other and
        /// the interpolation-axis is unpredictable. In this case axis (1,0,0) is used as
        /// well. If the angle is ~0 or ~PI, then a very small vector has to be normalized
        /// and this can cause numerical instability.
        /// </para>
        /// </remarks>
        /// <param name="m">First matrix.</param>
        /// <param name="n">Second matrix.</param>
        /// <param name="t">Interpolation parameter.</param>
        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 Vector3(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 Vector3(1, 0, 0);
            }

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

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

            this.M03 = m.M03 * (1 - t) + n.M03 * t;
            this.M13 = m.M13 * (1 - t) + n.M13 * t;
            this.M23 = m.M23 * (1 - t) + n.M23 * t;
        }
Ejemplo n.º 2
0
        /// <summary>
        /// Creates a new matrix which columns are initialized with values supplied by
        /// given vectors.
        /// </summary>
        /// <param name="vx">
        /// <see cref="Vector3"/> object that provides values for the first column.
        /// </param>
        /// <param name="vy">
        /// <see cref="Vector3"/> object that provides values for the second column.
        /// </param>
        /// <param name="vz">
        /// <see cref="Vector3"/> object that provides values for the third column.
        /// </param>
        /// <param name="vw">
        /// <see cref="Vector3"/> object that provides values for the fourth column.
        /// </param>
        /// <returns>
        /// New matrix which columns are initialized with values supplied by given
        /// vectors.
        /// </returns>
        public static Matrix34 CreateFromColumns(Vector3 vx, Vector3 vy, Vector3 vz, Vector3 vw)
        {
            var matrix = new Matrix34();

            matrix.SetColumns(vx, vy, vz, vw);

            return(matrix);
        }
Ejemplo n.º 3
0
        /// <summary>
        /// Creates a matrix that represents transformations represented by given
        /// arguments.
        /// </summary>
        /// <param name="s"><see cref="Vector3"/> object that represents scale.</param>
        /// <param name="q">
        /// <see cref="Quaternion"/> object that represents rotation.
        /// </param>
        /// <param name="t">
        /// Optional <see cref="Vector3"/> object that represents translation.
        /// </param>
        /// <returns>
        /// Matrix that represents transformations represented by given arguments.
        /// </returns>
        public static Matrix34 Create(Vector3 s, Quaternion q, Vector3 t = default(Vector3))
        {
            var matrix = new Matrix34();

            matrix.Set(s, q, t);

            return(matrix);
        }
Ejemplo n.º 4
0
        /// <summary>
        /// Creates a rotation matrix around an arbitrary axis (Eulers Theorem).
        /// </summary>
        /// <param name="c">   Cosine of the angle of the rotation.</param>
        /// <param name="s">   Sine of the angle of the rotation.</param>
        /// <param name="axis">
        /// <see cref="Vector3"/> object that represents axis of rotation.
        /// </param>
        /// <param name="t">   Optional translation vector.</param>
        /// <returns>A new rotation matrix around an arbitrary axis.</returns>
        public static Matrix34 CreateRotationAroundAxis(float c, float s, Vector3 axis, Vector3 t = default(Vector3))
        {
            var matrix = new Matrix34();

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

            return(matrix);
        }
Ejemplo n.º 5
0
        /// <summary>
        /// Creates a matrix that represents a spherical linear interpolation from one
        /// matrix to another.
        /// </summary>
        /// <param name="m">First matrix.</param>
        /// <param name="n">Second matrix.</param>
        /// <param name="t">Interpolation parameter.</param>
        /// <returns>
        /// Matrix which transformations are interpolated between given matrices.
        /// </returns>
        public static Matrix34 CreateSlerp(Matrix34 m, Matrix34 n, float t)
        {
            var matrix = new Matrix34();

            matrix.SetSlerp(m, n, t);

            return(matrix);
        }
Ejemplo n.º 6
0
        /// <summary>
        /// Creates a matrix that represents rotation represented by given Euler angles.
        /// </summary>
        /// <param name="rad">Angles of rotation.</param>
        /// <param name="t">  Optional translation vector.</param>
        /// <returns>
        /// A new matrix that represents rotation represented by given Euler angles.
        /// </returns>
        public static Matrix34 CreateRotationWithEulerAngles(EulerAngles rad, Vector3 t = default(Vector3))
        {
            var matrix = new Matrix34();

            matrix.SetRotationWithEulerAngles(rad, t);

            return(matrix);
        }
Ejemplo n.º 7
0
        /// <summary>
        /// Creates a matrix that represents a translation without any other
        /// transformations.
        /// </summary>
        /// <param name="v">
        /// <see cref="Vector3"/> object that represents translation.
        /// </param>
        /// <returns>
        /// Matrix that represents a translation without any other transformations.
        /// </returns>
        public static Matrix34 CreatePureTranslation(Vector3 v)
        {
            var matrix = new Matrix34();

            matrix.SetPureTranslation(v);

            return(matrix);
        }
Ejemplo n.º 8
0
        /// <summary>
        /// Creates a matrix that represents a rotation around Z axis.
        /// </summary>
        /// <param name="rad">Angle of rotation in radians.</param>
        /// <param name="t">  Optional translation vector.</param>
        /// <returns>A new matrix that represents a rotation around Z axis.</returns>
        public static Matrix34 CreateRotationAroundZ(float rad, Vector3 t = default(Vector3))
        {
            var matrix = new Matrix34();

            matrix.SetRotationAroundZ(rad, t);

            return(matrix);
        }
Ejemplo n.º 9
0
        /// <summary>
        /// Creates a rotation matrix around an arbitrary axis (Eulers Theorem).
        /// </summary>
        /// <param name="rot">
        /// <see cref="Vector3"/> object which length represents an angle of rotation and
        /// which direction represents an axis of rotation.
        /// </param>
        /// <param name="t">  Optional translation vector.</param>
        /// <returns>A new rotation matrix.</returns>
        public static Matrix34 CreateRotationAroundAxis(Vector3 rot, Vector3 t = default(Vector3))
        {
            var matrix = new Matrix34();

            matrix.SetRotationAroundAxis(rot, t);

            return(matrix);
        }
Ejemplo n.º 10
0
        /// <summary>
        /// Creates a matrix that represents scaling.
        /// </summary>
        /// <param name="s"><see cref="Vector3"/> object that represents scale.</param>
        /// <param name="t">
        /// Optional <see cref="Vector3"/> object that represents translation.
        /// </param>
        /// <returns>Matrix that represents scaling.</returns>
        public static Matrix34 CreateScale(Vector3 s, Vector3 t = default(Vector3))
        {
            var matrix = new Matrix34();

            matrix.SetScale(s, t);

            return(matrix);
        }
Ejemplo n.º 11
0
 /// <summary>
 /// Sets the values of this matrix to one that represents a rotation around an
 /// axis.
 /// </summary>
 /// <param name="c">   Cosine of the angle of the rotation.</param>
 /// <param name="s">   Sine of the angle of the rotation.</param>
 /// <param name="axis">
 /// <see cref="Vector3"/> object that represents axis of rotation.
 /// </param>
 /// <param name="t">   Optional translation vector.</param>
 public void SetRotationAroundAxis(float c, float s, Vector3 axis, Vector3 t = default(Vector3))
 {
     this = new Matrix34(Matrix33.CreateRotationAroundAxis(c, s, axis))
     {
         M03 = t.X,
         M13 = t.Y,
         M23 = t.Z
     };
 }
Ejemplo n.º 12
0
 /// <summary>
 /// Creates new instance of <see cref="Matrix44" /> class.
 /// </summary>
 /// <param name="m"> <see cref="Matrix34" /> to fill new matrix with. </param>
 public Matrix44(ref Matrix34 m)
     : this()
 {
     if (!m.IsValid)
     {
         throw new ArgumentException("Parameter must be a valid matrix.");
     }
     this.Row0.X = m.Row1.X; this.Row0.Y = m.Row1.Y; this.Row0.Z = m.Row1.Z; this.Row0.W = m.Row1.W;
     this.Row1.X = m.Row2.X; this.Row1.Y = m.Row2.Y; this.Row1.Z = m.Row2.Z; this.Row1.W = m.Row2.W;
     this.Row2.X = m.Row2.X; this.Row2.Y = m.Row2.Y; this.Row2.Z = m.Row2.Z; this.Row2.W = m.Row2.W;
     this.Row3.X = 0; this.Row3.Y = 0; this.Row3.Z = 0; this.Row3.W = 1;
 }
Ejemplo n.º 13
0
        /// <summary>
        /// Creates new instance of <see cref="Matrix33"/> struct.
        /// </summary>
        /// <param name="m">
        /// <see cref="Matrix34"/> object from which first 3 columns will be copied.
        /// </param>
        /// <returns>New instance of <see cref="Matrix33"/> struct.</returns>
        public Matrix33(Matrix34 m)
            : this()
        {
            this.M00 = m.M00;
            this.M01 = m.M01;
            this.M02 = m.M02;

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

            this.M20 = m.M20;
            this.M21 = m.M21;
            this.M22 = m.M22;
        }
Ejemplo n.º 14
0
 /// <summary>
 /// Determines whether this matrix can be considered equal to another one within
 /// bounds of given precision.
 /// </summary>
 /// <param name="m">Another matrix.</param>
 /// <param name="e">
 /// <see cref="Single"/> object that represents maximal difference between two
 /// values that allows to consider them equal.
 /// </param>
 /// <returns>
 /// True, if difference between values of each corresponding pair is less then
 /// <paramref name="e"/> .
 /// </returns>
 public bool IsEquivalent(Matrix34 m, float e = 0.05f)
 {
     return
         (
         (Math.Abs(this.M00 - m.M00) <= e) &&
         (Math.Abs(this.M01 - m.M01) <= e) &&
         (Math.Abs(this.M02 - m.M02) <= e) &&
         (Math.Abs(this.M03 - m.M03) <= e) &&
         (Math.Abs(this.M10 - m.M10) <= e) &&
         (Math.Abs(this.M11 - m.M11) <= e) &&
         (Math.Abs(this.M12 - m.M12) <= e) &&
         (Math.Abs(this.M13 - m.M13) <= e) &&
         (Math.Abs(this.M20 - m.M20) <= e) &&
         (Math.Abs(this.M21 - m.M21) <= e) &&
         (Math.Abs(this.M22 - m.M22) <= e) &&
         (Math.Abs(this.M23 - m.M23) <= e));
 }
Ejemplo n.º 15
0
        /// <summary>
        /// </summary>
        /// <param name="vx"> </param>
        /// <param name="vy"> </param>
        /// <param name="vz"> </param>
        /// <param name="pos"></param>
        public void SetFromVectors(Vector3 vx, Vector3 vy, Vector3 vz, Vector3 pos)
        {
            var m34 = new Matrix34
            {
                M00 = vx.X,
                M01 = vy.X,
                M02 = vz.X,
                M03 = pos.X,
                M10 = vx.Y,
                M11 = vy.Y,
                M12 = vz.Y,
                M13 = pos.Y,
                M20 = vx.Z,
                M21 = vy.Z,
                M22 = vz.Z,
                M23 = pos.Z
            };

            this = new QuaternionTranslation(m34);
        }
Ejemplo n.º 16
0
 /// <summary>
 /// Creates new instance of <see cref="Quaternion"/> struct.
 /// </summary>
 /// <param name="matrix">
 /// 3x4 matrix that contains representation of rotation.
 /// </param>
 public Quaternion(Matrix34 matrix)
     : this(new Matrix33(matrix))
 {
 }
Ejemplo n.º 17
0
 /// <summary>
 /// Creates new instance of type <see cref="QuaternionTranslation"/>.
 /// </summary>
 /// <param name="m">
 /// <see cref="Matrix34"/> that represents both translation and orientation.
 /// </param>
 public QuaternionTranslation(Matrix34 m)
 {
     this.Q = new Quaternion(m);
     this.T = m.Translation;
 }
Ejemplo n.º 18
0
        /// <summary>
        /// Convert three Euler angles to 3x3 matrix (rotation order:XYZ)
        /// </summary>
        /// <param name="rad">Angles of rotation.</param>
        /// <param name="t">  Optional translation vector.</param>
        public void SetRotationWithEulerAngles(EulerAngles rad, Vector3 t = default(Vector3))
        {
            this = new Matrix34(Matrix33.CreateRotationFromAngles(rad));

            this.SetTranslation(t);
        }
Ejemplo n.º 19
0
        /// <summary>
        /// Sets the values of this matrix to one that represents a rotation around Z
        /// axis.
        /// </summary>
        /// <param name="rad">Angle of rotation in radians.</param>
        /// <param name="t">  Optional translation vector.</param>
        public void SetRotationAroundZ(float rad, Vector3 t = default(Vector3))
        {
            this = new Matrix34(Matrix33.CreateRotationAroundZ(rad));

            this.SetTranslation(t);
        }
Ejemplo n.º 20
0
        /// <summary>
        /// Sets the values of this matrix to one that represents a rotation around an
        /// axis.
        /// </summary>
        /// <param name="rot">
        /// <see cref="Vector3"/> object which length represents an angle of rotation and
        /// which direction represents an axis of rotation.
        /// </param>
        /// <param name="t">  Optional translation vector.</param>
        public void SetRotationAroundAxis(Vector3 rot, Vector3 t = default(Vector3))
        {
            this = new Matrix34(Matrix33.CreateRotationAroundAxis(rot));

            this.SetTranslation(t);
        }
Ejemplo n.º 21
0
        /// <summary>
        /// Sets the value of the matrix to one that represents scaling.
        /// </summary>
        /// <param name="s"><see cref="Vector3"/> object that represents scale.</param>
        /// <param name="t">
        /// Optional <see cref="Vector3"/> object that represents translation.
        /// </param>
        public void SetScale(Vector3 s, Vector3 t = default(Vector3))
        {
            this = new Matrix34(Matrix33.CreateScale(s));

            this.SetTranslation(t);
        }