inverssolve.cpp

用vc++实现广义的最小二乘法程序

                  for (i=1; i<=n; i++)
                    { ix=(i-1)*n+kk-1; iy=(i-1)*n+kk;
                      d=v[ix]; v[ix]=v[iy]; v[iy]=d;
                    }
                if (kk<m)
                  for (i=1; i<=m; i++)
                    { ix=(i-1)*m+kk-1; iy=(i-1)*m+kk;
                      d=u[ix]; u[ix]=u[iy]; u[iy]=d;
                    }
                kk=kk+1;
              }
            it=60;
            mm=mm-1;
          }
        else
          { ks=mm;
            while ((ks>kk)&&(fabs(s[ks-1])!=0.0))
              { d=0.0;
                if (ks!=mm) d=d+fabs(e[ks-1]);
                if (ks!=kk+1) d=d+fabs(e[ks-2]);
                dd=fabs(s[ks-1]);
                if (dd>eps*d) ks=ks-1;
                else s[ks-1]=0.0;
              }
            if (ks==kk)
              { kk=kk+1;
                d=fabs(s[mm-1]);
                t=fabs(s[mm-2]);
                if (t>d) d=t;
                t=fabs(e[mm-2]);
                if (t>d) d=t;
                t=fabs(s[kk-1]);
                if (t>d) d=t;
                t=fabs(e[kk-1]);
                if (t>d) d=t;
                sm=s[mm-1]/d; sm1=s[mm-2]/d;
                em1=e[mm-2]/d;
                sk=s[kk-1]/d; ek=e[kk-1]/d;
                b=((sm1+sm)*(sm1-sm)+em1*em1)/2.0;
                c=sm*em1; c=c*c; shh=0.0;
                if ((b!=0.0)||(c!=0.0))
                  { shh=sqrt(b*b+c);
                    if (b<0.0) shh=-shh;
                    shh=c/(b+shh);
                  }
                fg[0]=(sk+sm)*(sk-sm)-shh;
                fg[1]=sk*ek;
                for (i=kk; i<=mm-1; i++)
                  { 
					least_sss(fg,cs);
                    if (i!=kk) e[i-2]=fg[0];
                    fg[0]=cs[0]*s[i-1]+cs[1]*e[i-1];
                    e[i-1]=cs[0]*e[i-1]-cs[1]*s[i-1];
                    fg[1]=cs[1]*s[i];
                    s[i]=cs[0]*s[i];
                    if ((cs[0]!=1.0)||(cs[1]!=0.0))
                      for (j=1; j<=n; j++)
                        { ix=(j-1)*n+i-1;
                          iy=(j-1)*n+i;
                          d=cs[0]*v[ix]+cs[1]*v[iy];
                          v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
                          v[ix]=d;
                        }
                    least_sss(fg,cs);
                    s[i-1]=fg[0];
                    fg[0]=cs[0]*e[i-1]+cs[1]*s[i];
                    s[i]=-cs[1]*e[i-1]+cs[0]*s[i];
                    fg[1]=cs[1]*e[i];
                    e[i]=cs[0]*e[i];
                    if (i<m)
                      if ((cs[0]!=1.0)||(cs[1]!=0.0))
                        for (j=1; j<=m; j++)
                          { ix=(j-1)*m+i-1;
                            iy=(j-1)*m+i;
                            d=cs[0]*u[ix]+cs[1]*u[iy];
                            u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
                            u[ix]=d;
                          }
                  }
                e[mm-2]=fg[0];
                it=it-1;
              }
            else
              { if (ks==mm)
                  { kk=kk+1;
                    fg[1]=e[mm-2]; e[mm-2]=0.0;
                    for (ll=kk; ll<=mm-1; ll++)
                      { i=mm+kk-ll-1;
                        fg[0]=s[i-1];
                        least_sss(fg,cs);
                        s[i-1]=fg[0];
                        if (i!=kk)
                          { fg[1]=-cs[1]*e[i-2];
                            e[i-2]=cs[0]*e[i-2];
                          }
                        if ((cs[0]!=1.0)||(cs[1]!=0.0))
                          for (j=1; j<=n; j++)
                            { ix=(j-1)*n+i-1;
                              iy=(j-1)*n+mm-1;
                              d=cs[0]*v[ix]+cs[1]*v[iy];
                              v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
                              v[ix]=d;
                            }
                      }
                  }
                else
                  { kk=ks+1;
                    fg[1]=e[kk-2];
                    e[kk-2]=0.0;
                    for (i=kk; i<=mm; i++)
                      { fg[0]=s[i-1];
                        least_sss(fg,cs);
                        s[i-1]=fg[0];
                        fg[1]=-cs[1]*e[i-1];
                        e[i-1]=cs[0]*e[i-1];
                        if ((cs[0]!=1.0)||(cs[1]!=0.0))
                          for (j=1; j<=m; j++)
                            { ix=(j-1)*m+i-1;
                              iy=(j-1)*m+kk-2;
                              d=cs[0]*u[ix]+cs[1]*u[iy];
                              u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
                              u[ix]=d;
                            }
                      }
                  }
              }
          }
      }
    return(1);
  }

 void least_ppp(double *a,double *e,double *s,double *v,int m,int n)
  //int m,n;
 // double a[],e[],s[],v[];
  { int i,j,p,q;
    double d;
    if (m>=n) i=n;
    else i=m;
    for (j=1; j<=i-1; j++)
      { a[(j-1)*n+j-1]=s[j-1];
        a[(j-1)*n+j]=e[j-1];
      }
    a[(i-1)*n+i-1]=s[i-1];
    if (m<n) a[(i-1)*n+i]=e[i-1];
    for (i=1; i<=n-1; i++)
    for (j=i+1; j<=n; j++)
      { p=(i-1)*n+j-1; q=(j-1)*n+i-1;
        d=v[p]; v[p]=v[q]; v[q]=d;
      }
  return;
  }

  void least_sss(double *fg,double *cs)
  //double cs[2],fg[2];
  { double r,d;
    if ((fabs(fg[0])+fabs(fg[1]))==0.0)
      { cs[0]=1.0; cs[1]=0.0; d=0.0;}
    else 
      { d=sqrt(fg[0]*fg[0]+fg[1]*fg[1]);
        if (fabs(fg[0])>fabs(fg[1]))
          { d=fabs(d);
            if (fg[0]<0.0) d=-d;
          }
        if (fabs(fg[1])>=fabs(fg[0]))
          { d=fabs(d);
            if (fg[1]<0.0) d=-d;
          }
        cs[0]=fg[0]/d; cs[1]=fg[1]/d;
      }
    r=1.0;
    if (fabs(fg[0])>fabs(fg[1])) r=cs[1];
    else
      if (cs[0]!=0.0) r=1.0/cs[0];
    fg[0]=d; fg[1]=r;
    return;
  }
