lequations.cpp

用牛顿法求解积分

//////////////////////////////////////////////////////////////////////
// LEquations.cpp
//
// 求解线性方程组的类 CLEquations 的实现代码
//
// 周长发编制, 2002/8
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "LEquations.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CLEquations::CLEquations()
{
}

CLEquations::CLEquations(const CMatrix& mtxCoef, const CMatrix& mtxConst)
{
	ASSERT(Init(mtxCoef, mtxConst));
}

CLEquations::~CLEquations()
{
}

BOOL CLEquations::Init(const CMatrix& mtxCoef, const CMatrix& mtxConst)
{
	if (mtxCoef.GetNumRows() != mtxConst.GetNumRows())
		return FALSE;

	m_mtxCoef = mtxCoef;
	m_mtxConst = mtxConst;

	return TRUE;
}

// 全选主元高斯消去法
BOOL CLEquations::GetRootsetGauss(CMatrix& mtxResult)
{ 
	int *pnJs,l,k,i,j,nIs,p,q;
    double d,t;

	mtxResult = m_mtxConst;
	double *pDataCoef = m_mtxCoef.GetData();
	double *pDataConst = mtxResult.GetData();
	int n = GetNumUnknowns();

    pnJs = new int[n];

    l=1;
    for (k=0;k<=n-2;k++)
    { 
		d=0.0;
        for (i=k;i<=n-1;i++)
		{
			for (j=k;j<=n-1;j++)
            { 
				t=fabs(pDataCoef[i*n+j]);
				if (t>d) 
				{ 
					d=t; 
					pnJs[k]=j; 
					nIs=i;
				}
            }
		}

        if (d == 0.0) 
			l=0;
        else
        { 
			if (pnJs[k]!=k)
            {
				for (i=0;i<=n-1;i++)
                { 
					p=i*n+k; 
					q=i*n+pnJs[k];
					t=pDataCoef[p]; 
					pDataCoef[p]=pDataCoef[q]; 
					pDataCoef[q]=t;
                }
			}

            if (nIs!=k)
            { 
				for (j=k;j<=n-1;j++)
                { 
					p=k*n+j; 
					q=nIs*n+j;
                    t=pDataCoef[p]; 
					pDataCoef[p]=pDataCoef[q]; 
					pDataCoef[q]=t;
                }
                
				t=pDataConst[k]; 
				pDataConst[k]=pDataConst[nIs]; 
				pDataConst[nIs]=t;
            }
        }
        
		if (l==0)
        { 
			delete[] pnJs;
            return FALSE;
        }
        
		d=pDataCoef[k*n+k];
        for (j=k+1;j<=n-1;j++)
        { 
			p=k*n+j; 
			pDataCoef[p]=pDataCoef[p]/d;
		}
        
		pDataConst[k]=pDataConst[k]/d;
        for (i=k+1;i<=n-1;i++)
        { 
			for (j=k+1;j<=n-1;j++)
            { 
				p=i*n+j;
                pDataCoef[p]=pDataCoef[p]-pDataCoef[i*n+k]*pDataCoef[k*n+j];
            }
            
			pDataConst[i]=pDataConst[i]-pDataCoef[i*n+k]*pDataConst[k];
        }
    }
    
	d=pDataCoef[(n-1)*n+n-1];
    if (d == 0.0)
	{ 
		delete[] pnJs;
		return FALSE;
	}

    pDataConst[n-1]=pDataConst[n-1]/d;
    for (i=n-2;i>=0;i--)
    { 
		t=0.0;
        for (j=i+1;j<=n-1;j++)
			t=t+pDataCoef[i*n+j]*pDataConst[j];
        pDataConst[i]=pDataConst[i]-t;
    }
    
	pnJs[n-1]=n-1;
    for (k=n-1;k>=0;k--)
    {
		if (pnJs[k]!=k)
        { 
			t=pDataConst[k]; 
			pDataConst[k]=pDataConst[pnJs[k]]; 
			pDataConst[pnJs[k]]=t;
		}
	}

    delete[] pnJs;

    return TRUE;
}

