matrix.cs

利用共线方程,用迭代法求解后方交会的参数

using System;
using System.Collections.Generic;
using System.Text;

namespace WindowsApplication1
{
    class Matrix
    {
      

        public static void Transpose(double[,] a, double[,] b)
        {
            int i, j;
            for (i = 0; i < 6; i++)
            {
                for (j = 0; j < 6; j++)
                {
                    b[i, j] = a[j, i];
                }
            }
        }
        //矩阵的乘法运算
        public  static void Matrix6_6(double[,] a, double[,] b, double[,] c)
        {
            int i, j, k;
            for (i = 0; i < 6; i++)
            {
                for (j = 0; j < 6; j++)
                {
                    c[i, j] = 0;
                    for (k = 0; k < 6; k++)
                        c[i, j] += a[i, k] * b[k, j];
                }
            }
        }
        //矩阵的乘法运算(结果为6*1的矩阵)
        public static  void Matrix6_1(double[,] a, double[,] b, double[,] c)
        {
            int i, j, k;
            for (i = 0; i < 6; i++)
            {
                for (j = 0; j < 1; j++)
                {
                    c[i, j] = 0;
                    for (k = 0; k < 6 ; k++)
                        c[i, j] += a[i, k] * b[k, j];
                }
            }

        }
        //计算逆矩阵
        public static  double[,] ReverseMatrix(double[,] dMatrix, int Level)
        {
            double dMatrixValue = MatrixValue(dMatrix, Level);

            if (dMatrixValue == 0) return null;

            double[,] dReverseMatrix = new double[Level, 2 * Level];

            double x, c;

            // Init Reverse matrix 
            for (int i = 0; i < Level; i++)
            {
                for (int j = 0; j < 2 * Level; j++)
                {
                    if (j < Level)
                        dReverseMatrix[i, j] = dMatrix[i, j];
                    else
                        dReverseMatrix[i, j] = 0;
                }
                dReverseMatrix[i, Level + i] = 1;
            }

            for (int i = 0, j = 0; i < Level && j < Level; i++, j++)
            {
                if (dReverseMatrix[i, j] == 0)
                {
                    int m = i;
                    for (; dMatrix[m, j] == 0; m++) ;
                    if (m == Level)
                        return null;
                    else
                    {
                        // Add i-row with m-row
                        for (int n = j; n < 2 * Level; n++)
                            dReverseMatrix[i, n] += dReverseMatrix[m, n];
                    }
                }
                // Format the i-row with "1" start

                x = dReverseMatrix[i, j];
                if (x != 1)
                {
                    for (int n = j; n < 2 * Level; n++)
                        if (dReverseMatrix[i, n] != 0)
                            dReverseMatrix[i, n] /= x;
                }
                // Set 0 to the current column in the rows after current row
                for (int s = Level - 1; s > i; s--)
                {
                    x = dReverseMatrix[s, j];
                    for (int t = j; t < 2 * Level; t++)
                        dReverseMatrix[s, t] -= (dReverseMatrix[i, t] * x);
                }
            }
            // Format the first matrix into unit-matrix
            for (int i = Level - 2; i >= 0; i--)
            {
                for (int j = i + 1; j < Level; j++)
                    if (dReverseMatrix[i, j] != 0)
                    {
                        c = dReverseMatrix[i, j];
                        for (int n = j; n < 2 * Level; n++)
                            dReverseMatrix[i, n] -= (c * dReverseMatrix[j, n]);
                    }
            }
            double[,] dReturn = new double[Level, Level];
            for (int i = 0; i < Level; i++)
                for (int j = 0; j < Level; j++)
                    dReturn[i, j] = dReverseMatrix[i, j + Level];
            return dReturn;
        }


        public static  double MatrixValue(double[,] MatrixList, int Level)
        {

            double[,] dMatrix = new double[Level, Level];
            for (int i = 0; i < Level; i++)
                for (int j = 0; j < Level; j++)
                    dMatrix[i, j] = MatrixList[i, j];
            double c, x;
            int k = 1;
            for (int i = 0, j = 0; i < Level && j < Level; i++, j++)
            {
                if (dMatrix[i, j] == 0)
                {
                    int m = i;
                    for (; dMatrix[m - 2, j] == 0; m++) ;
                    if (m == Level)
                        return 0;
                    else
                    {
                        // Row change between i-row and m-row
                        for (int n = j; n < Level; n++)
                        {
                            c = dMatrix[i, n];
                            dMatrix[i, n] = dMatrix[m, n];
                            dMatrix[m, n] = c;
                        }
                        // Change value pre-value
                        k *= (-1);
                    }
                }
                // Set 0 to the current column in the rows after current row
                for (int s = Level - 1; s > i; s--)
                {
                    x = dMatrix[s, j];
                    for (int t = j; t < Level; t++)
                        dMatrix[s, t] -= dMatrix[i, t] * (x / dMatrix[i, j]);
                }
            }
            double sn = 1;
            for (int i = 0; i < Level; i++)
            {
                if (dMatrix[i, i] != 0)
                    sn *= dMatrix[i, i];
                else
                    return 0;
            }
            return k * sn;
        }
        
    }
}