#include<stdio.h>
#include<math.h>
#define PI 3.1415926535897932
#define N 10 //控制點個數最多十個
/*矩陣轉置函式(MResult = MOrigin(T)),引數說明
*MOrigin -原始矩陣,以一維陣列形式存盤,m行n列
*MResult -轉置后矩陣,以一維陣列形式存盤,n行m列
*/
void Transpose(double *MOrigin,double *MResult,int m, int n)
{
int i,j;
for(i=0; j<n; i++)
for(j=0; j<m; j++)
MResult[i*m+j] = MOrigin[j*n+i];
}
/*矩陣求逆函式(Metrix = Metrix(-1)),引數說明;
*Metrix -原始矩陣,也是求逆之后的矩陣,以一維陣列形式存盤,n行n列
*****/
void Inverse (double *Metrix, int n)
{
int i, j ,k;
for(k=0; k<n; k++)
{
for(i=0; i<n; i++)
{
if(i != k)
Metrix[i*n+k] = - Metrix[i*n+k] / Metrix[i*n+k];
}
Metrix[i*n+k] = 1/Metrix[i*n+k];
for(i=0; i<n; i++)
{
if(i != k)
{
for(j=0; j<n; j++)
{
if(j !=k)
Metrix[i*n+j] += Metrix[k*n+j]* Metrix[i*n+k];
}
}
}
for(j=0; j<n; j++)
{
if(j != k)
Metrix[k*n+j] *=Metrix[k*n+k];
}
}
}
/*******/
void Multiply(double *MOrigin1, double*MOrigin2, double *MResult, int m, int n, int l)
{
int i, j, k;
for(i=0 ; i<m; i++)
for(j=0; j<l; j++)
{
MResultP[i*l+j] = 0.0;
for(k=0; k<n; k++)
MResultP[i*l+j] += MOrigin1[i*n+k] * MOrigin2[j+k*l];
}
}
/*******/
void Minus(double *MOrigin1, double*MOrigin2, double *MResult, int m, int n)
{
int i,j;
for (i=0; i<m; i++)
for (j=0; j<n; j++)
MResultP[n*i+j] = MOrigin1[n*i+j] - MOrigin2[n*i+j];
}
/***主函式***/
void main()
{
int i,m, n = 4,nCount = 0;
double phi, omega, lappa, Xs,Ys,Zs;
double x0, y0, f, Sx = 0.0, Sy = 0.0;
double m0 = 0.0;
double x[N]={-86.15, -56.40, -14.78, 10.46},
y[N]= {-68.99, 82.21, -76.63, 64.43};
double X[N]= {36589.41,37631.08, 39100.97, 40426.54},
y[N]= {25273.32, 31324.51, 24934.98, 30319.81},
z[N]= {2195.17, 728.69, 2386.50, 757.31};
/*
double x[N]= {-1.5930, -10.8900, 94.8513, 58.5874, 89.4396, 0.7293},
y[N] = {56.7500, -70.1450, 84.6073, -95.0100, 1.1978, 85.8260};
double x[N] = {4795.0363, 4780.9533, 5055.6107, 4974.7324, 5043.3627, 4798.8008},
Y[N]= {4303786.2012, 4303431.5728, 4303867.3983, 4303372.1529, 4303638.8498, 4303867.1120},
Z[N]= {68.7139, 57.9388, 89.7012, 70.7783, 101.5542, 71.2536};
*/
double H[6]= {1,1,1,1,1,1}, a[3],b[3], c[3], X_[N], Y_[N], Z_[N],
A[12*N], B[12*N], V[2*N], L[2*N], C[36], D[6], E[2*N], M[1];
x0 = y0 = 0.0;
m = 10000;
f = 153.24;
//m = 2700;
//f = 303.6400;
for(i=0;i<n;i++)
{
x[i] = x[i]/1000.0;
y[i] = y[i]/1000.0;
Sx += x[i];
Sy += y[i];
}
phi = omega = kappa = 0.0;
Xs = Sx/n;
Ys = Sy/n;
f = f/1000.0;
Zs = m*f;
while(fabs(H[3])>0.000000001 || fabs(H[4])>0.00000001 || fabs(H[5])>0.000000001)
{
a[0] = cos(phi)*cos(kappa) - sin(phi)*sin(omega)*sin(kappa);
a[1] = -cos(phi)*sin(kappa) - sin(phi)*sin(omega)*cos(kappa);
a[2] = - sin(phi)*cos(omega);
b[0] = cos(omega)*sin(kappa);
b[1] = cos(omega)*cos(kappa);
b[2] = -sin(omega);
c[0] = sin(phi)*cos(kappa) + cos(phi)*sin(omega)*sin(kappa);
c[1] = -sin(phi)*sin(kappa) + cos(phi)*sin(omega)*cos(kappa);
c[2] = cos(phi)*cos(moega);
for (i=0; i<n; i++)
{
X_[i] = x0 -f*(a[0]*(X[i]-Xs) + b[0]*(Y[i]-Ys) + c[0]*(z[i]-Zs))/
(a[2]*(X[i]-Xs) + b[2]*(Y[i]-Ys) + c[2]*(z[i]-Zs));
Y_[i] = y0 -f*(a[1]*(X[i]-Xs)+b[1]*(Y[i]-Ys)+c[1]*(z[i]-Zs))/
(a[2]*(X[i]-Xs) + b[2]*(Y[i]-Ys) + c[2]*(z[i]-Zs));
Z_[i] = a[2]*(X[i]-Xs) + b[2]*(Y[i]-Ys) + c[2]*(z[i]-Zs);
A[12*i+0] = (a[0]*f + a[2]*(x[i]-x0)) / Z_[i];
A[12*i+1] = (b[0]*f + b[2]*(x[i]-x0)) / Z_[i];
A[12*i+2] = (c[0]*f + c[2]*(x[i]-x0)) / Z_[i];
A[12*i+3] = (y[i]-y0)*sin(omega) - ((x[i]-x0)*((x[i]-x0)*cos(kappa) - (y[i]-y0)*sin(kappa))/f + f*cos(kappa))*cos(omega);
A[12*i+4] = -f*sin(kappa) - (x[i]-x0)*((x[i]-x0)*sin(kappa) + (y[i]-y0)*cos(kappa))/f;
A[12*i+5] = (y[i]-y0);
A[12*i+6] = (a[1]*f+a[2]*(y[i]-y0)) /Z_[i];
A[12*i+7] = (b[1]*f+b[2]*(y[i]-y0)) /Z_[i];
A[12*i+8] = (c[1]*f+c[2]*(y[i]-y0)) /Z_[i];
A[12*i+9] = -(x[i]-x0)*sin(omega) - ((y[i]-y0)*((x[i]-x0)*cos(kappa) - (y[i]-y0)*sin(kappa))/f - f*sin(kappa)*cos(omega);
A[12*i+10] = -f*cos(kappa) - (y[i]-y0)*((x[i]-x0)*sin(kappa) + (y[i]-y0)*cos(kappa))/f;
A[12*i+11] = -(x[i]-x0);
L[2*i] = x[i] - X_[i];
L[2*i+1] = y[i] - Y_[i];
}
Transpose(A, B, 2*n, 6);
Multiply(B, A, C, 6, 2*n, 6);
Multiply(B, L, D, 6, 2*n, 1);
Inverse(C, 6);
Multiply(C, D, H, 6, 6, 1);
Xs +=H[0];
Ys +=H[1];
Zs +=H[2];
phi +=H[3];
omega +=H[4];
kappa +=H[5];
nCount++;
}
Multiply(A, H, E, 2*n, 6, 1);
Minus(E, L, V, M, 2*N,1,1);
m0 = sqrt(M[0]/(2.0*n-6.0));
printf("六個外方位元素:\n");
printf("Xs = %.4f\n",Xs);
printf("Ys = %.4f\n",Ys);
printf("Zs = %.4f\n",Zs);
//printf("phi = %.10f\n",phi);
//printf("omega = %.10f\n",omega);
//printf("kappa = %.10f\n",kappa);
int degree[3],minute[3],second[3];
degree[0]-(int)(phi*180.0/PI);
mintute[0]-(int)((phi*180.0/PI-degree[0])*60.0);
second[0]-(int)(((phi*180.0/PI-degree[0])*60.0-minute[0])*60.0);
degree[1]-(int)(phi*180.0/PI);
mintute[1]-(int)((phi*180.0/PI-degree[1])*60.0);
second[1]-(int)(((phi*180.0/PI-degree[1])*60.0-minute[1])*60.0);
degree[2]-(int)(phi*180.0/PI);
mintute[2]-(int)((phi*180.0/PI-degree[2])*60.0);
second[2]-(int)(((phi*180.0/PI-degree[2])*60.0-minute[2])*60.0);
printf("phi-%d.%d%d(度.分秒)\n",degree[0],minute[0],second[0]);
printf("omega-%d.%d%d(度.分秒)\n",degree[1],minute[1],second[1]);
printf("kappa-%d.%d%d(度.分秒)\n",degree[2],minute[2],second[2]);
printf("\n單位權中誤差為:m0-%.10f\n",m0);
printf("\n六個外方位元素的中誤差為:\n");
for(i=0;i<6;i++)
printf("m%d-%.10f\n",i+1,m0*sqrt(C[i*6+i]));
printf("\n旋轉矩陣R為:\n");
for (i=0;i<3;i++)
printf("%.5f",a[i]);
printf("\n");
for (i=0;i<3;i++)
printf("%.5f",b[i]);
printf("\n");
for (i=0;i<3;i++)
printf("%.5f",c[i]);
printf("\n\n");
printf("迭代次數為:nCount-%d\n",nCount);
}
生成結果顯示:
1>------ 已啟動生成: 專案: 000, 配置: Debug Win32 ------
1> 00.cpp
1>f:\000\000\00.cpp(55): error C2065: “MResultP”: 未宣告的識別符號
1>f:\000\000\00.cpp(57): error C2065: “MResultP”: 未宣告的識別符號
1>f:\000\000\00.cpp(66): error C2065: “MResultP”: 未宣告的識別符號
1>f:\000\000\00.cpp(80): error C2374: “y”: 重定義;多次初始化
1> f:\000\000\00.cpp(77) : 參見“y”的宣告
1>f:\000\000\00.cpp(105): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(113): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(113): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(114): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(114): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(116): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(117): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(119): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(119): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(120): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(120): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(121): error C2065: “moega”: 未宣告的識別符號
1>f:\000\000\00.cpp(125): error C2065: “Y”: 未宣告的識別符號
1>f:\000\000\00.cpp(126): error C2065: “Y”: 未宣告的識別符號
1>f:\000\000\00.cpp(127): error C2065: “Y”: 未宣告的識別符號
1>f:\000\000\00.cpp(128): error C2065: “Y”: 未宣告的識別符號
1>f:\000\000\00.cpp(129): error C2065: “Y”: 未宣告的識別符號
1>f:\000\000\00.cpp(133): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(133): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(133): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(134): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(134): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(134): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(139): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(139): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(139): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(139): error C2143: 語法錯誤 : 缺少“)”(在“;”的前面)
1>f:\000\000\00.cpp(140): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(140): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(140): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(158): error C2065: “kappa”: 未宣告的識別符號
1>f:\000\000\00.cpp(163): error C2660: “Minus”: 函式不接受 7 個引數
1>f:\000\000\00.cpp(173): warning C4552: “-”: 運算子不起任何作用;應輸入帶副作用的運算子
1>f:\000\000\00.cpp(174): error C2065: “mintute”: 未宣告的識別符號
1>f:\000\000\00.cpp(175): warning C4552: “-”: 運算子不起任何作用;應輸入帶副作用的運算子
1>f:\000\000\00.cpp(176): warning C4552: “-”: 運算子不起任何作用;應輸入帶副作用的運算子
1>f:\000\000\00.cpp(177): error C2065: “mintute”: 未宣告的識別符號
1>f:\000\000\00.cpp(178): warning C4552: “-”: 運算子不起任何作用;應輸入帶副作用的運算子
1>f:\000\000\00.cpp(179): warning C4552: “-”: 運算子不起任何作用;應輸入帶副作用的運算子
1>f:\000\000\00.cpp(180): error C2065: “mintute”: 未宣告的識別符號
1>f:\000\000\00.cpp(181): warning C4552: “-”: 運算子不起任何作用;應輸入帶副作用的運算子
========== 生成: 成功 0 個,失敗 1 個,最新 0 個,跳過 0 個 ==========
uj5u.com熱心網友回復:
double phi, omega, lappa, Xs,Ys,Zs;phi = omega = kappa = 0.0;
代碼好長,看到了打錯變數名。其它的錯誤提示說的很清楚,對照著找
轉載請註明出處,本文鏈接:https://www.uj5u.com/houduan/93096.html
標籤:C++ 語言
上一篇:按鈕顏色
