matrix.h

用C++编写的摄影测量后方交汇的程序

int inverse(double a[], int n) //求逆函数
{ 
	int *is,*js,i,j,k,l,u,v;
	double d,p; 
	is=(int*)malloc(n*sizeof(int));//代表矩阵的行
	js=(int*)malloc(n*sizeof(int)); //代表矩阵的列
	for (k=0; k<=n-1; k++)
	{ 
		d=0.0;
		for (i=k; i<=n-1; i++)
			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); 
				free(js);
				printf("err**not inv\n");
				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; 
				} 
				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;
					a[l]=1.0/a[l];
					for (j=0; j<=n-1; j++)
						if (j!=k)
						{ 
							u=k*n+j; 
							a[u]=a[u]*a[l];
						} 
						for (i=0; i<=n-1; i++)
							if (i!=k) 
								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++)
							if (i!=k)
							{ 
								u=i*n+k; 
								a[u]=-a[u]*a[l];
							}
	}//结束for (k=0; k<=n-1; k++)
	for (k=n-1; k>=0; k--) 
	{
		if (js[k]!=k)
			for (j=0; j<=n-1; j++)
			{ 
				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];
				p=a[u]; a[u]=a[v]; a[v]=p; 
			} 
	} 
	free(is);
	free(js); 
	return(1); 
} //结束矩阵转置函数inverse()*/