#include <iostream>
#include <math.h>
#include <cstdio>
using namespace std;
class Matrix{
public :
double Val[10][10];
int Column,Row;
void show(){
puts("****");
cout<<"Row = "<<Row<<" Column = "<<Column<<endl;
puts("-----------------");
for(int i = 1 ; i <= Row ; i++){
for(int j = 1 ; j <= Column ; j++){
printf("%.3lf\t%c",Val[i][j],j==Column?'\n':' ');
}
}
puts("****");
}
void init(){
cout<<"gets row and column first"<<endl;
cin>>Row>>Column;
cout<<"gets values now"<<endl;
for(int i = 1 ; i <= Row ; i++){
for(int j = 1 ; j <= Column ; j++){
cin>>Val[i][j];
}
}
}
Matrix Transpose(){
Matrix ret;
ret.Column=Row;
ret.Row=Column;
for(int i=1 ; i<=Row ; i++){
for(int j=1 ; j<=Column ; j++){
ret.Val[j][i]=Val[i][j];
}
}
return ret;
}
Matrix Multiply(Matrix a){
Matrix ret;
ret.Row=Row;
ret.Column=a.Column;
for(int i=1 ; i<= ret.Row ; i++){
for(int j=1 ; j<= ret.Column ; j++){
double tmp=0;
for(int k=1 ; k<= Column ; k++){
tmp+=Val[i][k]*a.Val[k][j];
}
ret.Val[i][j]=tmp;
}
}
return ret;
}
//ȡ���ֵ�����Ƿ��������������
double Absolute(){
double ret=0;
if(Row==2){
ret=Val[1][1]*Val[2][2]-Val[1][2]*Val[2][1];
return ret;
}
else{
Matrix tmp[Row+1][Row+1];
for(int i=1 ; i<= Row ; i++){
for(int j=1 ; j<= Column ; j++){
int pre;
if((i+j)
else pre=-1;
tmp[i][j].Row=Row-1;
tmp[i][j].Column=Column-1;
for(int ii=1,xi=1 ; ii <= Row ; ii++){
if(ii==i)continue;
for(int jj=1,xj=1 ; jj <= Column ; jj++){
if(jj==j)continue;
tmp[i][j].Val[xi][xj]=Val[ii][jj];
xj++;
}
xi++;
}
// cout<<" i = "<<i<<" j = "<<j<<endl;
// tmp[i][j].show();
ret+=pre*Val[i][j]*tmp[i][j].Absolute();
}
}
return ret/Row;
}
}
Matrix Adjoint_matrix (){
Matrix ret;
Matrix tmp[Row+1][Row+1];
ret.Row=Row;
ret.Column=Column;
for(int i=1 ; i<= Row ; i++){
for(int j=1 ; j<= Column ; j++){
double values;
int pre;
if((i+j)
else pre=-1;
tmp[i][j].Row=Row-1;
tmp[i][j].Column=Column-1;
for(int ii=1,xi=1 ; ii <= Row ; ii++){
if(ii==i)continue;
for(int jj=1,xj=1 ; jj <= Column ; jj++){
if(jj==j)continue;
tmp[i][j].Val[xi][xj]=Val[ii][jj];
xj++;
}
xi++;
}
// cout<<" i = "<<i<<" j = "<<j<<endl;
// tmp[i][j].show();
values=pre*tmp[i][j].Absolute();
ret.Val[i][j]=values;
}
}
//��÷��ˣ���֪��Ϊʲô���Ժ��ٿ�
return ret.Transpose();
}
Matrix Inverse (Matrix a){
Matrix ret=a.Adjoint_matrix();
double ab=a.Absolute();
for(int i=1 ; i<= Row ; i++){
for(int j=1 ; j<= Row ; j++){
ret.Val[i][j]/=ab;
}
}
return ret;
}
Matrix operator = (Matrix x){
Row=x.Row;
Column=x.Column;
for(int i=1 ; i<= Row ; i++){
for(int j=1 ; j<= Column ; j++){
Val[i][j]=x.Val[i][j];
}
}
}
};
class Resection{
private:
double m,x0,y0,f;
double x[5],y[5],X[5],Y[5],Z[5];
double k1,k2,k3;
double Zs0,Xs0,H,Ys0;
double ans1,ans2,ans3,ans4,ans5,ans6;
Matrix R,A,L,matrix_X;
double _x[5],_y[5];
public:
void init(){
cout<<"gets m , x0 , y0 , f ,first!"<<endl;
cout<<"unit 1 , mm , mm , mm "<<endl;
cin>>m>>x0>>y0>>f;
f=f/1000;
cout<<"gets x[] , y[] , X[] , Y[] , Z[],second!"<<endl;
cout<<"unit mm , mm , m , m , m !"<<endl;
for(int i=1 ; i<= 4 ; i++){
printf("gets x[%d] , y[%d] , X[%d] , Y[%d] , Z[%d] now\n",i,i,i,i,i);
scanf("%lf%lf%lf%lf%lf",&x[i],&y[i],&X[i],&Y[i],&Z[i]);
x[i]=x[i]/1000;
y[i]=y[i]/1000;
}
k1=0,k2=0,k3=0;
H=m*f;
Zs0=H;
Xs0=(X[1]+X[2]+X[3]+X[4])/4;
Ys0=(Y[1]+Y[2]+Y[3]+Y[4])/4;
ans1=Xs0;
ans2=Ys0;
ans3=Zs0;
ans4=k1;
ans5=k2;
ans6=k3;
}
void work(){
R.Column=3,R.Row=3;
R.Val[1][1]=cos(k1)*cos(k3)-sin(k1)*sin(k2)*sin(k3);
R.Val[1][2]=-cos(k1)*sin(k3)-sin(k1)*sin(k2)*cos(k3);
R.Val[1][3]=-sin(k1)*cos(k2);
R.Val[2][1]=cos(k2)*sin(k3);
R.Val[2][2]=cos(k2)*cos(k3);
R.Val[2][3]=-sin(k2);
R.Val[3][1]=sin(k1)*cos(k3)+cos(k1)*sin(k2)*sin(k3);
R.Val[3][2]=-sin(k1)*sin(k3)+cos(k1)*sin(k2)*cos(k3);
R.Val[3][3]=cos(k1)*cos(k2);
for(int i=1 ; i<=4 ; i++){
_x[i]=-f*(R.Val[1][1]*(X[i]-Xs0)+R.Val[2][1]*(Y[i]-Ys0)+R.Val[3][1]*(Z[i]-Zs0))
/(R.Val[1][3]*(X[i]-Xs0)+R.Val[2][3]*(Y[i]-Ys0)+R.Val[3][3]*(Z[i]-Zs0));
_y[i]=-f*(R.Val[1][2]*(X[i]-Xs0)+R.Val[2][2]*(Y[i]-Ys0)+R.Val[3][2]*(Z[i]-Zs0))
/(R.Val[1][3]*(X[i]-Xs0)+R.Val[2][3]*(Y[i]-Ys0)+R.Val[3][3]*(Z[i]-Zs0));
}
for(int i=1 ; i<=4 ; i++){
A.Column=6;A.Row=8;
A.Val[i*2-1][1]=-f/H;
A.Val[i*2-1][2]=0;
A.Val[i*2-1][3]=-x[i]/H;
A.Val[i*2-1][4]=-f-x[i]*x[i]/f;
A.Val[i*2-1][5]=-x[i]*y[i]/f;
A.Val[i*2-1][6]=y[i];
A.Val[i*2][1]=0;
A.Val[i*2][2]=-f/H;
A.Val[i*2][3]=-y[i]/H;
A.Val[i*2][4]=-x[i]*y[i]/f;
A.Val[i*2][5]=-f-y[i]*y[i]/f;
A.Val[i*2][6]=-x[i];
L.Column=1;L.Row=8;
L.Val[i*2-1][1]=x[i]-_x[i];
L.Val[i*2][1]=y[i]-_y[i];
}
matrix_X.Column=1;matrix_X.Row=6;
Matrix tmp;
tmp.Column=6;tmp.Row=6;
tmp=(A.Transpose()).Multiply(A);
matrix_X=tmp.Inverse(tmp);
matrix_X=matrix_X.Multiply(A.Transpose());
matrix_X=matrix_X.Multiply(L);
}
void repeat(){
init();
while(1){
work();
ans1+=matrix_X.Val[1][1];
ans2+=matrix_X.Val[2][1];
ans3+=matrix_X.Val[3][1];
ans4+=matrix_X.Val[4][1];
ans5+=matrix_X.Val[5][1];
ans6+=matrix_X.Val[6][1];
Xs0=ans1;
Ys0=ans2;
Zs0=ans3;
k1=ans4;
k2=ans5;
k3=ans6;
puts(".");
if(fabs(matrix_X.Val[1][1])<=1e-5 &&
fabs(matrix_X.Val[2][1])<=1e-5 &&
fabs(matrix_X.Val[3][1])<=1e-5 &&
fabs(matrix_X.Val[4][1])<=1e-5 &&
fabs(matrix_X.Val[5][1])<=1e-5 &&
fabs(matrix_X.Val[6][1])<=1e-5 )
break;
}
cout<<
"Xs0 = "<<ans1<<endl;
cout<<
"Ys0 = "<<ans2<<endl;
cout<<
"Zs0 = "<<ans3<<endl;
cout<<
"fie = "<<ans4<<endl;
cout<<
"omiga = "<<ans5<<endl;
cout<<
"kapa = "<<ans6<<endl;
}
};
int main(){
Resection a;
a.repeat();
}
