#include <iostream.h>
#include <stdlib.h>
#include <math.h>
#include <conio.h>
#define f0(x1,x2) (x1*x1-x2-1)
#define f1(x1,x2) ((x1-2)*(x1-2)+(x2-0.5)*(x2-0.5)-1)
#define x_ 0.000001
#define matrixNum 2
double *matrixF2(double *x);
double *matrixB1(double *b,double *q,double *matrixB);

void main()
{
    int i,j,n;
    double *x;
    double *b,*c,*q,p;
    double *matrixF;     //矩陣F
    double *matrixB;   //矩陣F的雅可比矩陣的逆 
    x=(double *)malloc(matrixNum);
    b=(double *)malloc(matrixNum);
    c=(double *)malloc(matrixNum);
	matrixF=(double *)malloc(matrixNum);
    matrixB=(double *)malloc(matrixNum*matrixNum);
    cout<<"請輸入初值:";
	for(i=0;i<matrixNum;i++)
        cin>>*(x+i);
    matrixB=matrixF2(x);
    *matrixF=f0(*x,*(x+1));
    *(matrixF+1)=f1(*x,*(x+1));
    do
	{
	    p=0.0;
        for(i=0;i<matrixNum;i++)
		     *(c+i)=*(matrixF+i);
            for(i=0;i<matrixNum;i++)
			{
				*(b+i)=0;
                *(q+i)=0;
                cout<<*(c+i)<<" ";
			}
        cout<<endl<<"x值";
		for(i=0;i<matrixNum;i++)
		{
			for(j=0;j<matrixNum;j++)
				*(b+i)+=*(matrixB+i*matrixNum+j)*(*(c+j));
            *(b+i)=-*(b+i);
            *(x+i)=*(x+i)+(*(b+i));
            cout<<*(x+i)<<" ";
		}
	    cout<<endl;
        *matrixF=f0(*x,*(x+1));
        *(matrixF+1)=f1(*x,*(x+1));
        for(i=0;i<matrixNum;i++)
        {
            p+=pow(*(b+i),2);
            *(q+i)=*(matrixF+i)-*(c+i);
         }
         matrixB=matrixB1(b,q,matrixB);
	}while(sqrt(p)>x_);
    delete [] matrixF;
    delete [] matrixB;
    getch();
}


double *matrixF2(double *x)
{
    int i,j;
    double t; 
    double *matrixF1;   //矩陣F的雅可比矩陣
    double *matrixF2;   //矩陣F的雅可比矩陣的逆 
    matrixF1=(double *)malloc(matrixNum*matrixNum);
    matrixF2=(double *)malloc(matrixNum*matrixNum);
    for(i=0;i<matrixNum;i++)
        for(j=0;j<matrixNum;j++)
			if(i==j)
              *(matrixF2+i*matrixNum+j)=1;
			else *(matrixF2+i*matrixNum+j)=0;
    *matrixF1=(f0((*x+x_),*(x+1))-f0(*x,*(x+1)))/x_;
    *(matrixF1+1)=(f0(*x,(*(x+1)+x_))-f0(*x,*(x+1)))/x_;
    *(matrixF1+2)=(f1((*x+x_),*(x+1))-f1(*x,*(x+1)))/x_;
    *(matrixF1+3)=(f1(*x,(*(x+1)+x_))-f1(*x,*(x+1)))/x_;
    for(i=0;i<matrixNum;i++)
      cout<<*(x+i)<<endl;  
	cout<<"矩陣F在["<<*x<<','<<*(x+1)<<"]的雅可比矩陣"<<endl;
	for(i=0;i<matrixNum;i++)    
	{
		for(j=0;j<matrixNum;j++)
	       cout<<*(matrixF1+i*matrixNum+j)<<" ";
        cout<<endl;
	}
    //求矩陣F的雅可比矩陣的逆 
	t=*matrixF1;
    for(i=0,j=0;j<matrixNum;j++)           
	{
	 *(matrixF1+i*matrixNum+j)/=t;
         *(matrixF2+i*matrixNum+j)/=t;
	}
    t=*(matrixF1+1*matrixNum);
	for(i=1,j=0;j<matrixNum;j++)
	{
		*(matrixF1+i*matrixNum+j)-=*(matrixF1+j)*t;
        *(matrixF2+i*matrixNum+j)-=*(matrixF2+j)*t;
	}
	t=*(matrixF1+1*matrixNum+1);

    for(i=1,j=0;j<matrixNum;j++)
	 {
		 *(matrixF1+i*matrixNum+j)/=t;
         *(matrixF2+i*matrixNum+j)/=t;
	}  
	t=*(matrixF1+1);
	for(i=i,j=0;j<matrixNum;j++)
	{	 
	    *(matrixF1+j)-=*(matrixF1+i*matrixNum+j)*t;
        *(matrixF2+j)-=*(matrixF2+i*matrixNum+j)*t;
	}
    for(i=0;i<matrixNum;i++)           
	{
		for(j=0;j<matrixNum;j++)
	       cout<<*(matrixF1+i*matrixNum+j)<<" ";
        cout<<endl;
	}
    for(i=0;i<matrixNum;i++)           
	{
		for(j=0;j<matrixNum;j++)
	       cout<<*(matrixF2+i*matrixNum+j)<<" ";
        cout<<endl;
	}
	getch();
    return matrixF2;
    delete [] matrixF1;
    delete [] matrixF2;
}

double *matrixB1(double *b,double *q,double *matrixB)
{
    int i,j;
	double n=0;
    double *t;          //(p-matrixB*q)
    double *s;          //(pT*matrixB)
    double *matrixB1;   //矩陣F的雅可比矩陣
    s=(double *)malloc(matrixNum);
	t=(double *)malloc(matrixNum);
    matrixB1=(double *)malloc(matrixNum*matrixNum);
    for(i=0;i<matrixNum;i++)
	{
		*(t+i)=0;
        *(s+i)=0;
	}	
    for(i=0;i<matrixNum;i++)
	{
           for(j=0;j<matrixNum;j++)
	       {
                 *(t+i)+=*(matrixB+i*matrixNum+j)*(*(q+j));
                 *(s+i)+=*(b+j)*(*(matrixB+j*matrixNum+i));      
		   }
           for(j=0;j<matrixNum;j++)
		       n+=*(s+i)*(*(q+j));
           *(t+i)=*(b+i)-*(t+i);    
	}

    for(i=0;i<matrixNum;i++)
		cout<<endl<<*(q+i)<<" ";  
    cout<<endl<<n<<endl;

    for(i=0;i<matrixNum;i++)
       for(j=0;j<matrixNum;j++)
          *(matrixB1+i*matrixNum+j)=*(t+i)*(*(s+j))/n+*(matrixB+i*matrixNum+j);
    for(i=0;i<matrixNum;i++)           
	{
		for(j=0;j<matrixNum;j++)
	       cout<<*(matrixB1+i*matrixNum+j)<<"   ";
        cout<<endl;
	}
	getch();
    return matrixB1;
    delete [] matrixB1;
}