Пример #1
0
 /// <summary>
 /// Calculate the transpose of the given matrix
 /// </summary>
 /// <param name="mat">The matrix to transpose</param>
 public static void Transpose(ref Matrix4 mat, ref Matrix4 result)
 {
     result.Row0 = mat.Column0;
     result.Row1 = mat.Column1;
     result.Row2 = mat.Column2;
     result.Row3 = mat.Column3;
 }
Пример #2
0
        /// <summary>
        /// Calculate the inverse of the given matrix
        /// </summary>
        /// <param name="mat">The matrix to invert</param>
        /// <returns>The inverse of the given matrix if it has one, or the input if it is singular</returns>
        /// <exception cref="InvalidOperationException">Thrown if the Matrix4 is singular.</exception>
        public static Matrix4 Invert(Matrix4 mat)
        {
            int[] colIdx = { 0, 0, 0, 0 };
            int[] rowIdx = { 0, 0, 0, 0 };
            int[] pivotIdx = { -1, -1, -1, -1 };

            // convert the matrix to an array for easy looping
            float[,] inverse = {{mat.Row0.x, mat.Row0.y, mat.Row0.z, mat.Row0.w}, 
								{mat.Row1.x, mat.Row1.y, mat.Row1.z, mat.Row1.w}, 
								{mat.Row2.x, mat.Row2.y, mat.Row2.z, mat.Row2.w}, 
								{mat.Row3.x, mat.Row3.y, mat.Row3.z, mat.Row3.w} };
            int icol = 0;
            int irow = 0;
            for (int i = 0; i < 4; i++)
            {
                // Find the largest pivot value
                float maxPivot = 0.0f;
                for (int j = 0; j < 4; j++)
                {
                    if (pivotIdx[j] != 0)
                    {
                        for (int k = 0; k < 4; ++k)
                        {
                            if (pivotIdx[k] == -1)
                            {
                                float absVal = System.Math.Abs(inverse[j, k]);
                                if (absVal > maxPivot)
                                {
                                    maxPivot = absVal;
                                    irow = j;
                                    icol = k;
                                }
                            }
                            else if (pivotIdx[k] > 0)
                            {
                                return mat;
                            }
                        }
                    }
                }

                ++(pivotIdx[icol]);

                // Swap rows over so pivot is on diagonal
                if (irow != icol)
                {
                    for (int k = 0; k < 4; ++k)
                    {
                        float f = inverse[irow, k];
                        inverse[irow, k] = inverse[icol, k];
                        inverse[icol, k] = f;
                    }
                }

                rowIdx[i] = irow;
                colIdx[i] = icol;

                float pivot = inverse[icol, icol];
                // check for singular matrix
                if (pivot == 0.0f)
                {
                    throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
                    //return mat;
                }

                // Scale row so it has a unit diagonal
                float oneOverPivot = 1.0f / pivot;
                inverse[icol, icol] = 1.0f;
                for (int k = 0; k < 4; ++k)
                    inverse[icol, k] *= oneOverPivot;

                // Do elimination of non-diagonal elements
                for (int j = 0; j < 4; ++j)
                {
                    // check this isn't on the diagonal
                    if (icol != j)
                    {
                        float f = inverse[j, icol];
                        inverse[j, icol] = 0.0f;
                        for (int k = 0; k < 4; ++k)
                            inverse[j, k] -= inverse[icol, k] * f;
                    }
                }
            }

            for (int j = 3; j >= 0; --j)
            {
                int ir = rowIdx[j];
                int ic = colIdx[j];
                for (int k = 0; k < 4; ++k)
                {
                    float f = inverse[k, ir];
                    inverse[k, ir] = inverse[k, ic];
                    inverse[k, ic] = f;
                }
            }

            mat.Row0 = new float4(inverse[0, 0], inverse[0, 1], inverse[0, 2], inverse[0, 3]);
            mat.Row1 = new float4(inverse[1, 0], inverse[1, 1], inverse[1, 2], inverse[1, 3]);
            mat.Row2 = new float4(inverse[2, 0], inverse[2, 1], inverse[2, 2], inverse[2, 3]);
            mat.Row3 = new float4(inverse[3, 0], inverse[3, 1], inverse[3, 2], inverse[3, 3]);
            return mat;
        }
Пример #3
0
 /// <summary>
 /// Calculate the transpose of the given matrix
 /// </summary>
 /// <param name="mat">The matrix to transpose</param>
 /// <returns>The transpose of the given matrix</returns>
 public static Matrix4 Transpose(Matrix4 mat)
 {
     return new Matrix4(mat.Column0, mat.Column1, mat.Column2, mat.Column3);
 }
