/// <summary>
        /// Transforms the current vector by a matrix.
        /// </summary>
        /// <param name="m">Matrix to transform current vector with.</param>
        public void Mul(ref Matrix4 m)
        {
            float ox = x;
            float oy = y;
            float oz = z;

            x = ox * m.A1 + oy * m.B1 + oz * m.C1 + m.D1;
            y = ox * m.A2 + oy * m.B2 + oz * m.C2 + m.D2;
            z = ox * m.A3 + oy * m.B3 + oz * m.C3 + m.D3;
        }
        /// <summary>
        /// Spherical quadratic interpolation between matrices.
        /// </summary>
        /// <remarks>The matrices are converted to quaternion for every invocation => it is recommended to use the Quaternion.Squad 
        /// method instead.</remarks>
        /// <param name="pre">One matrix before the actual interpolation matrices.</param>
        /// <param name="a">First matrix.</param>
        /// <param name="b">Second matrix.</param>
        /// <param name="post">One matrix after the actual interpolation matrices.</param>
        /// <param name="time">The interpolation time.</param>
        /// <returns>The interpolated matrix.</returns>
        public static Matrix4 Squad(Matrix4 pre, Matrix4 a, Matrix4 b, Matrix4 post, float time)
        {
            Quaternion qPre = pre.Quaternion;
            Quaternion qa = a.Quaternion;
            Quaternion qb = b.Quaternion;
            Quaternion qPost = post.Quaternion;

            Quaternion interpolated = //Quaternion.Slerp(qa, qb, time);
              Quaternion.SimpleSquad(qPre, qa, qb, qPost, time);

            Matrix4 result = interpolated.Matrix4;

            //result.TranslationVector = CRSpline.Interpolate(time, pre.TranslationVector,
             // a.TranslationVector, b.TranslationVector, post.TranslationVector);
            return result;
        }
 //---------------------------------------------------------------
 /// <summary>
 /// Adds the weighted skinned source vector.
 /// </summary>
 public void AddSkinned(Vector3 sourceVec, ref Matrix4 joint, float weight)
 {
     x += (sourceVec.x * joint.A1 + sourceVec.y * joint.B1 + sourceVec.z * joint.C1 + joint.D1) * weight;
     y += (sourceVec.x * joint.A2 + sourceVec.y * joint.B2 + sourceVec.z * joint.C2 + joint.D2) * weight;
     z += (sourceVec.x * joint.A3 + sourceVec.y * joint.B3 + sourceVec.z * joint.C3 + joint.D3) * weight;
 }
 /// <summary>
 /// builds a rotation matrix for a given vector and an angle
 /// </summary>
 /// <param name="vec">to rotate around</param>
 /// <param name="angle">rotation angle</param>
 /// <returns>rotation matrix</returns>
 public static Matrix4 Rotation(Vector3 vec, float angle)
 {
     float c = Math.Trigonometry.Cos(angle);
     float s = Math.Trigonometry.Sin(angle);
     float t = 1 - c;
     Matrix4 m =
             new Matrix4(t * vec.X * vec.X + c, t * vec.X * vec.Y + s * vec.Z, t * vec.X * vec.Z + s * vec.Y, 0,
                                             t * vec.X * vec.Y - s * vec.Z, t * vec.Y * vec.Y + c, t * vec.Y * vec.Z + s * vec.X, 0,
                                             t * vec.X * vec.Z + s * vec.Y, t * vec.Y * vec.Z - s * vec.X, t * vec.Z * vec.Z + c, 0,
                                                                                         0, 0, 0, 1);
     return Matrix4.Transpose(m);
 }
        /// <summary>
        /// Spherical linear interpolation between to matrices.
        /// </summary>
        /// <param name="a">First matrix.</param>
        /// <param name="b">Second matrix.</param>
        /// <param name="time">Interpolation time [0..1].</param>
        /// <returns>Interpolated matrix.</returns>
        public static Matrix4 Slerp(Matrix4 a, Matrix4 b, float time)
        {
            Quaternion qa = a.Quaternion;
            Quaternion qb = b.Quaternion;

            Quaternion interpolated = Quaternion.Slerp(qa, qb, time);

            Matrix4 result = interpolated.Matrix4;
            result.TranslationVector = Vector3.Lerp(a.TranslationVector, b.TranslationVector, time);
            return result;
        }
 /// <summary>
 /// get 3x3 matrix from 4x4 matrix without specified column and row
 /// </summary>
 /// <param name="source">source matrix (4x4)</param>
 /// <param name="column">column to remove</param>
 /// <param name="row">row to remove</param>
 /// <returns>3x3 matrix from 4x4 matrix without specified column and row</returns>
 public static Matrix3 Minor(Matrix4 source, int column, int row)
 {
     int r = 0;
     Matrix3 result = new Matrix3();
     for (int iRow = 0; iRow < 4; iRow++)
     {
         int c = 0;
         if (iRow != row)
         {
             for (int iColumn = 0; iColumn < 4; iColumn++)
             {
                 if (iColumn != column)
                 {
                     result[c, r] = source[iColumn, iRow];
                     c++;
                 }
             }
             r++;
         }
     }
     return result;
 }
        /// <summary>
        /// calculates the inverse matrix of a source matrix
        /// </summary>
        /// <param name="m">to calculate inverse matrix from</param>
        /// <returns>inverse matrix</returns>
        public static Matrix4 Invert(Matrix4 m)
        {
            // not that efficient at the moment - however, who cares?
            //Matrix4 inverse = Matrix4.Adjoint(m) / m.Det();

            // Hmm perhaps I do care ;-) This is about 50% faster
            Matrix4 inverse = Matrix4.Zero;
            inverse.A1 = m.B3 * m.C4 * m.D2 - m.B4 * m.C3 * m.D2 + m.B4 * m.C2 * m.D3 - m.B2 * m.C4 * m.D3 - m.B3 * m.C2 * m.D4 + m.B2 * m.C3 * m.D4;
            inverse.A2 = m.A4 * m.C3 * m.D2 - m.A3 * m.C4 * m.D2 - m.A4 * m.C2 * m.D3 + m.A2 * m.C4 * m.D3 + m.A3 * m.C2 * m.D4 - m.A2 * m.C3 * m.D4;
            inverse.A3 = m.A3 * m.B4 * m.D2 - m.A4 * m.B3 * m.D2 + m.A4 * m.B2 * m.D3 - m.A2 * m.B4 * m.D3 - m.A3 * m.B2 * m.D4 + m.A2 * m.B3 * m.D4;
            inverse.A4 = m.A4 * m.B3 * m.C2 - m.A3 * m.B4 * m.C2 - m.A4 * m.B2 * m.C3 + m.A2 * m.B4 * m.C3 + m.A3 * m.B2 * m.C4 - m.A2 * m.B3 * m.C4;

            inverse.B1 = m.B4 * m.C3 * m.D1 - m.B3 * m.C4 * m.D1 - m.B4 * m.C1 * m.D3 + m.B1 * m.C4 * m.D3 + m.B3 * m.C1 * m.D4 - m.B1 * m.C3 * m.D4;
            inverse.B2 = m.A3 * m.C4 * m.D1 - m.A4 * m.C3 * m.D1 + m.A4 * m.C1 * m.D3 - m.A1 * m.C4 * m.D3 - m.A3 * m.C1 * m.D4 + m.A1 * m.C3 * m.D4;
            inverse.B3 = m.A4 * m.B3 * m.D1 - m.A3 * m.B4 * m.D1 - m.A4 * m.B1 * m.D3 + m.A1 * m.B4 * m.D3 + m.A3 * m.B1 * m.D4 - m.A1 * m.B3 * m.D4;
            inverse.B4 = m.A3 * m.B4 * m.C1 - m.A4 * m.B3 * m.C1 + m.A4 * m.B1 * m.C3 - m.A1 * m.B4 * m.C3 - m.A3 * m.B1 * m.C4 + m.A1 * m.B3 * m.C4;

            inverse.C1 = m.B2 * m.C4 * m.D1 - m.B4 * m.C2 * m.D1 + m.B4 * m.C1 * m.D2 - m.B1 * m.C4 * m.D2 - m.B2 * m.C1 * m.D4 + m.B1 * m.C2 * m.D4;
            inverse.C2 = m.A4 * m.C2 * m.D1 - m.A2 * m.C4 * m.D1 - m.A4 * m.C1 * m.D2 + m.A1 * m.C4 * m.D2 + m.A2 * m.C1 * m.D4 - m.A1 * m.C2 * m.D4;
            inverse.C3 = m.A2 * m.B4 * m.D1 - m.A4 * m.B2 * m.D1 + m.A4 * m.B1 * m.D2 - m.A1 * m.B4 * m.D2 - m.A2 * m.B1 * m.D4 + m.A1 * m.B2 * m.D4;
            inverse.C4 = m.A4 * m.B2 * m.C1 - m.A2 * m.B4 * m.C1 - m.A4 * m.B1 * m.C2 + m.A1 * m.B4 * m.C2 + m.A2 * m.B1 * m.C4 - m.A1 * m.B2 * m.C4;

            inverse.D1 = m.B3 * m.C2 * m.D1 - m.B2 * m.C3 * m.D1 - m.B3 * m.C1 * m.D2 + m.B1 * m.C3 * m.D2 + m.B2 * m.C1 * m.D3 - m.B1 * m.C2 * m.D3;
            inverse.D2 = m.A2 * m.C3 * m.D1 - m.A3 * m.C2 * m.D1 + m.A3 * m.C1 * m.D2 - m.A1 * m.C3 * m.D2 - m.A2 * m.C1 * m.D3 + m.A1 * m.C2 * m.D3;
            inverse.D3 = m.A3 * m.B2 * m.D1 - m.A2 * m.B3 * m.D1 - m.A3 * m.B1 * m.D2 + m.A1 * m.B3 * m.D2 + m.A2 * m.B1 * m.D3 - m.A1 * m.B2 * m.D3;
            inverse.D4 = m.A2 * m.B3 * m.C1 - m.A3 * m.B2 * m.C1 + m.A3 * m.B1 * m.C2 - m.A1 * m.B3 * m.C2 - m.A2 * m.B1 * m.C3 + m.A1 * m.B2 * m.C3;
            return inverse * (1 / m.Det());
        }
 /// <summary>
 /// Returns an array of matrices.
 /// </summary>
 /// <param name="data">The data stream containing the matrices.</param>
 /// <param name="offset">The offset in the stream to start with.</param>
 /// <param name="count">The number of matrices to load.</param>
 /// <returns>The array of matrices.</returns>
 public static unsafe Matrix4[] From(byte[] data, int offset, int count)
 {
     Matrix4[] ret = new Matrix4[count];
     fixed (byte* p = &data[offset])
     {
         float* floatPtr = (float*)(p);
         for (int i = 0; i < count; i++)
         {
             ret[i] = new Matrix4(
               floatPtr[0], floatPtr[1], floatPtr[2], floatPtr[3],
               floatPtr[4], floatPtr[5], floatPtr[6], floatPtr[7],
               floatPtr[8], floatPtr[9], floatPtr[10], floatPtr[11],
               floatPtr[12], floatPtr[13], floatPtr[14], floatPtr[15]);
             floatPtr += 16;
         }
     }
     return ret;
 }
 /// <summary>
 /// returns the adjoint matrix
 /// </summary>
 /// <param name="source">matrix to calculate adjoint matrix from</param>
 /// <returns></returns>
 public static Matrix4 Adjoint(Matrix4 source)
 {
     Matrix4 result = Matrix4.Zero;
     for (int iRow = 0; iRow < 4; iRow++)
         for (int iCol = 0; iCol < 4; iCol++)
         {
             if (((iCol + iRow) % 2) == 0)
                 result[iCol, iRow] = Matrix4.Minor(source, iRow, iCol).Det();
             else
                 result[iCol, iRow] = -Matrix4.Minor(source, iRow, iCol).Det();
         }
     return result;
 }
Beispiel #10
0
 /// <summary>
 /// returns the transposed matrix
 /// </summary>
 /// <param name="matrix">matrix to transpose</param>
 /// <returns>the transposed matrix</returns>
 public static Matrix4 Transpose(Matrix4 matrix)
 {
     Matrix4 m = Matrix4.Zero;
     m.A1 = matrix.A1; m.A2 = matrix.B1; m.A3 = matrix.C1; m.A4 = matrix.D1;
     m.B1 = matrix.A2; m.B2 = matrix.B2; m.B3 = matrix.C2; m.B4 = matrix.D2;
     m.C1 = matrix.A3; m.C2 = matrix.B3; m.C3 = matrix.C3; m.C4 = matrix.D3;
     m.D1 = matrix.A4; m.D2 = matrix.B4; m.D3 = matrix.C4; m.D4 = matrix.D4;
     return m;
 }