Ƭռ󷽽
2008-12-05 10:18
뻷VC6.0̣Win32 Console Application

#include <stdio.h>
#include <math.h>

#define PI 3.1415926535897932
#define N 10 //ƵΪ10

/*************************************************************************************
* תú(MResult = MOrigin(T))˵:
*   MOrigin   - ԭʼһάʽ洢mn
*   MResult   - תúһάʽ洢nm
*************************************************************************************/
void Transpose(double *MOrigin, double *MResult, int m, int n)
{ 
int i, j;
for(i=0; i<n; i++)
   for(j=0; j<m; j++)
    MResult[i*m+j] = MOrigin[j*n+i];
}

/*************************************************************************************
* 溯(Metrix = Metrix(-1))˵:
*   Metrix - ԭʼҲ֮ľһάʽ洢nn
*************************************************************************************/
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[k*n+k];
   }
   Metrix[k*n+k] = 1/Metrix[k*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];
   }
}
}

/*************************************************************************************
* ˺(MResult = MOrigin1 * MOrigin2)˵:
*   MOrigin1 - ԭʼ1һάʽ洢mn
*   MOrigin2 - ԭʼ2һάʽ洢nl
*   MResult   - ˺һάʽ洢ml
*************************************************************************************/
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++)
   {
    MResult[i*l+j] = 0.0;
            for(k=0; k<n; k++)
     MResult[i*l+j] += MOrigin1[i*n+k] * MOrigin2[j+k*l];
   }
}

/*************************************************************************************
* (MResult = MOrigin1 - MOrigin2)˵:
*   MOrigin1 - ԭʼ1һάʽ洢mn
*   MOrigin2 - ԭʼ2һάʽ洢mn
*   MResult   - һάʽ洢mn
*************************************************************************************/
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++)
    MResult[n*i+j] = MOrigin1[n*i+j] - MOrigin2[n*i+j];
}

/*************************************************************************************
* 
*************************************************************************************/
void main()
{
int i, m, n = 4, nCount = 0;
    double phi, omega, kappa, Xs, Ys, Zs;
double x0, y0, f, Sx = 0.0, Sy = 0.0;
double m0 =0.0;


    double x[N]={-86.15, -53.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.4369, 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.000000001 || 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(omega);
  
   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, 2*n, 1);
Multiply(V, 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);
minute[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)(omega*180.0/PI);
minute[1] = (int)((omega*180.0/PI-degree[1])*60.0);
second[1] = (int)(((omega*180.0/PI-degree[1])*60.0-minute[1])*60.0);
degree[2] = (int)(kappa*180.0/PI);
minute[2] = (int)((kappa*180.0/PI-degree[2])*60.0);
second[2] = (int)(((kappa*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);

}
 