// 全选主元高斯－约当消去法
BOOL CLEquations::GetRootsetGaussJordan(CMatrix& mtxResult)
{ 
	int *pnJs,l,k,i,j,nIs,p,q;
    double d,t;

	mtxResult = m_mtxConst;
	double *pDataCoef = m_mtxCoef.GetData();
	double *pDataConst = mtxResult.GetData();
	int n = GetNumUnknowns();
	int m = m_mtxConst.GetNumColumns();

    pnJs = new int[n];

    l=1;
    for (k=0;k<=n-1;k++)
    { 
		d=0.0;
        for (i=k;i<=n-1;i++)
        {
			for (j=k;j<=n-1;j++)
            { 
				t=fabs(pDataCoef[i*n+j]);
				if (t>d) 
				{ 
					d=t; 
					pnJs[k]=j; 
					nIs=i;
				}
            }
		}

        if (d+1.0==1.0) 
			l=0;
        else
        { 
			if (pnJs[k]!=k)
            {
				for (i=0;i<=n-1;i++)
                { 
					p=i*n+k; 
					q=i*n+pnJs[k];
					t=pDataCoef[p]; 
					pDataCoef[p]=pDataCoef[q]; 
					pDataCoef[q]=t;
                }
			}

            if (nIs!=k)
            { 
				for (j=k;j<=n-1;j++)
                { 
					p=k*n+j; 
					q=nIs*n+j;
                    t=pDataCoef[p]; 
					pDataCoef[p]=pDataCoef[q]; 
					pDataCoef[q]=t;
                }
                
				for (j=0;j<=m-1;j++)
                { 
					p=k*m+j; 
					q=nIs*m+j;
                    t=pDataConst[p]; 
					pDataConst[p]=pDataConst[q]; 
					pDataConst[q]=t;
                }
            }
        }
        
		if (l==0)
        { 
			delete[] pnJs;
            return FALSE;
        }
        
		d=pDataCoef[k*n+k];
        for (j=k+1;j<=n-1;j++)
        { 
			p=k*n+j; 
			pDataCoef[p]=pDataCoef[p]/d;
		}
        
		for (j=0;j<=m-1;j++)
        { 
			p=k*m+j; 
			pDataConst[p]=pDataConst[p]/d;
		}
        
		for (j=k+1;j<=n-1;j++)
        {
			for (i=0;i<=n-1;i++)
            { 
				p=i*n+j;
				if (i!=k)
					pDataCoef[p]=pDataCoef[p]-pDataCoef[i*n+k]*pDataCoef[k*n+j];
            }
		}

        for (j=0;j<=m-1;j++)
        {
			for (i=0;i<=n-1;i++)
			{ 
				p=i*m+j;
				if (i!=k)
				pDataConst[p]=pDataConst[p]-pDataCoef[i*n+k]*pDataConst[k*m+j];
			}
		}
    }
    
	for (k=n-1;k>=0;k--)
    {
		if (pnJs[k]!=k)
		{
			for (j=0;j<=m-1;j++)
			{ 
				p=k*m+j; 
				q=pnJs[k]*m+j;
				t=pDataConst[p]; 
				pDataConst[p]=pDataConst[q]; 
				pDataConst[q]=t;
			}
		}
	}

    delete[] pnJs;
    
	return TRUE;
}

