private double[,] GetV(double[,] X, double[,] Y, CoordTrans7Param dpp) { int rowNum = X.GetLength(0); double[,] B, F, A, B2, B3, F2, V; double[,] AT = MatrixTool.Init(6, 3); A = GetA(); MatrixTool.AT(A, ref AT); MatrixTool.MutliConst(ref AT, 1 / (1 + (1 + k) * (1 + k))); F = GetF(X, Y); B = GetB(X); B2 = MatrixTool.Init(3, 7); B3 = MatrixTool.Init(3, 1); F2 = MatrixTool.Init(rowNum, 1); for (int i = 0; i < rowNum / 3; i++) { MatrixTool.CopySub(B, i * 3, 0, 3, 7, ref B2, 0, 0); MatrixTool.Multi(B2, dpp.values, ref B3); MatrixTool.CopySub(B3, 0, 0, 3, 1, ref F2, i * 3, 0); } MatrixTool.Sub(F, F2, ref F2); V = specialMulti(AT, F2); return(V); }
private double[,] GetB(double[,] X) { int rowNum = X.GetLength(0); double[,] B = MatrixTool.Init(rowNum, 7); double[,] M = GetM(); double[,] Mdx = GetMdx(); double[,] Mdy = GetMdy(); double[,] Mdz = GetMdz(); double[,] mi = MatrixTool.Ident(3); double[,] MX, MY, MZ, MK; MK = specialMulti(M, X); MX = specialMulti(Mdx, X); MY = specialMulti(Mdy, X); MZ = specialMulti(Mdz, X); for (int i = 0; i < rowNum; i += 3) { MatrixTool.CopySub(mi, 0, 0, 3, 3, ref B, i, 0); } MatrixTool.CopySub(MX, 0, 0, rowNum, 1, ref B, 0, 3); MatrixTool.CopySub(MY, 0, 0, rowNum, 1, ref B, 0, 4); MatrixTool.CopySub(MZ, 0, 0, rowNum, 1, ref B, 0, 5); MatrixTool.CopySub(MK, 0, 0, rowNum, 1, ref B, 0, 6); return(B); }
private double[,] GetA() { double[,] M = GetM(); double[,] I2 = MatrixTool.Ident(3); double[,] A = MatrixTool.Init(3, 6); MatrixTool.MutliConst(ref I2, -1); MatrixTool.MutliConst(ref M, (1 + k)); MatrixTool.CopySub(M, 0, 0, 3, 3, ref A, 0, 0); MatrixTool.CopySub(I2, 0, 0, 3, 3, ref A, 0, 3); return(A); }
private double[,] specialSub(double[,] m, double[,] X) { int rowNumM = m.GetLength(0); int colNumM = m.GetLength(1); int rowNumX = X.GetLength(0); int colNumX = X.GetLength(1); int lines = rowNumX / rowNumM; double[,] subX = MatrixTool.Init(rowNumM, colNumX); double[,] res = MatrixTool.Init(rowNumX, colNumX); for (int i = 0; i < rowNumX; i += rowNumM) { MatrixTool.CopySub(X, i, 0, rowNumM, colNumX, ref subX, 0, 0); MatrixTool.Sub(m, subX, ref subX); MatrixTool.CopySub(subX, 0, 0, rowNumM, colNumX, ref res, i, 0); } return(res); }
private double[,] specialMulti(double[,] m, double[,] X) { int rowNumM = m.GetLength(0); int colNumM = m.GetLength(1); int rowNumX = X.GetLength(0); int colNumX = X.GetLength(1); int lines = rowNumX / colNumM; double[,] mt = MatrixTool.Init(rowNumM, colNumX); double[,] subX = MatrixTool.Init(colNumM, colNumX); double[,] res = MatrixTool.Init(rowNumM * lines, colNumX); for (int i = 0; i < lines; i++) { MatrixTool.CopySub(X, i * colNumM, 0, colNumM, colNumX, ref subX, 0, 0); MatrixTool.Multi(m, subX, ref mt); MatrixTool.CopySub(mt, 0, 0, rowNumM, colNumX, ref res, i * rowNumM, 0); } return(res); }