单片空间后方交会[C#]

 

 

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

namespace _33
...{
    class Program
    ...{
        public class Data
        ...{
            //内方位元素
            double f = 0.15324, m = 50000,x0,y0;          
            //地面点坐标
            double[] X = new double[3];
            double[] Y = new double[3];
            double[] Z = new double[3];
            //X,Y,Z和
            double SX, SY, SZ;
            //像点坐标
            double[] x = new double[3];
            double[] y = new double[3];
            //x,y近似值
            double[] _x = new double[3];
            double[] _y = new double[3];
            //方便代入公式，定义_Z
            double[] _Z = new double[3];
            //外方位元素
            double XS, YS, ZS, r = 0, w = 0, k = 0;
            //旋转矩阵各值
            double a1, a2, a3, b1, b2, b3, c1, c2, c3;
            //解方程所用到矩阵
            double[,] matrix_A = new double[6, 6];
            double[,] matrix_AT = new double[6, 6];
            double[,] matrix_AA = new double[6, 6];
            double[,] matrix_AAtemp = new double[6, 12];
            double[,] matrix_AAR = new double[6, 6];
            double[,] matrix_AARAT = new double[6, 6];
            double[,] L = new double[6, 1];
            double[,] matrix_X = new double[6, 1];


            public void Input()    //坐标输入及数值初始化
            ...{
                //输入x,y,X,Y,Z坐标
                for (int i = 0; i < 3; i++)
                ...{                
                    Console.WriteLine("======= 输入第{0}个已知点坐标 =======",i+1);
                    Console.WriteLine("<像点坐标> x{0}(mm)",i+1);
                    string userInput_x = Console.ReadLine();
                    x[i] = double.Parse(userInput_x);
                    Console.WriteLine("<像点坐标> y{0}(mm)", i + 1);
                    string userInput_y = Console.ReadLine();
                    y[i] = double.Parse(userInput_y);
                    Console.WriteLine("<地面点坐标> X{0}(m)", i + 1);
                    string userInput_X = Console.ReadLine();
                    X[i] = double.Parse(userInput_X);
                    Console.WriteLine("<地面点坐标> Y{0}(m)", i + 1);
                    string userInput_Y = Console.ReadLine();
                    Y[i] = double.Parse(userInput_Y);

                    Console.WriteLine("<地面点坐标> Z{0}(m)", i + 1);
                    string userInput_Z = Console.ReadLine();
                    Z[i] = double.Parse(userInput_Z);
                    Console.WriteLine();
                    
                    //求X,Y,Z和
                   
                    SX += X[i];
                    SY += Y[i];
                    SZ += Z[i];
                }
                //x,y值换算成m
                for (int i = 0; i < 3; i++)
                ...{
                    x[i] /= 1000;
                    y[i] /= 1000;

                }
                //定义初始值
                x0 = y0 = 0;
                r = w = k = 0;
                XS = SX / 3;
                YS = SY / 3;
                ZS = SZ / 3 + m * f;
            }

            public void Matrix()//计算旋转矩阵R
              ...{
                    a1 = Math.Cos(r) * Math.Cos(k) - Math.Sin(r) * Math.Sin(w) * Math.Sin(k);
                    a2 = -Math.Cos(r) * Math.Sin(k) - Math.Sin(r) * Math.Sin(w) * Math.Cos(k);
                    a3 = -Math.Sin(r) * Math.Cos(w);
                    b1 = Math.Cos(w) * Math.Sin(k);
                    b2 = Math.Cos(w) * Math.Cos(k);
                    b3 = -Math.Sin(w);
                    c1 = Math.Sin(r) * Math.Cos(k) + Math.Cos(r) * Math.Sin(w) * Math.Sin(k);
                    c2 = -Math.Sin(r) * Math.Sin(k) + Math.Cos(r) * Math.Sin(w) * Math.Cos(k);
                    c3 = Math.Cos(r) * Math.Cos(w);

                    for (int i = 0; i < 3; i++)
                    ...{
                        //计算_Z
                        _Z[i] = a3 * (X[i] - XS) + b3 * (Y[i] - YS) + c3 * (Z[i] - ZS);

                        //计算x,y近似值
                        _x[i] = -f * (a1 * (X[i] - XS) + b1 * (Y[i] - YS) + c1 * (Z[i] - ZS)) / (a3 * (X[i] - XS) + b3 * (Y[i] - YS) + c3 * (Z[i] - ZS));
                        _y[i] = -f * (a2 * (X[i] - XS) + b2 * (Y[i] - YS) + c2 * (Z[i] - ZS)) / (a3 * (X[i] - XS) + b3 * (Y[i] - YS) + c3 * (Z[i] - ZS));
                    }

　　                //Matrix_A赋值
                    for (int i = 0; i < 3; i++)
                    ...{
                        matrix_A[2 * i, 0] = 1 / _Z[i] * (a1 * f + a3 * x[i]);
                        matrix_A[2 * i, 1] = 1 / _Z[i] * (b1 * f + b3 * x[i]);
                        matrix_A[2 * i, 2] = 1 / _Z[i] * (c1 * f + c3 * x[i]);
                        matrix_A[2 * i, 3] = y[i] * Math.Sin(w) - (x[i] / f * (x[i] * Math.Cos(k) - y[i] * Math.Sin(k)) + f * Math.Cos(k)) * Math.Cos(w);
                        matrix_A[2 * i, 4] = -f * Math.Sin(k) - x[i] / f * (x[i] * Math.Sin(k) + y[i] * Math.Cos(k));
                        matrix_A[2 * i, 5] = y[i];
                        matrix_A[2 * i + 1, 0] = 1 / _Z[i] * (c2 * f + c3 * y[i]);
                        matrix_A[2 * i + 1, 1] = 1 / _Z[i] * (b2 * f + b3 * y[i]);
                        matrix_A[2 * i + 1, 2] = 1 / _Z[i] * (c2 * f + c3 * y[i]);
                        matrix_A[2 * i + 1, 3] = -x[i] * Math.Sin(w) - (y[i] / f * (x[i] * Math.Cos(k) - y[i] * Math.Sin(k)) - f * Math.Sin(k)) * Math.Cos(w);
                        matrix_A[2 * i + 1, 4] = -f * Math.Cos(k) - y[i] / f * (x[i] * Math.Sin(k) + y[i] * Math.Cos(k));
                        matrix_A[2 * i + 1, 5] = -x[i];
                        L[2 * i, 0] = x[i] - _x[i];
                        L[2 * i + 1, 0] = y[i] - _y[i];
                    }

                    /**/////得到法方程的解

                    ////求matrix_A的转置
                    for (int i = 0; i < 6; i++)
                    ...{

                        for (int j = 0; j < 6; j++)
                        ...{

                            matrix_AT[i, j] = matrix_A[j, i];
                        }
                    }

                    /**/////A的转置与A的乘积,存在matrix_AA中

                    for (int i = 0; i < 6; i++)
                    ...{
                        for (int j = 0; j < 6; j++)
                        ...{
                            matrix_AA[i, j] = 0;

                            for (int vv = 0; vv < 6; vv++)
                            ...{
                                matrix_AA[i, j] += matrix_AT[i, vv] * matrix_A[vv, j];
                            }
                        }
                    }

                    //matrix_AA的逆阵

                    //定义matrix_AAtemp--临时数组

                    for (int vv = 0; vv < 6; vv++)
                    ...{
                        for (int t = 0; t < 12; t++)
                        ...{
                            matrix_AAtemp[vv, t] = 0;
                        }
                    }

                    //把matrix_AA各值赋给matrix_AAtemp

                    for (int i = 0; i < 6; i++)
                    ...{
                        for (int j = 0; j < 6; j++)
                        ...{
                            matrix_AAtemp[i, j] = matrix_AA[i, j];
                        }
                    }

                    /**/////在matrix_AAtemp加入初等方阵
                   
                    for (int v = 0; v < 6; v++)
                    ...{

                        matrix_AAtemp[v, v + 6] = 1;
                    }

                    //初等变换
                    for (int vv = 0; vv < 6; vv++)
                    ...{

                        if (matrix_AAtemp[vv, vv] != 1)
                        ...{

                            double bs = matrix_AAtemp[vv, vv];
                            matrix_AAtemp[vv, vv] = 1;
                            for (int p = vv + 1; p < 12; p++)
                            ...{
                                matrix_AAtemp[vv, p] /= bs;
                            }
                        }
                        for (int q = 0; q < 6; q++)
                        ...{
                            if (q != vv)
                            ...{
                                double bs = matrix_AAtemp[q, vv];
                                for (int p = 0; p < 12; p++)
                                ...{
                                    matrix_AAtemp[q, p] -= bs * matrix_AAtemp[vv, p];
                                }
                            }
                            else
                            ...{
                                continue;
                            }
                        }
                    }

                    //得到matrix_AA的逆阵后存在matrix_AAR

                    for (int i = 0; i < 6; i++)
                    ...{
                        for (int j = 0; j < 6; j++)
                        ...{
                            matrix_AAR[i, j] = matrix_AAtemp[i, j + 6];
                        }
                    }

                    //matrix_AAR * matrix_AT存在matrix_AARAT

                    for (int i = 0; i < 6; i++)
                    ...{
                        for (int j = 0; j < 6; j++)
                        ...{
                            matrix_AARAT[i, j] = 0;
                            for (int vv = 0; vv < 6; vv++)
                            ...{

                                matrix_AARAT[i, j] += matrix_AAR[i, vv] * matrix_AT[vv, j];
                            }
                        }
                    }


                    //matrix_AARAT x L
                    //存在matrix_X中

                    for (int i = 0; i < 6; i++)
                    ...{
                        for (int j = 0; j < 1; j++)
                        ...{
                            matrix_X[i, j] = 0;
                            for (int vv = 0; vv < 6; vv++)
                            ...{
                                matrix_X[i, j] += matrix_AARAT[i, vv] * L[vv, j];
                            }
                        }
                    }
                  
                   //计算外方位元素值
                    XS += matrix_X[0, 0];
                    YS += matrix_X[1, 0];
                    ZS += matrix_X[2, 0];
                    r += matrix_X[3, 0];
                    w += matrix_X[4, 0];
                    k += matrix_X[5, 0];
                                 
 }
            public void Judge(int n,double d) //反复调用Matrix()方法进行迭代,并判断是否满足限差
            ...{   

                if (Math.Abs(matrix_X[3, 0]) >= d || Math.Abs(matrix_X[4, 0]) >= d || Math.Abs(matrix_X[5, 0]) >= d)
                ...{

                    Console.WriteLine("================== 迭代开始 ==================");
                    Console.WriteLine();
                    for (int nu = 0; nu< n; nu++)
                    ...{  
                       Matrix();    
                       Console.WriteLine("======第{0}次迭代 φ, w ,k 值====== ", nu + 1);
                        
                        for (int i = 3; i < 6; i++)
                        ...{
                            for (int j = 0; j < 1; j++)
                            ...{
                                Console.WriteLine(matrix_X[i, j]);

                            }
                        }

                        if (Math.Abs(matrix_X[3, 0]) <= d && Math.Abs(matrix_X[4, 0]) <= d && Math.Abs(matrix_X[5, 0]) <= d)
                        ...{
                            Console.WriteLine("===============在第{0}次迭代改正数值小于限差==============", nu + 1);
                            Console.WriteLine();
                            Console.WriteLine("===========================迭代结束==========================");
                            Console.WriteLine();
                            Console.WriteLine("Enter键显示计算结果......");
                            Console.ReadLine();
                            Output();
                            TestRusults();

                            break;
                        }

                    }
                    if (Math.Abs(matrix_X[3, 0]) >= d || Math.Abs(matrix_X[4, 0]) >= d || Math.Abs(matrix_X[5, 0]) >= d)
                    ...{
                        Console.WriteLine("            -------------------------  错误！-------------------------");
                        Console.WriteLine("已达到指定迭代次数,仍不能满足指定限差要求，程序无法继续执行。");
                        Console.WriteLine();
                        Console.WriteLine("大虾请检查数据后重新来过.");                   
                    }
                }
                else 
                ...{
                    Console.WriteLine();
                    Console.WriteLine("====满足限差要求,无需迭代====");
                    Console.WriteLine();
                    Console.WriteLine("Enter键显示计算结果......");
                    Console.ReadLine();
                    Output();
                    TestRusults();
                }    
                   
                   
               
            

            }
            public void Output()//输出计算结果
            ...{

                Console.WriteLine("================= 计算结果 =================");
                Console.WriteLine("XS={0}",XS);
                Console.WriteLine("YS={0}",YS);
                Console.WriteLine("ZS={0}",ZS);
                Console.WriteLine("φ={0}",r);
                Console.WriteLine("w={0}",w);
                Console.WriteLine("k={0}",k);
                Console.WriteLine();
                Console.WriteLine("Enter键进入第四点检验.....");
                Console.ReadLine();

              }
            public void TestRusults()//测试结果
            ...{
                //像点坐标和地面点坐标
                double t_X, t_Y, t_Z;
                double test_x, test_y; 

                Console.WriteLine("================ 第四点验检验 ================");
                Console.WriteLine("输入地面点坐标 X(m) ");
                string strTest_X = Console.ReadLine();
                t_X = double.Parse(strTest_X);
                Console.WriteLine("输入地面点坐标 Y(m)");
                string strTest_Y = Console.ReadLine();
                t_Y = double.Parse(strTest_Y);
                Console.WriteLine("输入地面点坐标 Z(m)");
                string strTest_Z = Console.ReadLine();
                t_Z = double.Parse(strTest_Z);
                Console.WriteLine();

                test_x = -f * (a1 * (t_X - XS) + b1 * (t_Y - YS) + c1 * (t_Z - ZS)) / (a3 * (t_X - XS) + b3 * (t_Y - YS) + c3 * (t_Z - ZS)) + x0;
                test_y = -f * (a2 * (t_X - XS) + b2 * (t_Y - YS) + c2 * (t_Z - ZS)) / (a3 * (t_X - XS) + b3 * (t_Y - YS) + c3 * (t_Z - ZS)) + y0;

                Console.WriteLine("======== 该点像点坐标(mm) ========");
                Console.WriteLine("x={0}", test_x * 1000);
                Console.WriteLine("y={0}",test_y*1000);
                Console.WriteLine("Enter退出......");
                Console.ReadLine();
                
                
            }

        }//end of public class Data
        public static void Main()//主方法
            ...{
                try
                ...{
                    Console.WriteLine("============ 输入迭代次数  ============");
                    string str_n = Console.ReadLine();
                    int n = int.Parse(str_n);
                    Console.WriteLine("============ 输入限差 ============");
                    string str_d = Console.ReadLine();
                    double d = double.Parse(str_d);
                    //生成实例
                    Data p = new Data();
                    //调用方法
                    p.Input();
                    p.Matrix();
                    p.Judge(n,d);
      
                    
               }
                catch(Exception)
                ...{
                    Console.WriteLine("发生错误，大虾请重试!");
                    Console.ReadLine();
                }  
                

            }

        }//end of class Program
    }

 相关文档：
http://hi.baidu.com/huqing7002/blog/item/3ceb3cd8816e283732fa1c50.html
http://blog.mop.com/bys02201/2006/11/21/2591919.html
http://221.232.129.83/jpkc2005/syclx/4.teaching%20guide/img/houjiao.pdf