单向空间后方交会源程序

for (j=0; j<=n-1; j++)
if (j!=k)
{
u=i*n+j;
a[u]=a[u]-a[i*n+k]*a[k*n+j];
}
for (i=0; i<=n-1; i++)
y_tmp[i]=-f/Z_assistance[i]*(R[0][1]*(X[i]-Xs)+R[1][1]*(Y[i]-Ys)+R[2][1]*(Z[i]-Zs));
}
for (i=0;i<4;i++)
{
A[i*2][0]=(R[0][0]*f+R[0][2]*(x[i]))/Z_assistance[i];//计算各点相应线性系数
{
u=k*n+j;
v=js[k]*n+j;
p=a[u];
a[u]=a[v];
a[v]=p;
}
if (is[k]!=k)
for (i=0; i<=n-1; i++)
{
u=i*n+k;
v=i*n+is[k];
{
R[0][0]=cos(Fai)*cos(Kab)-sin(Fai)*sin(Omg)*sin(Kab);//计算旋转矩阵
R[0][1]=-cos(Fai)*sin(Kab)-sin(Fai)*sin(Omg)*cos(Kab);
R[0][2]=-sin(Fai)*cos(Omg);
for (j=k; j<=n-1; j++)
{
l=i*n+j;
p=fabs(a[l]);
if (p>d)
{
d=p; is[k]=i; js[k]=j;}
}
if (d+1.0==1.0) { free(is);
#include <math.h>
//#include <stdlib.h>
#include <iostream>
using namespace std;
double Average(double const *Data,int num)//求均值
{
double sum=0;
for (int i=0;i<num;i++)
}
if (js[k]!=k)
for (i=0; i<=n-1; i++)
{
u=i*n+k;
v=i*n+js[k];
p=a[u];
a[u]=a[v];
a[v]=p;
}
l=k*n+k;
free(js);
return(0);
}
if (is[k]!=k)
for (j=0; j<=n-1; j++)
{
u=k*n+j;
v=is[k]*n+j;
p=a[u];
a[u]=a[v];
a[v]=p;
p=a[u];
a[u]=a[v];
a[v]=p;
}
}
free(is);
free(js);
return(1);
}
void main()
{
const double x[4]={-0.08615,-0.0534,-0.01478,0.01046},//已知数据
cout<<"迭代阈值条件:\nfabs(X_correction[0])<0.000001\nfabs(X_correction[1])<0.000001\nfabs(X_correction[2])<0.000001\nfabs(X_correction[3])<0.001\nfabs(X_correction[4])<0.001\nfabs(X_correction[5])<0.001\n"<<endl<<endl;
A[i*2][1]=(R[1][0]*f+R[1][2]*(x[i]))/Z_assistance[i];
A[i*2][2]=(R[2][0]*f+R[2][2]*(x[i]))/Z_assistance[i];
A[i*2][3]=y[i]*sin(Omg)-cos(Omg)*(x[i]/f*(x[i]*cos(Kab)-y[i]*sin(Kab))+f*cos(Kab));
for (int j=0;j<c;j++)
{
*(Result+c*i+j)=0;
for (int k=0;k<b;k++)
*(Result+c*i+j)+=(*(A+i*b+k))*(*(B+k*c+j));
}
}
int brinv(double a[], int n) //矩阵求逆，来自网络
{
sum+=*(Data+i);
}
return sum/num;
}
void MatrixMultiply(double const *A,double const *B,double *Result,int a,int b,int c)//矩阵相乘
{
for (int i=0;i<a;i++)
A[i*2][4]=-f*sin(Kab)-x[i]/f*(x[i]*sin(Kab)+y[i]*cos(Kab));
A[i*2][5]=y[i];
A[i*2+1][0]=(R[0][1]*f+R[0][2]*(y[i]))/Z_assistance[i];
A[i*2+1][1]=(R[1][1]*f+R[1][2]*(y[i]))/Z_assistance[i];
R[2][2]=cos(Fai)*cos(Omg);
for (int i=0;i<4;i++)
{
Z_assistance[i]=(R[0][2]*(X[i]-Xs)+R[1][2]*(Y[i]-Ys)+R[2][2]*(Z[i]-Zs));
x_tmp[i]=-f/Z_assistance[i]*(R[0][0]*(X[i]-Xs)+R[1][0]*(Y[i]-Ys)+R[2][0]*(Z[i]-Zs));//计算像点坐标近似值
A[i*2+1][5]=-x[i];
}
double At[6][8];
for (i=0;i<8;i++)
for (int j=0;j<6;j++)
At[j][i]=A[i][j];//计算A的转置At
MatrixMultiply(&(At[0][0]),&(A[0][0]),&(A_assistance[0][0]),6,8,6);//计算辅助矩阵(At*A)
cout<<"------------------------------------------------------------"<<endl;
for (int T=1;
fabs(X_correction[0])>0.000001||
fabs(X_correction[1])>0.000001||
fabs(X_correction[2])>0.000001||
fabs(X_correction[3])>0.001||
fabs(X_correction[4])>0.001||
fabs(X_correction[5])>0.001
;T++)
if (i!=k)
{
u=i*n+k;
a[u]=-a[u]*a[l];
}
}
for (k=n-1; k>=0; k--)
{
if (js[k]!=k)
for (j=0; j<=n-1; j++)
R[1][0]=sin(Kab)*cos(Omg);
R[1][1]=cos(Kab)*cos(Omg);
R[1][2]=-sin(Omg);
R[2][0]=sin(Fai)*cos(Kab)+cos(Fai)*sin(Omg)*sin(Kab);
R[2][1]=-sin(Fai)*sin(Kab)+cos(Fai)*sin(Omg)*cos(Kab);
y[4]={-0.06899,0.08221,-0.07663,0.06443},
X[4]={36589.41,37631.08,39100.97,40426.54},
Y[4]={25273.32,31324.51,24934.98,30319.81},
Z[4]={2195.17,728.69,2386.5,757.31},
A[i*2+1][2]=(R[2][1]*f+R[2][2]*(y[i]))/Z_assistance[i];
A[i*2+1][3]=-x[i]*sin(Omg)-cos(Omg)*(y[i]/f*(x[i]*cos(Kab)-y[i]*sin(Kab))-f*sin(Kab));