double CSpaceResectionDlg::Function(double initXS,double initYS,double initZS,double initalfa,double initw,double initk)
{
UpdateData();
//initXS,initYS,initZS,initalfa,initw,initk的初始值
initXS=(m_nXA+m_nXB+m_nXC+m_nXD)/4;
initYS=(m_nYA+m_nYB+m_nYC+m_nYD)/4;
initZS= m_nf*(1/m_nscale);
initalfa=0.0;
initw=0.0;
initk=0.0;
//以下是书上的公式
double H = m_nf*(1/m_nscale);
double a1 = cos(initalfa)*cos(initk)-sin(initalfa)*sin(initw)*sin(initk);
double a2 = -cos(initalfa)*sin(initk)-sin(initalfa)*sin(initw)*cos(initk);
double a3 = -sin(initalfa)*cos(initw);
double b1 = cos(initw)*sin(initk);
double b2 = cos(initw)*cos(initk);
double b3 = -sin(initw);
double c1 = sin(initalfa)*cos(initk)+cos(initalfa)*sin(initw)*sin(initk);
double c2 = -sin(initalfa)*sin(initk)+cos(initalfa)*sin(initw)*sin(initk);
double c3 = cos(initalfa)*cos(initw);
double a11_1=1/(a3*(m_nXA-initXS)+b3*(m_nYA-initYS)+c3*(m_nZA-initZS))*(a1*m_nf+a3*m_nxa);
double a12_1=1/(a3*(m_nXA-initXS)+b3*(m_nYA-initYS)+c3*(m_nZA-initZS))*(b1*m_nf+b3*m_nxa);
double a13_1=1/(a3*(m_nXA-initXS)+b3*(m_nYA-initYS)+c3*(m_nZA-initZS))*(c1*m_nf+c3*m_nxa);
double a21_1=1/(a3*(m_nXA-initXS)+b3*(m_nYA-initYS)+c3*(m_nZA-initZS))*(a2*m_nf+a3*m_nxa);
double a22_1=1/(a3*(m_nXA-initXS)+b3*(m_nYA-initYS)+c3*(m_nZA-initZS))*(b2*m_nf+b3*m_nxa);
double a23_1=1/(a3*(m_nXA-initXS)+b3*(m_nYA-initYS)+c3*(m_nZA-initZS))*(c2*m_nf+c3*m_nxa);
double a14_1=m_nya*sin(initw)-(m_nxa/m_nf*(m_nxa*cos(initk)-m_nya*sin(initk))+m_nf*cos(initk))*cos(initw);
double a15_1=-m_nf*sin(initk)-m_nxa/m_nf*(m_nxa*sin(initk)+m_nya*cos(initk));
double a16_1=m_nya;
double a24_1=-m_nxa*sin(initw)-(m_nya/m_nf*(m_nxa*cos(initk)-m_nya*sin(initk))-m_nf*sin(initk))*cos(initw);
double a25_1=-m_nf*cos(initk)-m_nya/m_nf*(m_nxa*sin(initk)+m_nya*cos(initk));
double a26_1=-m_nxa;
double a11_2=1/(a3*(m_nXB-initXS)+b3*(m_nYB-initYS)+c3*(m_nZB-initZS))*(a1*m_nf+a3*m_nxb);
double a12_2=1/(a3*(m_nXB-initXS)+b3*(m_nYB-initYS)+c3*(m_nZB-initZS))*(b1*m_nf+b3*m_nxb);
double a13_2=1/(a3*(m_nXB-initXS)+b3*(m_nYB-initYS)+c3*(m_nZB-initZS))*(c1*m_nf+c3*m_nxb);
double a21_2=1/(a3*(m_nXB-initXS)+b3*(m_nYB-initYS)+c3*(m_nZB-initZS))*(a2*m_nf+a3*m_nxb);
double a22_2=1/(a3*(m_nXB-initXS)+b3*(m_nYB-initYS)+c3*(m_nZB-initZS))*(b2*m_nf+b3*m_nxb);
double a23_2=1/(a3*(m_nXB-initXS)+b3*(m_nYB-initYS)+c3*(m_nZB-initZS))*(c2*m_nf+c3*m_nxb);
double a14_2=m_nyb*sin(initw)-(m_nxb/m_nf*(m_nxb*cos(initk)-m_nyb*sin(initk))+m_nf*cos(initk))*cos(initw);
double a15_2=-m_nf*sin(initk)-m_nxb/m_nf*(m_nxb*sin(initk)+m_nyb*cos(initk));
double a16_2=m_nyb;
double a24_2=-m_nxb*sin(initw)-(m_nyb/m_nf*(m_nxb*cos(initk)-m_nyb*sin(initk))-m_nf*sin(initk))*cos(initw);
double a25_2=-m_nf*cos(initk)-m_nyb/m_nf*(m_nxb*sin(initk)+m_nyb*cos(initk));
double a26_2=-m_nxb;
double a11_3=1/(a3*(m_nXC-initXS)+b3*(m_nYC-initYS)+c3*(m_nZC-initZS))*(a1*m_nf+a3*m_nxc);
double a12_3=1/(a3*(m_nXC-initXS)+b3*(m_nYC-initYS)+c3*(m_nZC-initZS))*(b1*m_nf+b3*m_nxc);
double a13_3=1/(a3*(m_nXC-initXS)+b3*(m_nYC-initYS)+c3*(m_nZC-initZS))*(c1*m_nf+c3*m_nxc);
double a21_3=1/(a3*(m_nXC-initXS)+b3*(m_nYC-initYS)+c3*(m_nZC-initZS))*(a2*m_nf+a3*m_nxc);
double a22_3=1/(a3*(m_nXC-initXS)+b3*(m_nYC-initYS)+c3*(m_nZC-initZS))*(b2*m_nf+b3*m_nxc);
double a23_3=1/(a3*(m_nXC-initXS)+b3*(m_nYC-initYS)+c3*(m_nZC-initZS))*(c2*m_nf+c3*m_nxc);
double a14_3=m_nyc*sin(initw)-(m_nxc/m_nf*(m_nxc*cos(initk)-m_nyc*sin(initk))+m_nf*cos(initk))*cos(initw);
double a15_3=-m_nf*sin(initk)-m_nxc/m_nf*(m_nxc*sin(initk)+m_nyc*cos(initk));
double a16_3=m_nyc;
double a24_3=-m_nxc*sin(initw)-(m_nyc/m_nf*(m_nxc*cos(initk)-m_nyc*sin(initk))-m_nf*sin(initk))*cos(initw);
double a25_3=-m_nf*cos(initk)-m_nyc/m_nf*(m_nxc*sin(initk)+m_nyc*cos(initk));
double a26_3=-m_nxc;
double a11_4=1/(a3*(m_nXD-initXS)+b3*(m_nYD-initYS)+c3*(m_nZD-initZS))*(a1*m_nf+a3*m_nxd);
double a12_4=1/(a3*(m_nXD-initXS)+b3*(m_nYD-initYS)+c3*(m_nZD-initZS))*(b1*m_nf+b3*m_nxd);
double a13_4=1/(a3*(m_nXD-initXS)+b3*(m_nYD-initYS)+c3*(m_nZD-initZS))*(c1*m_nf+c3*m_nxd);
double a21_4=1/(a3*(m_nXD-initXS)+b3*(m_nYD-initYS)+c3*(m_nZD-initZS))*(a2*m_nf+a3*m_nxd);
double a22_4=1/(a3*(m_nXD-initXS)+b3*(m_nYD-initYS)+c3*(m_nZD-initZS))*(b2*m_nf+b3*m_nxd);
double a23_4=1/(a3*(m_nXD-initXS)+b3*(m_nYD-initYS)+c3*(m_nZD-initZS))*(c2*m_nf+c3*m_nxd);
double a14_4=m_nyd*sin(initw)-(m_nxd/m_nf*(m_nxd*cos(initk)-m_nyd*sin(initk))+m_nf*cos(initk))*cos(initw);
double a15_4=-m_nf*sin(initk)-m_nxd/m_nf*(m_nxd*sin(initk)+m_nyd*cos(initk));
double a16_4=m_nyd;
double a24_4=-m_nxd*sin(initw)-(m_nyd/m_nf*(m_nxd*cos(initk)-m_nyd*sin(initk))-m_nf*sin(initk))*cos(initw);
double a25_4=-m_nf*cos(initk)-m_nyd/m_nf*(m_nxd*sin(initk)+m_nyd*cos(initk));
double a26_4=-m_nxd;
//矩阵A
double A[8][6]=
{{a11_1,a12_1,a13_1,a14_1,a15_1,a16_1},
{a21_1,a22_1,a23_1,a24_1,a25_1,a26_1},
{a11_2,a12_2,a13_2,a14_2,a15_2,a16_2},
{a21_2,a22_2,a23_2,a24_2,a25_2,a26_2},
{a11_3,a12_3,a13_3,a14_3,a15_3,a16_3},
{a21_3,a22_3,a23_3,a24_3,a25_3,a26_3},
{a11_4,a12_4,a13_4,a14_4,a15_4,a16_4},
{a21_4,a22_4,a23_4,a24_4,a25_4,a26_4}};
//矩阵A的转置矩阵A_zhuan
double A_zhuan[6][8]=
{{a11_1,a21_1,a11_2,a21_2,a11_3,a21_3,a11_4,a21_4},
{a12_1,a22_1,a12_2,a22_2,a12_3,a22_3,a12_4,a22_4},
{a13_1,a23_1,a13_2,a23_2,a13_3,a23_3,a13_4,a23_4},
{a14_1,a24_1,a14_2,a24_2,a14_3,a24_3,a14_4,a24_4},
{a15_1,a25_1,a15_2,a25_2,a15_3,a25_3,a15_4,a25_4},
{a16_1,a26_1,a16_2,a26_2,a16_3,a26_3,a16_4,a26_4},
};
double opprox_xa = -m_nf*(m_nXA-initXS)/(m_nZA-initZS);
double opprox_ya = -m_nf*(m_nYA-initXS)/(m_nZA-initZS);
double opprox_xb = -m_nf*(m_nXB-initXS)/(m_nZB-initZS);
double opprox_yb = -m_nf*(m_nYB-initXS)/(m_nZB-initZS);
double opprox_xc = -m_nf*(m_nXC-initXS)/(m_nZC-initZS);
double opprox_yc = -m_nf*(m_nYC-initXS)/(m_nZC-initZS);
double opprox_xd = -m_nf*(m_nXD-initXS)/(m_nZD-initZS);
double opprox_yd = -m_nf*(m_nYD-initXS)/(m_nZD-initZS);
double l_xa = m_nxa-opprox_xa;
double l_ya = m_nya-opprox_ya;
double l_xb = m_nxb-opprox_xb;
double l_yb = m_nyb-opprox_yb;
double l_xc = m_nxc-opprox_xc;
double l_yc = m_nyc-opprox_yc;
double l_xd = m_nxd-opprox_xd;
double l_yd = m_nyd-opprox_yd;
//矩阵L
double L[8][1]={{l_xa},{l_ya},{l_xb},{l_yb},{l_xc},{l_yc},{l_xd},{l_yd}};
//为矩阵A,A_zhuan,L,B,C,X分配内存
A[100][100];
A_zhuan[100][100];
L[100][100];
double B[100][100];
double C[100][100];
double X[100][100];
brmul((double*)A_zhuan,(double*)A,6,8,6,(double*)B);
//矩阵A的转置矩阵乘以矩阵A,返回矩阵B
brinv((double*)B,6);
//矩阵B的逆
brmul((double*)B,(double*)A_zhuan,6,6,8,(double*)C);
//矩阵B的转置矩阵乘以矩阵A的转置矩阵,返回矩阵C
brmul((double*)C,(double*)L,6,8,1,(double*)X);
//矩阵C乘以矩阵L,返回矩阵X
//计算m_XS,m_YS,m_ZS,m_alfa,m_w,m_k
m_XS = initXS+X[0][0];
m_YS = initYS+X[1][0];
m_ZS = initZS+X[2][0];
m_alfa=initalfa+X[3][0];
m_w = initw+X[4][0];
m_k = initk+X[5][0];
//如果m_alfa或者m_w或者m_k的值大于0.1,
if((fabs(m_alfa)>=0.1)||(fabs(m_w)>=0.1)||(fabs(m_k)>=0.1))
{
initXS+=X[0][0];
initYS+=X[1][0];
initZS+=X[2][0];
initalfa+=X[3][0];
initw+=X[4][0];
initk+=X[5][0];
//递归
Function(initXS,initYS,initZS,initalfa,initw,initk);
return m_XS,m_YS,m_ZS,m_alfa,m_w,m_k;
}
else
return m_XS,m_YS,m_ZS,m_alfa,m_w,m_k;
UpdateData(false);
}
编译运行都没错,就是出不了结果
我用断点看了看,提示:
Unhandled exception in **.exe:0xC00000FD:Stack Overflow
求助~~~~