16,472
社区成员
发帖
与我相关
我的任务
分享
double R[9];
double U=0.0,V=0.0,W=0.0;
double *L=new double[8];
double *A=new double[8*6];
double *AT=new double[];
double *N=new double[];
double *ATL=new double[];
double *K=new double[];
do{
//旋转矩阵R
R[0]=cos(fi)*cos(kappa)-sin(fi)*sin(omega)*sin(kappa);
R[1]=-cos(fi)*sin(kappa)-sin(fi)*sin(omega)*cos(kappa);
R[2]=-sin(fi)*cos(omega);
R[3]=cos(omega)*sin(kappa);
R[4]=cos(omega)*cos(kappa);
R[5]=-sin(omega);
R[6]=sin(fi)*cos(kappa)+cos(fi)*sin(omega)*sin(kappa);
R[7]=-sin(fi)*sin(kappa)+cos(fi)*sin(omega)*cos(kappa);
R[8]=cos(fi)*cos(omega);
for(i=0;i<number;i++)
{
//辅助参数计算
U=R[0]*(X[i]-XS)+R[3]*(Y[i]-YS)+R[6]*(Z[i]-ZS);
V=R[1]*(X[i]-XS)+R[4]*(Y[i]-YS)+R[7]*(Z[i]-ZS);
W=R[2]*(X[i]-XS)+R[5]*(Y[i]-YS)+R[8]*(Z[i]-ZS);
//常数项
L[2*i]=ximage[i]+f*U/W;
L[2*i+1]=yimage[i]+f*V/W;
//误差方程系数阵
A[2*6*i+0]=(R[0]*f+R[2]*x)/W;
A[2*6*i+1]=(R[3]*f+R[5]*x)/W;
A[2*6*i+2]=(R[6]*f+R[8]*x)/W;
A[2*6*i+3]=y*sin(omega)-cos(omega)*(x*(x*cos(kappa)-y*sin(kappa))/f+f*cos(kappa));
A[2*6*i+4]=-f*sin(kappa)-x*(x*sin(kappa)+y*cos(kappa))/f;
A[2*6*i+5]=y;
A[2*6*i+6+0]=(R[1]*f+R[2]*x)/W;
A[2*6*i+6+1]=(R[4]*f+R[5]*x)/W;
A[2*6*i+6+2]=(R[7]*f+R[8]*x)/W;
A[2*6*i+6+3]=-x*sin(omega)-(y*(x*cos(kappa)-y*sin(kappa))/f-f*sin(kappa))*cos(omega);
A[2*6*i+6+4]=-f*cos(kappa)-y*(x*sin(kappa)+y*cos(kappa))/f;
A[2*6*i+6+5]=-x;
}
//解法方程
rm.MatrixTranpose(A,AT,8,6);
rm.MatrixMul(AT,A,N,6,8,6);
rm.invers_matrix(N,6);
rm.MatrixMul(N,AT,K,6,6,8);
rm.MatrixMul(K,L,XX,6,8,1);
//
XS+=XX[0];
YS+=XX[1];
ZS+=XX[2];
fi+=XX[3];
omega+=XX[4];
kappa+=XX[5];
count++;
}
while(XX[3]>(0.1/60*3.14159265354/180)&&XX[4]>(0.1/60*3.14159265354/180)&&XX[5]>(0.1/60*3.14159265354/180));
delete[] L;
delete[] A;
delete[] AT;
delete[] ATL;
delete[] N;
delete[] K;
#include "stdafx.h"
#include "example.h"
#include "matrix.h"
#include "ModalDlg.h"
#include "math.h"
#include "malloc.h"
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
matrix::matrix()
{
}
matrix::~matrix()
{
}
//转置
void matrix::MatrixTranpose(double *A,double *B,int m,int n)
{
int i,j;
for(i=0;i<n;i++)
for(j=0;j<m;j++)
B[i*m+j]=A[j*n+i];
return ;
}
//求逆
int matrix::invers_matrix(double *m1,int n)
{
int *is,*js;
int i,j,k,l,u,v;
double temp,max_v;
is=new int(n*sizeof(int));
js=new int(n*sizeof(int));
for(k=0;k<n;k++)
{
max_v=0.0;
for(i=k;i<n;i++)
for(j=k;j<n;j++)
{
temp=fabs(m1[i*n+j]);
if(temp>max_v)
{
max_v=temp; is[k]=i; js[k]=j;
}
}
if(max_v==0.0)
{
delete is; delete js;
AfxMessageBox("error **not inverse",MB_OK);
return(0);
}
if(is[k]!=k)
for(j=0;j<n;j++)
{
u=k*n+j; v=is[k]*n+j;
temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
}
if(js[k]!=k)
for(i=0;i<n;i++)
{
u=i*n+k; v=i*n+js[k];
temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
}
l=k*n+k;
m1[l]=1.0/m1[l];
for(j=0;j<n;j++)
if(j!=k)
{
u=k*n+j;
m1[u]*=m1[l];
}
for(i=0;i<n;i++)
if(i!=k)
for(j=0;j<n;j++)
if(j!=k)
{
u=i*n+j;
m1[u]-=m1[i*n+k]*m1[k*n+j];
}
for(i=0;i<n;i++)
if(i!=k)
{
u=i*n+k;
m1[u]*=-m1[l];
}
}
for(k=n-1;k>=0;k--)
{
if(js[k]!=k)
for(j=0;j<n;j++)
{
u=k*n+j; v=js[k]*n+j;
temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
}
if(is[k]!=k)
for(i=0;i<n;i++)
{
u=i*n+k; v=i*n+is[k];
temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
}
}
delete is; delete js;
return(1);
}
//乘法
void matrix::MatrixMul(double *A,double *B,double *C,int m,int n,int z)
{
int i,j,k;
for(i=0;i<m;i++)
{
for(j=0;j<z;j++)
{
C[i*z+j]=0;
for(k=0;k<n;k++)
C[i*z+j]+=A[i*n+k]*B[k*z+j];
}
}
return;
}
我把程序上传了,你能帮我看看吗 http://download.csdn.net/detail/claire_zhaoj/5336481 都加上try catch,也有初始化不正确的可能。
死的时候,直接看堆栈不就行了?至于前面说的堆坏的问题,先保证其他问题解决了再说,
都加上try catch,也有初始化不正确的可能。