Пример #4
0
        public static void Mult(ref Matrix4 left, ref Matrix4 right, ref Matrix4 result)
        {
            float4 col0 = right.Column0;
            float4 col1 = right.Column1;
            float4 col2 = right.Column2;
            float4 col3 = right.Column3;

            result.Row0 = new float4(left.Row0 * col0, left.Row0 * col1, left.Row0 * col2, left.Row0 * col3);
            result.Row1 = new float4(left.Row1 * col0, left.Row1 * col1, left.Row1 * col2, left.Row1 * col3);
            result.Row2 = new float4(left.Row2 * col0, left.Row2 * col1, left.Row2 * col2, left.Row2 * col3);
            result.Row3 = new float4(left.Row3 * col0, left.Row3 * col1, left.Row3 * col2, left.Row3 * col3);
        }
Пример #5
0
        /// <summary>
        /// Post multiply this matrix by another matrix
        /// </summary>
        /// <param name="right">The matrix to multiply</param>
        /// <returns>A new Matrix44 that is the result of the multiplication</returns>
        public static Matrix4 Mult(Matrix4 left, Matrix4 right)
        {
            float4 col0 = right.Column0;
            float4 col1 = right.Column1;
            float4 col2 = right.Column2;
            float4 col3 = right.Column3;

            left.Row0 = new float4(left.Row0*col0, left.Row0*col1, left.Row0*col2, left.Row0*col3);
            left.Row1 = new float4(left.Row1*col0, left.Row1*col1, left.Row1*col2, left.Row1*col3);
            left.Row2 = new float4(left.Row2*col0, left.Row2*col1, left.Row2*col2, left.Row2*col3);
            left.Row3 = new float4(left.Row3*col0, left.Row3*col1, left.Row3*col2, left.Row3*col3);
            
            return left;
        }
Пример #6
0
        /// <summary>
        /// Build a world space to camera space matrix
        /// </summary>
        /// <param name="eye">Eye (camera) position in world space</param>
        /// <param name="target">Target position in world space</param>
        /// <param name="up">Up vector in world space (should not be parallel to the camera direction, that is target - eye)</param>
        /// <returns>A Matrix that transforms world space to camera space</returns>
        public static Matrix4 LookAt(Vector3f eye, Vector3f target, Vector3f up)
        {
            Vector3f z = Vector3f.Normalize(eye - target);
            Vector3f x = Vector3f.Normalize(Vector3f.Cross(up, z));
            Vector3f y = Vector3f.Normalize(Vector3f.Cross(z, x));

            Matrix4 rot = new Matrix4(new float4(x.x, y.x, z.x, 0.0f),
                                        new float4(x.y, y.y, z.y, 0.0f),
                                        new float4(x.z, y.z, z.z, 0.0f),
                                        float4.UnitW);

            Matrix4 trans = Matrix4.Translation(-eye);

            return trans * rot;
        }
Пример #7
0
        /// <summary>
        /// Build a rotation matrix to rotate about the given axis
        /// </summary>
        /// <param name="axis">the axis to rotate about</param>
        /// <param name="angle">angle in radians to rotate counter-clockwise (looking in the direction of the given axis)</param>
        /// <returns>A rotation matrix</returns>
        public static Matrix4 Rotate(float3 axis, float angle)
        {
            float cos = (float)System.Math.Cos(-angle);
            float sin = (float)System.Math.Sin(-angle);
            float t = 1.0f - cos;

            axis = axis.Normalize;

            Matrix4 result = new Matrix4();
            result.Row0 = new float4(t * axis.x * axis.x + cos, t * axis.x * axis.y - sin * axis.z, t * axis.x * axis.z + sin * axis.y, 0.0f);
            result.Row1 = new float4(t * axis.x * axis.y + sin * axis.z, t * axis.y * axis.y + cos, t * axis.y * axis.z - sin * axis.x, 0.0f);
            result.Row2 = new float4(t * axis.x * axis.z - sin * axis.y, t * axis.y * axis.z + sin * axis.x, t * axis.z * axis.z + cos, 0.0f);
            result.Row3 = float4.UnitW;
            return result;
        }
Пример #8
0
        /// <summary>
        /// Build a rotation matrix that rotates about the z-axis
        /// </summary>
        /// <param name="angle">angle in radians to rotate counter-clockwise around the z-axis</param>
        /// <returns>A rotation matrix</returns>
        public static Matrix4 RotateZ(float angle)
        {
            float cos = (float)System.Math.Cos(angle);
            float sin = (float)System.Math.Sin(angle);

            Matrix4 result = new Matrix4() ;
            result.Row0 = new float4(cos, sin, 0.0f, 0.0f);
            result.Row1 = new float4(-sin, cos, 0.0f, 0.0f);
            result.Row2 = float4.UnitZ;
            result.Row3 = float4.UnitW;
            return result;
        }