// 复系数方程组的全选主元高斯消去法
BOOL CLEquations::GetRootsetGauss(CMatrix mtxCoefImag, CMatrix mtxConstImag, CMatrix& mtxResult, CMatrix& mtxResultImag)
{ 
	int *pnJs,l,k,i,j,nIs,u,v;
    double p,q,s,d;

	mtxResult = m_mtxConst;
	mtxResultImag = mtxConstImag;
	double *pDataCoef = m_mtxCoef.GetData();
	double *pDataConst = mtxResult.GetData();
	double *pDataCoefImag = mtxCoefImag.GetData();
	double *pDataConstImag = mtxResultImag.GetData();
	int n = GetNumUnknowns();
	int m = m_mtxConst.GetNumColumns();

    pnJs = new int[n];
    
	for (k=0;k<=n-2;k++)
    { 
		d=0.0;
        for (i=k;i<=n-1;i++)
        {
			for (j=k;j<=n-1;j++)
			{ 
				u=i*n+j;
				p=pDataCoef[u]*pDataCoef[u]+pDataCoefImag[u]*pDataCoefImag[u];
				if (p>d) 
				{
					d=p;
					pnJs[k]=j;
					nIs=i;
				}
			}
        }
        
		if (d == 0.0)
		{
			delete[] pnJs;
			return FALSE;
        }
        
		if (nIs!=k)
        { 
			for (j=k;j<=n-1;j++)
            { 
				u=k*n+j; 
				v=nIs*n+j;
                p=pDataCoef[u]; 
				pDataCoef[u]=pDataCoef[v]; 
				pDataCoef[v]=p;
                p=pDataCoefImag[u]; 
				pDataCoefImag[u]=pDataCoefImag[v]; 
				pDataCoefImag[v]=p;
            }
            
			p=pDataConst[k]; 
			pDataConst[k]=pDataConst[nIs]; 
			pDataConst[nIs]=p;
            p=pDataConstImag[k]; 
			pDataConstImag[k]=pDataConstImag[nIs]; 
			pDataConstImag[nIs]=p;
        }
        
		if (pnJs[k]!=k)
        {
			for (i=0;i<=n-1;i++)
            { 
				u=i*n+k; 
				v=i*n+pnJs[k];
				p=pDataCoef[u]; 
				pDataCoef[u]=pDataCoef[v]; 
				pDataCoef[v]=p;
				p=pDataCoefImag[u]; 
				pDataCoefImag[u]=pDataCoefImag[v]; 
				pDataCoefImag[v]=p;
            }
		}

        v=k*n+k;
        for (j=k+1;j<=n-1;j++)
        { 
			u=k*n+j;
            p=pDataCoef[u]*pDataCoef[v]; 
			q=-pDataCoefImag[u]*pDataCoefImag[v];
            s=(pDataCoef[v]-pDataCoefImag[v])*(pDataCoef[u]+pDataCoefImag[u]);
            pDataCoef[u]=(p-q)/d; 
			pDataCoefImag[u]=(s-p-q)/d;
        }
        
		p=pDataConst[k]*pDataCoef[v]; 
		q=-pDataConstImag[k]*pDataCoefImag[v];
        s=(pDataCoef[v]-pDataCoefImag[v])*(pDataConst[k]+pDataConstImag[k]);
        pDataConst[k]=(p-q)/d; 
		pDataConstImag[k]=(s-p-q)/d;

        for (i=k+1;i<=n-1;i++)
        { 
			u=i*n+k;
            for (j=k+1;j<=n-1;j++)
            { 
				v=k*n+j; 
				l=i*n+j;
                p=pDataCoef[u]*pDataCoef[v]; 
				q=pDataCoefImag[u]*pDataCoefImag[v];
                s=(pDataCoef[u]+pDataCoefImag[u])*(pDataCoef[v]+pDataCoefImag[v]);
                pDataCoef[l]=pDataCoef[l]-p+q;
                pDataCoefImag[l]=pDataCoefImag[l]-s+p+q;
            }
            
			p=pDataCoef[u]*pDataConst[k]; 
			q=pDataCoefImag[u]*pDataConstImag[k];
            s=(pDataCoef[u]+pDataCoefImag[u])*(pDataConst[k]+pDataConstImag[k]);
            pDataConst[i]=pDataConst[i]-p+q; 
			pDataConstImag[i]=pDataConstImag[i]-s+p+q;
        }
    }
    
	u=(n-1)*n+n-1;
    d=pDataCoef[u]*pDataCoef[u]+pDataCoefImag[u]*pDataCoefImag[u];

    if (d == 0.0)
	{
		delete[] pnJs;
		return FALSE;
    }

    p=pDataCoef[u]*pDataConst[n-1]; q=-pDataCoefImag[u]*pDataConstImag[n-1];
    s=(pDataCoef[u]-pDataCoefImag[u])*(pDataConst[n-1]+pDataConstImag[n-1]);
    pDataConst[n-1]=(p-q)/d; pDataConstImag[n-1]=(s-p-q)/d;

    for (i=n-2;i>=0;i--)
    {
		for (j=i+1;j<=n-1;j++)
		{ 
			u=i*n+j;
			p=pDataCoef[u]*pDataConst[j]; 
			q=pDataCoefImag[u]*pDataConstImag[j];
			s=(pDataCoef[u]+pDataCoefImag[u])*(pDataConst[j]+pDataConstImag[j]);
			pDataConst[i]=pDataConst[i]-p+q;
			pDataConstImag[i]=pDataConstImag[i]-s+p+q;
		}
    }

    pnJs[n-1]=n-1;
    for (k=n-1;k>=0;k--)
    {
		if (pnJs[k]!=k)
        { 
			p=pDataConst[k]; 
			pDataConst[k]=pDataConst[pnJs[k]]; 
			pDataConst[pnJs[k]]=p;
			p=pDataConstImag[k]; 
			pDataConstImag[k]=pDataConstImag[pnJs[k]]; 
			pDataConstImag[pnJs[k]]=p;
        }
	}

	delete[] pnJs;

	return TRUE;
}