void least_dnginf(int m, int n, double *x, double *d)
{

	/*double  x1a,y1a,z1a,x1b,y1b,z1b,x1c,y1c,z1c,x1d,y1d,z1d,x1e,y1e,z1e,x1f,y1f,z1f,x1g,y1g,z1g,x1h,y1h,z1h;
	double  x2a,y2a,z2a,x2b,y2b,z2b,x2c,y2c,z2c,x2d,y2d,z2d,x2e,y2e,z2e,x2f,y2f,z2f,x2g,y2g,z2g,x2h,y2h,z2h;	
	x1a=m_firstpointset[0].x;y1a=m_firstpointset[0].y;z1a=m_firstpointset[0].z;
	x1b=m_firstpointset[1].x;y1b=m_firstpointset[1].y;z1b=m_firstpointset[1].z;
	x1c=m_firstpointset[2].x;y1c=m_firstpointset[2].y;z1c=m_firstpointset[2].z;
	x1d=m_firstpointset[3].x;y1d=m_firstpointset[3].y;z1d=m_firstpointset[3].z;
	x1e=m_firstpointset[4].x;y1e=m_firstpointset[4].y;z1e=m_firstpointset[4].z;
	x1f=m_firstpointset[5].x;y1f=m_firstpointset[5].y;z1f=m_firstpointset[5].z;
	x1g=m_firstpointset[6].x;y1g=m_firstpointset[6].y;z1g=m_firstpointset[6].z;
	x1h=m_firstpointset[7].x;y1h=m_firstpointset[7].y;z1h=m_firstpointset[7].z;
	x2a=m_secondpointset[0].x;y2a=m_secondpointset[0].y;z2a=m_secondpointset[0].z;
	x2b=m_secondpointset[1].x;y2b=m_secondpointset[1].y;z2b=m_secondpointset[1].z;
	x2c=m_secondpointset[2].x;y2c=m_secondpointset[2].y;z2c=m_secondpointset[2].z;
	x2d=m_secondpointset[3].x;y2d=m_secondpointset[3].y;z2d=m_secondpointset[3].z;
	x2e=m_secondpointset[4].x;y2e=m_secondpointset[4].y;z2e=m_secondpointset[4].z;
	x2f=m_secondpointset[5].x;y2f=m_secondpointset[5].y;z2f=m_secondpointset[5].z;
	x2g=m_secondpointset[6].x;y2g=m_secondpointset[6].y;z2g=m_secondpointset[6].z;
	x2h=m_secondpointset[7].x;y2h=m_secondpointset[7].y;z2h=m_secondpointset[7].z;*/
	double x1a=140.528351 ,y1a=6.433036,z1a=42.823979;
	double x1b=140.723190, y1b=6.032580,z1b= 43.023979;
	double x1c=140.921997,y1c=  7.032120,z1c=  43.223979;
	double x1d=141.119751,y1d=  6.431657,z1d=  43.423979;
	double x1e=141.325027,y1e=  5.831176,z1e=  43.623980;
	double x1f=141.522736 ,y1f= 6.030716,z1f= 43.823979;
	double x2a=140.528351 ,y2a=6.433036,z2a=42.823979;
	double x2b=140.723190, y2b=6.032580,z2b= 43.023979;
	double x2c=140.921997,y2c=  7.032120,z2c=  43.223979;
	double x2d=141.119751,y2d=  6.431657,z2d=  43.423979;
	double x2e=141.325027,y2e=  5.831176,z2e=  43.623980;
	double x2f=141.522736 ,y2f= 6.030716,z2f= 43.823979;
	d[0]=x2a*x[0]+y2a*x[1]+z2a*x[2]+x[3]-x1a;
	d[1]=x2a*x[4]+y2a*x[5]+z2a*x[6]+x[7]-y1a;
	d[2]=x2a*x[8]+y2a*x[9]+z2a*x[10]+x[11]-z1a;
	d[3]=x2b*x[0]+y2b*x[1]+z2b*x[2]+x[3]-x1b;
	d[4]=x2b*x[4]+y2b*x[5]+z2b*x[6]+x[7]-y1b;
	d[5]=x2b*x[8]+y2b*x[9]+z2b*x[10]+x[11]-z1b;
	d[6]=x2c*x[0]+y2c*x[1]+z2c*x[2]+x[3]-x1c;
	d[7]=x2c*x[4]+y2c*x[5]+z2c*x[6]+x[7]-y1c;
	d[8]=x2c*x[8]+y2c*x[9]+z2c*x[10]+x[11]-z1c;
	///////
	//every two row mul is zero
	d[9]=x[0]*x[4]+x[1]*x[5]+x[2]*x[6];
	d[10]=x[0]*x[8]+x[1]*x[9]+x[2]*x[10];
	d[11]=x[4]*x[8]+x[5]*x[9]+x[6]*x[10];
	//the square of every row is one 
	d[12]=x[0]*x[0]+x[1]*x[1]+x[2]*x[2]-1;   
	d[13]=x[6]*x[6]+x[4]*x[4]+x[5]*x[5]-1;
	d[14]=x[9]*x[9]+x[10]*x[10]+x[8]*x[8]-1;
	////////////////
	d[15]=x2d*x[0]+y2d*x[1]+z2d*x[2]+x[3]-x1d;
	d[16]=x2d*x[4]+y2d*x[5]+z2d*x[6]+x[7]-y1d;
	d[17]=x2d*x[8]+y2d*x[9]+z2d*x[10]+x[11]-z1d;
	d[18]=x2e*x[0]+y2e*x[1]+z2e*x[2]+x[3]-x1e;
	d[19]=x2e*x[4]+y2e*x[5]+z2e*x[6]+x[7]-y1e;
	d[20]=x2e*x[8]+y2e*x[9]+z2e*x[10]+x[11]-z1e;
	d[21]=x2f*x[0]+y2f*x[1]+z2f*x[2]+x[3]-x1f;
	d[22]=x2f*x[4]+y2f*x[5]+z2f*x[6]+x[7]-y1f;
	d[23]=x2f*x[8]+y2f*x[9]+z2f*x[10]+x[11]-z1f;
   /* d[24]=x2g*x[0]+y2g*x[1]+z2g*x[2]+x[3]-x1g;
	d[25]=x2g*x[4]+y2g*x[5]+z2g*x[6]+x[7]-y1g;
	d[26]=x2g*x[8]+y2g*x[9]+z2g*x[10]+x[11]-z1g;
	d[27]=x2h*x[0]+y2h*x[1]+z2h*x[2]+x[3]-x1h;
	d[28]=x2h*x[4]+y2h*x[5]+z2h*x[6]+x[7]-y1h;
	d[29]=x2h*x[8]+y2h*x[9]+z2h*x[10]+x[11]-z1h;*/
	
	
}

