坐标系转换算法：从7参数到旋转矩阵

public class CoordTrans7Param
    {
        public double[,] values = new double[7, 1];
        //{{dx},{dy},{dz},{rx},{ry},{rz},{k}};
        //public double dx,dy,dz,rx,ry,rz,k;
        public void Set4Param(double dx, double dy, double dz, double k)
        {
            this.dx = dx;
            this.dy = dy;
            this.dz = dz;
            this.k = k;
            this.rx = this.ry = this.rz = 0;
        }
        public void SetRotationParamRad(double rx, double ry, double rz)
        {
            this.rx = rx;
            this.ry = ry;
            this.rz = rz;
        }
        public void SetRotationParamMM(double rx, double ry, double rz)
        {
            SetRotationParamRad(rx * Math.PI / 648000, ry * Math.PI / 648000, rz * Math.PI / 648000);
        }
        private double[,] GetMx()
        {
            double[,] Mx = new double[,]{{1,0,0},{0,Math.Cos(rx),Math.Sin(rx)},{0,-Math.Sin(rx),Math.Cos(rx)}};
            return Mx;
        }
        private double[,] GetMy()
        {

            double[,] My = new double[,]{{Math.Cos(ry),0,-Math.Sin(ry)},{0,1,0},{Math.Sin(ry),0,Math.Cos(ry)}};      
            return My;
        }
        private double[,] GetMz()
        {
            double[,] Mz = new double[,]{{Math.Cos(rz),Math.Sin(rz),0},{-Math.Sin(rz),Math.Cos(rz),0},{0,0,1}};
            return Mz;
        }

        private double[,] GetM() //M=Mx*My*Mz? or M=Mz*My*Mx?
        {
            double[,] M = new double[3, 3];

            MatrixTool.Multi(GetMz(), GetMy(), ref M);
            MatrixTool.Multi(M, GetMx(), ref M);
            return M;
        }
        private double[,] GetMdx()
        {
            double[,] mt = {{ 0, 0, 0 },{ 0, -Math.Sin(rx), Math.Cos(rx) },{ 0, -Math.Cos(rx), -Math.Sin(rx) }};

            double[,] m = new double[3, 3];

            MatrixTool.Multi(GetMz(), GetMy(), ref m);
            MatrixTool.Multi(m, mt, ref m);
            return m;
        }
        private double[,] GetMdy()
        {
            double[,] mt = {{ -Math.Sin(ry), 0, -Math.Cos(ry) },{ 0, 0, 0 },{ Math.Cos(ry), 0, -Math.Sin(ry) }};