// 复系数方程组的全选主元高斯－约当消去法
BOOL CLEquations::GetRootsetGaussJordan(CMatrix mtxCoefImag, CMatrix mtxConstImag, CMatrix& mtxResult, CMatrix& mtxResultImag)
{
	int *pnJs,l,k,i,j,nIs,u,v;
    double p,q,s,d;

	mtxResult = m_mtxConst;
	mtxResultImag = mtxConstImag;
	double *pDataCoef = m_mtxCoef.GetData();
	double *pDataConst = mtxResult.GetData();
	double *pDataCoefImag = mtxCoefImag.GetData();
	double *pDataConstImag = mtxResultImag.GetData();
	int n = GetNumUnknowns();
	int m = m_mtxConst.GetNumColumns();

    pnJs = new int[n];

    for (k=0;k<=n-1;k++)
    { 
		d=0.0;
        for (i=k;i<=n-1;i++)
        {
			for (j=k;j<=n-1;j++)
			{ 
				u=i*n+j;
				p=pDataCoef[u]*pDataCoef[u]+pDataCoefImag[u]*pDataCoefImag[u];
				if (p>d) 
				{
					d=p;
					pnJs[k]=j;
					nIs=i;
				}
			}
        }
        
		if (d == 0.0)
        {
			delete[] pnJs;
            return FALSE;
        }
        
		if (nIs!=k)
        { 
			for (j=k;j<=n-1;j++)
            { 
				u=k*n+j; 
				v=nIs*n+j;
                p=pDataCoef[u]; 
				pDataCoef[u]=pDataCoef[v]; 
				pDataCoef[v]=p;
                p=pDataCoefImag[u]; 
				pDataCoefImag[u]=pDataCoefImag[v]; 
				pDataCoefImag[v]=p;
            }
            
			for (j=0;j<=m-1;j++)
            { 
				u=k*m+j; 
				v=nIs*m+j;
                p=pDataConst[u]; 
				pDataConst[u]=pDataConst[v]; 
				pDataConst[v]=p;
                p=pDataConstImag[u]; 
				pDataConstImag[u]=pDataConstImag[v]; 
				pDataConstImag[v]=p;
            }
        }
        
		if (pnJs[k]!=k)
        {
			for (i=0;i<=n-1;i++)
            { 
				u=i*n+k; 
				v=i*n+pnJs[k];
				p=pDataCoef[u]; 
				pDataCoef[u]=pDataCoef[v]; 
				pDataCoef[v]=p;
				p=pDataCoefImag[u]; 
				pDataCoefImag[u]=pDataCoefImag[v]; 
				pDataCoefImag[v]=p;
            }
		}

        v=k*n+k;
        for (j=k+1;j<=n-1;j++)
        { 
			u=k*n+j;
            p=pDataCoef[u]*pDataCoef[v]; 
			q=-pDataCoefImag[u]*pDataCoefImag[v];
            s=(pDataCoef[v]-pDataCoefImag[v])*(pDataCoef[u]+pDataCoefImag[u]);
            pDataCoef[u]=(p-q)/d; 
			pDataCoefImag[u]=(s-p-q)/d;
        }
        
		for (j=0;j<=m-1;j++)
        { 
			u=k*m+j;
            p=pDataConst[u]*pDataCoef[v]; 
			q=-pDataConstImag[u]*pDataCoefImag[v];
            s=(pDataCoef[v]-pDataCoefImag[v])*(pDataConst[u]+pDataConstImag[u]);
            pDataConst[u]=(p-q)/d; 
			pDataConstImag[u]=(s-p-q)/d;
        }
        
		for (i=0;i<=n-1;i++)
        {
			if (i!=k)
            { 
				u=i*n+k;
				for (j=k+1;j<=n-1;j++)
                { 
					v=k*n+j; 
					l=i*n+j;
					p=pDataCoef[u]*pDataCoef[v]; 
					q=pDataCoefImag[u]*pDataCoefImag[v];
					s=(pDataCoef[u]+pDataCoefImag[u])*(pDataCoef[v]+pDataCoefImag[v]);
					pDataCoef[l]=pDataCoef[l]-p+q;
					pDataCoefImag[l]=pDataCoefImag[l]-s+p+q;
                }
            
				for (j=0;j<=m-1;j++)
				{ 
					l=i*m+j; 
					v=k*m+j;
					p=pDataCoef[u]*pDataConst[v]; q=pDataCoefImag[u]*pDataConstImag[v];
					s=(pDataCoef[u]+pDataCoefImag[u])*(pDataConst[v]+pDataConstImag[v]);
					pDataConst[l]=pDataConst[l]-p+q; 
					pDataConstImag[l]=pDataConstImag[l]-s+p+q;
				}
            }
		}
    }

	for (k=n-1;k>=0;k--)
    {
		if (pnJs[k]!=k)
		{
			for (j=0;j<=m-1;j++)
			{ 
				u=k*m+j;
				v=pnJs[k]*m+j;
				p=pDataConst[u]; 
				pDataConst[u]=pDataConst[v]; 
				pDataConst[v]=p;
				p=pDataConstImag[u]; 
				pDataConstImag[u]=pDataConstImag[v]; 
				pDataConstImag[v]=p;
			}
		}
	}

	delete[] pnJs;

	return TRUE;
}

