ellipse_par.cpp

形成椭圆参数

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdio.h>
#include <math.h>

void gaussj(double a[], int n, double b[])
{
	int i,j,k,l,ll,irow,icol;
	double big,pivinv,dum;
    int ipiv[50], indxr[50], indxc[50];

  /*for(i=0;i<n*n;i++)
  {
	  printf("m   %5d %12f   \n",i,a[i]);
  }

 for(i=0;i<n;i++)
  {
	  printf("b   %5d %12f   \n",i,b[i]);
  }*/


    for (j=0;j<=n-1;j++)
	{
        ipiv[j]=0;
    }
    for (i=0;i<=n-1;i++)
	{
        big=0.0;
        for (j=0;j<=n-1;j++)
		{
            if(ipiv[j]!=1)
			{
                for(k=0;k<=n-1;k++)
				{
					if(ipiv[k]==0)
					{
						if(fabs(a[j*n+k])>=big)
						{
							big=fabs(a[j*n+k]);
							irow=j;
							icol=k;
						}
						else if(ipiv[k]>1)
						{
							printf("singular matrix\n");
						}
					}
                }
            }
        }
        ipiv[icol]=ipiv[icol]+1;
        if(irow!=icol)
		{
            for(l=0;l<=n-1;l++)
			{
                dum=(a[irow*n+l]);
                a[irow*n+l]=a[icol*n+l];
                a[icol*n+l]=dum;
            }
            dum=b[irow];
            b[irow]=b[icol];
            b[icol]=dum;
		}
        indxr[i]=irow;
        indxc[i]=icol;
        if(a[icol*n+icol]==0.0)
		{
			printf("singular matrix\n");
		}
        pivinv=1.0/(a[icol*n+icol]);
        a[icol*n+icol]=1.0;
        for(l=0;l<=n-1;l++)
		{
            a[icol*n+l]=a[icol*n+l]*pivinv;
        }
        b[icol]=b[icol]*pivinv;
        for(ll=0;ll<=n-1;ll++)
        {
            if(ll!=icol)
			{
                dum=a[ll*n+icol];
                a[ll*n+icol]=0.0;
                for(l=0;l<=n-1;l++)
				{
                    a[ll*n+l]=a[ll*n+l]-a[icol*n+l]*dum;
                }
                b[ll]=b[ll]-b[icol]*dum;
            }
        }
	}	
    for(l=n-1;l<=0;l--)
	{
        if(indxr[l]!=indxc[l])
		{
            for(k=0;k<=n-1;k++)
			{
                dum=a[k*n+indxr[l]];
                a[k*n+indxr[l]]=a[k*n+indxc[l]];
                a[k*n+indxr[l]]=dum;
            }
        }
    }
}