void least_dngins(int m, int n, double *x, double *p)
{//雅可比矩阵
/*	double  x1a,y1a,z1a,x1b,y1b,z1b,x1c,y1c,z1c,x1d,y1d,z1d,x1e,y1e,z1e,x1f,y1f,z1f,x1g,y1g,z1g,x1h,y1h,z1h;
	double  x2a,y2a,z2a,x2b,y2b,z2b,x2c,y2c,z2c,x2d,y2d,z2d,x2e,y2e,z2e,x2f,y2f,z2f,x2g,y2g,z2g,x2h,y2h,z2h;;	
	x1a=m_firstpointset[0].x;y1a=m_firstpointset[0].y;z1a=m_firstpointset[0].z;
	x1b=m_firstpointset[1].x;y1b=m_firstpointset[1].y;z1b=m_firstpointset[1].z;
	x1c=m_firstpointset[2].x;y1c=m_firstpointset[2].y;z1c=m_firstpointset[2].z;
	x1d=m_firstpointset[3].x;y1d=m_firstpointset[3].y;z1d=m_firstpointset[3].z;
	x1e=m_firstpointset[4].x;y1e=m_firstpointset[4].y;z1e=m_firstpointset[4].z;
	x1f=m_firstpointset[5].x;y1f=m_firstpointset[5].y;z1f=m_firstpointset[5].z;
	x1g=m_firstpointset[6].x;y1g=m_firstpointset[6].y;z1g=m_firstpointset[6].z;
	x1h=m_firstpointset[7].x;y1h=m_firstpointset[7].y;z1h=m_firstpointset[7].z;
	x2a=m_secondpointset[0].x;y2a=m_secondpointset[0].y;z2a=m_secondpointset[0].z;
	x2b=m_secondpointset[1].x;y2b=m_secondpointset[1].y;z2b=m_secondpointset[1].z;
	x2c=m_secondpointset[2].x;y2c=m_secondpointset[2].y;z2c=m_secondpointset[2].z;
	x2d=m_secondpointset[3].x;y2d=m_secondpointset[3].y;z2d=m_secondpointset[3].z;
	x2e=m_secondpointset[4].x;y2e=m_secondpointset[4].y;z2e=m_secondpointset[4].z;
	x2f=m_secondpointset[5].x;y2f=m_secondpointset[5].y;z2f=m_secondpointset[5].z;
	x2g=m_secondpointset[6].x;y2g=m_secondpointset[6].y;z2g=m_secondpointset[6].z;
	x2h=m_secondpointset[7].x;y2h=m_secondpointset[7].y;z2h=m_secondpointset[7].z;*/
	double x1a=140.528351 ,y1a=6.433036,z1a=42.823979;
	double x1b=140.723190, y1b=6.032580,z1b= 43.023979;
		double x1c=140.921997,y1c=  7.032120,z1c=  43.223979;
		double x1d=141.119751,y1d=  6.431657,z1d=  43.423979;
		double x1e=141.325027,y1e=  5.831176,z1e=  43.623980;
		double x1f=141.522736 ,y1f= 6.030716,z1f= 43.823979;
		double x2a=140.528351 ,y2a=6.433036,z2a=42.823979;
		double x2b=140.723190, y2b=6.032580,z2b= 43.023979;
		double x2c=140.921997,y2c=  7.032120,z2c=  43.223979;
		double x2d=141.119751,y2d=  6.431657,z2d=  43.423979;
		double x2e=141.325027,y2e=  5.831176,z2e=  43.623980;
		double x2f=141.522736 ,y2f= 6.030716,z2f= 43.823979;
		
	//雅可比矩阵第一行
	p[0*n+0]=x2a;p[0*n+1]=y2a;	p[0*n+2]=z2a;
	p[0*n+3]=1;p[0*n+4]=0;p[0*n+5]=0;p[0*n+6]=0;p[0*n+7]=0;p[0*n+8]=0;
	p[0*n+9]=0;p[0*n+10]=0;p[0*n+11]=0;
  //雅可比矩阵第二行
    p[1*n+0]=0;p[1*n+1]=0;p[1*n+2]=0;p[1*n+3]=0;
	p[1*n+4]=x2a;p[1*n+5]=y2a;p[1*n+6]=z2a;p[1*n+7]=1;p[1*n+8]=0;
    p[1*n+9]=0;p[1*n+10]=0;p[1*n+11]=0;
	//third row
	p[2*n+0]=0;p[2*n+1]=0;p[2*n+2]=0;p[2*n+3]=0;p[2*n+4]=0;
	p[2*n+5]=0;p[2*n+6]=0;p[2*n+7]=0;
	p[2*n+8]=x2a;p[2*n+9]=y2a;p[2*n+10]=z2a;p[2*n+11]=1;
	//forth row
	p[3*n+0]=x2b;p[3*n+1]=y2b;	p[3*n+2]=z2b;
	p[3*n+3]=1;p[3*n+4]=0;p[3*n+5]=0;p[3*n+6]=0;p[3*n+7]=0;p[3*n+8]=0;
	p[3*n+9]=0;p[3*n+10]=0;p[3*n+11]=0;
	//雅可比矩阵第fifth行
    p[4*n+0]=0;p[4*n+1]=0;p[4*n+2]=0;p[4*n+3]=0;
	p[4*n+4]=x2b;p[4*n+5]=y2b;p[4*n+6]=z2b;p[4*n+7]=1;p[4*n+8]=0;
    p[4*n+9]=0;p[4*n+10]=0;p[4*n+11]=0;
	//sixth row
	p[5*n+0]=0;p[5*n+1]=0;p[5*n+2]=0;p[5*n+3]=0;p[5*n+4]=0;
	p[5*n+5]=0;p[5*n+6]=0;p[5*n+7]=0;
	p[5*n+8]=x2b;p[5*n+9]=y2b;p[5*n+10]=z2b;p[5*n+11]=1;
	//seventh
	p[6*n+0]=x2c;p[6*n+1]=y2c;	p[6*n+2]=z2c;
	p[6*n+3]=1;p[6*n+4]=0;p[6*n+5]=0;p[6*n+6]=0;p[6*n+7]=0;p[6*n+8]=0;
	p[6*n+9]=0;p[6*n+10]=0;p[6*n+11]=0;
	//雅可比矩阵第eighth行
    p[7*n+0]=0;p[7*n+1]=0;p[7*n+2]=0;p[7*n+3]=0;
	p[7*n+4]=x2c;p[7*n+5]=y2c;p[7*n+6]=z2c;p[7*n+7]=1;p[7*n+8]=0;
    p[7*n+9]=0;p[7*n+10]=0;p[7*n+11]=0;
	//ninth row
	p[8*n+0]=0;p[8*n+1]=0;p[8*n+2]=0;p[8*n+3]=0;p[8*n+4]=0;
	p[8*n+5]=0;p[8*n+6]=0;p[8*n+7]=0;
	p[8*n+8]=x2c;p[8*n+9]=y2c;p[8*n+10]=z2c;p[8*n+11]=1;
	///tenth
	p[9*n+0]=x[4];p[9*n+1]=x[5];p[9*n+2]=x[6];
	p[9*n+3]=0;p[9*n+4]=x[0];p[9*n+5]=x[1];p[9*n+6]=x[2];p[9*n+7]=0;p[9*n+8]=0;
	p[9*n+9]=0;p[9*n+10]=0;p[9*n+11]=0;
	//雅可比矩阵第eleven行
    p[10*n+0]=x[8];p[10*n+1]=x[9];p[10*n+2]=x[10];p[10*n+3]=0;
	p[10*n+4]=0;p[10*n+5]=0;p[10*n+6]=0;p[10*n+7]=0;p[10*n+8]=x[0];
    p[10*n+9]=x[1];p[10*n+10]=x[2];p[10*n+11]=0;
	//twele row
	p[11*n+0]=0;p[11*n+1]=0;p[11*n+2]=0;p[11*n+3]=0;p[11*n+4]=x[8];
	p[11*n+5]=x[9];p[11*n+6]=x[10];p[11*n+7]=0;
	p[11*n+8]=x[4];p[11*n+9]=x[5];p[11*n+10]=x[6];p[11*n+11]=0;
//////thirteen
	p[12*n+0]=2*x[0];p[12*n+1]=2*x[1];p[12*n+2]=2*x[2];
	p[12*n+3]=0;p[12*n+4]=0;p[12*n+5]=0;p[12*n+6]=0;p[12*n+7]=0;p[12*n+8]=0;
	p[12*n+9]=0;p[12*n+10]=0;p[12*n+11]=0;
	//雅可比矩阵第forteen行
    p[13*n+0]=0;p[13*n+1]=0;p[13*n+2]=0;p[13*n+3]=0;
	p[13*n+4]=2*x[4];p[13*n+5]=2*x[5];p[13*n+6]=2*x[6];p[13*n+7]=0;p[13*n+8]=x[0];
    p[13*n+9]=0;p[13*n+10]=0;p[13*n+11]=0;
	//fifteen row
	p[14*n+0]=0;p[14*n+1]=0;p[14*n+2]=0;p[14*n+3]=0;p[14*n+4]=0;
	p[14*n+5]=0;p[14*n+6]=0;p[14*n+7]=0;
	p[14*n+8]=2*x[8];p[14*n+9]=2*x[9];p[14*n+10]=2*x[10];p[14*n+11]=0;
/////////////////////////////////////////////////////////////////////////
	//sixteen
	
	//雅可比矩阵第
	p[15*n+0]=x2d;p[15*n+1]=y2d;	p[15*n+2]=z2d;
	p[15*n+3]=1;p[15*n+4]=0;p[15*n+5]=0;p[15*n+6]=0;p[15*n+7]=0;p[15*n+8]=0;
	p[15*n+9]=0;p[15*n+10]=0;p[15*n+11]=0;
	//雅可比矩阵第17行
    p[16*n+0]=0;p[16*n+1]=0;p[16*n+2]=0;p[16*n+3]=0;
	p[16*n+4]=x2d;p[16*n+5]=y2d;p[16*n+6]=z2d;p[16*n+7]=1;p[16*n+8]=0;
    p[16*n+9]=0;p[16*n+10]=0;p[16*n+11]=0;
	//18 row
	p[17*n+0]=0;p[17*n+1]=0;p[17*n+2]=0;p[17*n+3]=0;p[17*n+4]=0;
	p[17*n+5]=0;p[17*n+6]=0;p[17*n+7]=0;
	p[17*n+8]=x2d;p[17*n+9]=y2d;p[17*n+10]=z2d;p[17*n+11]=1;
//////
	//19row
	p[18*n+0]=x2e;p[18*n+1]=y2e;p[18*n+2]=z2e;
	p[18*n+3]=1;p[18*n+4]=0;p[18*n+5]=0;p[18*n+6]=0;p[18*n+7]=0;p[18*n+8]=0;
	p[18*n+9]=0;p[18*n+10]=0;p[18*n+11]=0;
	//雅可比矩阵第20行
    p[19*n+0]=0;p[19*n+1]=0;p[19*n+2]=0;p[19*n+3]=0;
	p[19*n+4]=x2e;p[19*n+5]=y2e;p[19*n+6]=z2e;p[19*n+7]=1;p[19*n+8]=0;
    p[19*n+9]=0;p[19*n+10]=0;p[19*n+11]=0;
	//21 row
	p[20*n+0]=0;p[20*n+1]=0;p[20*n+2]=0;p[20*n+3]=0;p[20*n+4]=0;
	p[20*n+5]=0;p[20*n+6]=0;p[20*n+7]=0;
	p[20*n+8]=x2e;p[20*n+9]=y2e;p[20*n+10]=z2e;p[20*n+11]=1;
////////22
	
	p[21*n+0]=x2f;p[21*n+1]=y2f;p[21*n+2]=z2f;
	p[21*n+3]=1;p[21*n+4]=0;p[21*n+5]=0;p[21*n+6]=0;p[21*n+7]=0;p[21*n+8]=0;
	p[21*n+9]=0;p[21*n+10]=0;p[21*n+11]=0;
	//雅可比矩阵第23行
//	d[22]=x2f*x[4]+y2f*x[5]+z2f*x[6]+x[7]-y1f;
    p[22*n+0]=0;p[22*n+1]=0;p[22*n+2]=0;p[22*n+3]=0;
	p[22*n+4]=x2f;p[22*n+5]=y2f;p[22*n+6]=z2f;p[22*n+7]=1;p[22*n+8]=0;
    p[22*n+9]=0;p[22*n+10]=0;p[22*n+11]=0;
	//24 row
	//d[23]=x2f*x[8]+y2f*x[9]+z2f*x[10]+x[11]-z1f;
	p[23*n+0]=0;p[23*n+1]=0;p[23*n+2]=0;p[23*n+3]=0;p[23*n+4]=0;
	p[23*n+5]=0;p[23*n+6]=0;p[23*n+7]=0;
	p[23*n+8]=x2f;p[23*n+9]=y2f;p[23*n+10]=z2f;p[23*n+11]=1;
	/////
	//25
/*	p[24*n+0]=x2g;p[24*n+1]=y2g;p[24*n+2]=z2g;
	p[24*n+3]=1;p[24*n+4]=0;p[24*n+5]=0;p[24*n+6]=0;p[24*n+7]=0;p[24*n+8]=0;
	p[24*n+9]=0;p[24*n+10]=0;p[24*n+11]=0;
	//雅可比矩阵第26行
    p[25*n+0]=0;p[25*n+1]=0;p[25*n+2]=0;p[25*n+3]=0;
	p[25*n+4]=x2g;p[25*n+5]=y2g;p[25*n+6]=z2g;p[25*n+7]=1;p[25*n+8]=0;
    p[25*n+9]=0;p[25*n+10]=0;p[25*n+11]=0;
	//27 row
	p[26*n+0]=0;p[26*n+1]=0;p[26*n+2]=0;p[26*n+3]=0;p[26*n+4]=0;
	p[26*n+5]=0;p[26*n+6]=0;p[26*n+7]=0;
	p[26*n+8]=x2g;p[26*n+9]=y2g;p[26*n+10]=z2g;p[26*n+11]=1;
	/////28
	p[27*n+0]=x2h;p[27*n+1]=y2h;p[27*n+2]=z2h;
	p[27*n+3]=1;p[27*n+4]=0;p[27*n+5]=0;p[27*n+6]=0;p[27*n+7]=0;p[27*n+8]=0;
	p[27*n+9]=0;p[27*n+10]=0;p[27*n+11]=0;
	//雅可比矩阵第29行
    p[28*n+0]=0;p[28*n+1]=0;p[28*n+2]=0;p[28*n+3]=0;
	p[28*n+4]=x2h;p[28*n+5]=y2h;p[28*n+6]=z2h;p[28*n+7]=1;p[28*n+8]=0;
    p[28*n+9]=0;p[28*n+10]=0;p[28*n+11]=0;
	//30 row
	p[29*n+0]=0;p[29*n+1]=0;p[29*n+2]=0;p[29*n+3]=0;p[29*n+4]=0;
	p[29*n+5]=0;p[29*n+6]=0;p[29*n+7]=0;
	p[29*n+8]=x2h;p[29*n+9]=y2h;p[29*n+10]=z2h;p[29*n+11]=1;*/
}