// 求解三对角线方程组的追赶法
BOOL CLEquations::GetRootsetTriDiagonal(CMatrix& mtxResult)
{ 
	int k,j;
    double s;
    
	mtxResult = m_mtxConst;
	double *pDataConst = mtxResult.GetData();

	int n = GetNumUnknowns();
	ASSERT(m_mtxCoef.GetNumRows() == n);

	double* pDiagData = new double[3*n-2];

	k = j = 0;
	if (k == 0)
	{
		pDiagData[j++] = m_mtxCoef.GetElement(k, k);
		pDiagData[j++] = m_mtxCoef.GetElement(k, k+1);
	}
    for (k=1; k<n-1; ++k)
	{
		pDiagData[j++] = m_mtxCoef.GetElement(k, k-1);
		pDiagData[j++] = m_mtxCoef.GetElement(k, k);
		pDiagData[j++] = m_mtxCoef.GetElement(k, k+1);
	}
	if (k == n-1)
	{
		pDiagData[j++] = m_mtxCoef.GetElement(k, k-1);
		pDiagData[j++] = m_mtxCoef.GetElement(k, k);
	}

	for (k=0;k<=n-2;k++)
    { 
		j=3*k; 
		s=pDiagData[j];

        if (fabs(s)+1.0==1.0)
		{
			delete[] pDiagData;
			return FALSE;
		}

        pDiagData[j+1]=pDiagData[j+1]/s;
        pDataConst[k]=pDataConst[k]/s;
        pDiagData[j+3]=pDiagData[j+3]-pDiagData[j+2]*pDiagData[j+1];
        pDataConst[k+1]=pDataConst[k+1]-pDiagData[j+2]*pDataConst[k];
    }
    
	s=pDiagData[3*n-3];
    if (s == 0.0)
	{
		delete[] pDiagData;
		return FALSE;
	}
    
	pDataConst[n-1]=pDataConst[n-1]/s;
    for (k=n-2;k>=0;k--)
		pDataConst[k]=pDataConst[k]-pDiagData[3*k+1]*pDataConst[k+1];
    
	delete[] pDiagData;

	return TRUE;
}

// 一般带型方程组的求解
BOOL CLEquations::GetRootsetBand(int nBandWidth, CMatrix& mtxResult)
{ 
	int ls,k,i,j,is,u,v;
    double p,t;
    
	if ((nBandWidth-1)%2 != 0)
		return FALSE;

	mtxResult = m_mtxConst;
	double *pDataConst = mtxResult.GetData();

	int m = m_mtxConst.GetNumColumns();
	int n = GetNumUnknowns();
	ASSERT(m_mtxCoef.GetNumRows() == n);

	double* pBandData = new double[nBandWidth*n];
	// 半带宽
    ls = (nBandWidth-1)/2;

	// 构造带宽数组
    for (i=0; i<n; ++i)
	{
		j = 0;
		for (k=max(0, i-ls); k<max(0, i-ls)+nBandWidth; ++k)
		{
			if (k < n)
				pBandData[i*nBandWidth+j++] = m_mtxCoef.GetElement(i, k);
			else
				pBandData[i*nBandWidth+j++] = 0;
		}
	}

    for (k=0;k<=n-2;k++)
    { 
		p=0.0;
        for (i=k;i<=ls;i++)
        { 
			t=fabs(pBandData[i*nBandWidth]);
            if (t>p) 
			{
				p=t; 
				is=i;
			}
        }
        
		if (p == 0.0)
		{
			delete[] pBandData;
			return FALSE;
		}

        for (j=0;j<=m-1;j++)
        { 
			u=k*m+j; 
			v=is*m+j;