|
publicstaticvoid Canshu7(Point3d[] p1, Point3d[] p2, int PointCount, refdouble rotax, refdouble rotay, refdouble rotaz, refdouble scale, refdouble dx, refdouble dy, refdouble dz) { double[,] B1 = newdouble[PointCount * 3, 7]; Matrix B = newMatrix(B1); double[,] dx1 = newdouble[7, 1];//V=B*X-L double[,] L = newdouble[PointCount * 3, 1]; double[,] BT = newdouble[7, PointCount * 3]; double[,] N = newdouble[7, 7]; double[,] InvN = newdouble[7, 7]; double[,] BTL = newdouble[7, 1]; //初始化L矩阵 for(int i=0;i<PointCount*3;i++) { if(i%3==0) { L[i, 0] = p2[i / 3].X; } elseif(i%3==1) { L[i, 0] = p2[i / 3].Y; } elseif (i % 3 == 2) { L[i, 0] = p2[i / 3].Z; } } //初始化B矩阵 for (int i = 0; i < PointCount * 3; i++) { if(i%3==0) { B1[i, 0] = 1; B1[i, 1] = 0; B1[i, 2] = 0; B1[i, 3] = p1[i/3].X; B1[i, 4] = 0; B1[i, 5] = -p1[i/3].Z; B1[i, 6] = p1[i/3].Y;
} elseif(i%3==1) { B1[i, 0] = 0; B1[i, 1] = 1; B1[i, 2] = 0; B1[i, 3] = p1[i / 3].Y; B1[i, 4] = p1[i/3].Z; B1[i, 5] = 0; B1[i, 6] = -p1[i / 3].X; } elseif (i % 3 == 2) { B1[i, 0] = 0; B1[i, 1] = 0; B1[i, 2] = 1; B1[i, 3] = p1[i / 3].Z; B1[i, 4] = -p1[i / 3].Y; B1[i, 5] = p1[i/3].X; B1[i, 6] = 0; }
} //转置 B.MatrixInver(B1,ref BT); //法方程矩阵 //N=BT*B B.MatrixMultiply(BT,B1,ref N); //求逆 InvN=B.MatrixOpp(N); //BTL=BT*L B.MatrixMultiply(BT,L,ref BTL); //dx1=invN*BTL; B.MatrixMultiply(InvN,BTL,ref dx1); // dx = Math.Round(dx1[0, 0], 6); dy = Math.Round(dx1[1, 0], 6); dz = Math.Round(dx1[2, 0], 6); scale = Math.Round(dx1[3, 0], 6); rotax = Math.Round(dx1[4, 0] / dx1[3, 0], 6); rotay = Math.Round(dx1[5, 0] / dx1[3, 0], 6); rotaz = Math.Round(dx1[6, 0] / dx1[3, 0], 6); } |
|
|