void Ellipse_par(double x1, double y1, 
double x2, double y2, 
double x3, double y3,
double x4, double y4,
double x5, double y5, 
double RC[])
{
	int i;
	double M[25],B[25];

	double a,b,c,d,e,da1,tmp,tx,ty,pai = 3.1415926;
	double major, minor, x0, y0;

	M[0] = x1*x1; 
	M[1] = x1*y1; 
	M[2] = y1*y1; 
	M[3] = x1; 
	M[4] = y1;

	M[5] = x2*x2; 
	M[6] = x2*y2; 
	M[7] = y2*y2; 
	M[8] = x2; 
	M[9] = y2;

	M[10] = x3*x3; 
	M[11] = x3*y3; 
	M[12] = y3*y3; 
	M[13] = x3; 
	M[14] = y3;

	M[15] = x4*x4; 
	M[16] = x4*y4; 
	M[17] = y4*y4; 
	M[18] = x4; 
	M[19] = y4;

	M[20] = x5*x5; 
	M[21] = x5*y5; 
	M[22] = y5*y5; 
	M[23] = x5; 
	M[24] = y5;

	B[0] = 1; 
	B[1] = 1; 
	B[2] = 1; 
	B[3] = 1; 
	B[4] = 1;

	i = 5;
	gaussj( M, i, B);

	a = B[0]; 
	b = B[1]; 
	c = B[2]; 
	d = B[3]; 
	e = B[4];

	// b*b - 4ac check!!!! 
	    if( (b*b-4*a*c)>=0 ) {
		RC[98] = 1 ;
		return;
	} else
	{
		RC[98] = -1;
	}

	tx = (b*e-2.*c*d)/(4*a*c-b*b);
	ty = (b*d-2.*a*e)/(4*a*c-b*b);
	da1 = .5*180 * atan(b/(a-c))/pai;

	if(a>0) {
		major = 2.*sqrt( 2./(a+c+sqrt(b*b+(a-c)*(a-c))) );
		minor = 2.*sqrt( 2./(a+c-sqrt(b*b+(a-c)*(a-c))) );
	}
	if(a<0) {
		major = 2.*sqrt( -2./(a+c+sqrt(b*b+(a-c)*(a-c))) );
		minor = 2.*sqrt( -2./(a+c-sqrt(b*b+(a-c)*(a-c))) );
	}
	tmp = da1/180.*pai;
	x0 = tx*cos(tmp) + ty*sin(tmp);
	y0 = -tx*sin(tmp) + ty*cos(tmp);
	
	//printf("B %14.2f %14.2f %14.2f %14.2f %14.2f  \n", tx,ty,major,minor,da1);
	//printf("B %14.2f %14.2f %14.2f %14.2f %14.2f  \n", x0,y0,major,minor,da1);

	RC[0] = major/2;
	RC[1] = minor/2;
	RC[2] = x0;
	RC[3] = y0;
	RC[4] = da1;
	RC[5] = -1;
}


main( int argc, char **argv )
{
  int i,j;
  double x1,x2,x3,x4,x5,y1,y2,y3,y4,y5,RC[100];
  double x[360],y[360];
  double aa =  40.,
         bb =  30.,
	     dx =  10.,
	     dy = -10.,
	     da =  30.,
		 da1,pai = 3.14159265358979323846;

  double xx,yy,x0,y0,major,minor,p1; 

  for(i=0;i<360;i++) {
	  xx = aa*sin((i)/180.*pai) + dx;                          
      yy = bb*cos((i)/180.*pai) + dy;
	  x[i] =  xx*cos(da/180.*pai) + yy*sin(da/180.*pai);
	  y[i] = -xx*sin(da/180.*pai) + yy*cos(da/180.*pai);

	  //x0 =  dx*cos(da/180.*pai) + dy*sin(da/180.*pai);
	  //y0 = -dx*sin(da/180.*pai) + dy*cos(da/180.*pai);
	  //p1 = sqrt( (x[i]-x0)*(x[i]-x0) + (y[i]-y0)*(y[i]-y0) );
	  //printf("%5d  %14.2f  %14.2f  %14.2f  %14.2f  %14.2f   \n", i,x[i],y[i],p1,dx,dy);
  }

  for(i=2;i<360-2;i++){

	  x1 = x[i-2];y1=y[i-2];
	  x2 = x[i-1];y2=y[i-1];
	  x3 = x[i+0];y3=y[i+0];
	  x4 = x[i+1];y4=y[i+1];
	  x5 = x[i+2];y5=y[i+2];

	  Ellipse_par(x1,y1,x2,y2,x3,y3,x4,y4,x5,y5,RC);
	  
	  major = RC[0];
	  minor = RC[1];
	  dx    = RC[2];
	  dy    = RC[3];
	  da  = -RC[4];
	  
	  x0 =  dx*cos(da/180.*pai) + dy*sin(da/180.*pai);
	  y0 = -dx*sin(da/180.*pai) + dy*cos(da/180.*pai);
	  p1 = sqrt( (x[i]-x0)*(x[i]-x0) + (y[i]-y0)*(y[i]-y0) );
	 
	  printf("%5d  %14.2f %14.2f  %14.2f %14.2f %14.2f %14.2f %14.2f %14.2f  \n", i,x[i],y[i],x0,y0,major,minor,da,p1);
  }

}