pinvs.cpp

实用的算法

void pinvs(int ie1, int ie2, int je1, int jsf, int jc1, int k,
		   double c[], int nci, int ncj, int nck, double s[], 
		   int nsi, int nsj)
{
    double zero = 0.0; int one = 1;
    double pscl[11];
	int indxr[11];
    int je2 = je1 + ie2 - ie1;
    int i,j,js1 = je2 + 1;
    for (i = ie1; i<=ie2; i++)
	{
        double big = zero;
        for (j = je1; j<=je2; j++)
		{
            if (fabs(s[(i-1)*nsj+j]) > big)
			{
				big = fabs(s[(i-1)*nsj+j]);
			}
        }
        if (big == zero)
		{
			cout<<"singular matrix, row all 0"<<endl;
		}
        pscl[i] = one / big;
        indxr[i] = 0;
    }
	int id,jp,ipiv,jpiv;
	double pivinv,big;
    for (id = ie1; id<=ie2; id++)
	{
        double piv = zero;
        for (i = ie1; i<=ie2; i++)
		{
            if (indxr[i] == 0)
			{
                big = zero;
                for (j = je1; j<=je2; j++)
				{
                    if (fabs(s[(i-1)*nsj+j]) > big)
					{
                        jp = j;
                        big = fabs(s[(i-1)*nsj+j]);
                    }
                }
                if (big * pscl[i] > piv)
				{
                    ipiv = i;
                    jpiv = jp;
                    piv = big * pscl[i];
                }
            }
        }
        if (s[(ipiv-1)*nsj+jpiv] == zero)
		{
			cout<<"singular matrix";
		}
        indxr[ipiv] = jpiv;
        pivinv = (double)one / s[(ipiv-1)*nsj+jpiv];
        for (j = je1; j<=jsf; j++)
		{
            s[(ipiv-1)*nsj+j] = s[(ipiv-1)*nsj+j] * pivinv;
        }
        s[(ipiv-1)*nsj+jpiv] = (double)one;
        for (i = ie1; i<=ie2; i++)
		{
            if (indxr[i] != jpiv)
			{
                if (s[(i-1)*nsj+jpiv] != zero)
				{
                    double dum = s[(i-1)*nsj+jpiv];
                    for (j = je1; j<=jsf; j++)
					{
                        s[(i-1)*nsj+j] = s[(i-1)*nsj+j] - dum * 
							             s[(ipiv-1)*nsj+j];
                    }
                    s[(i-1)*nsj+jpiv] = zero;
                }
            }
        }
    }
    int jcoff = jc1 - js1;
    int icoff = ie1 - je1;
    for (i = ie1; i<=ie2; i++)
	{
        int irow = indxr[i] + icoff;
        for (j = js1; j<=jsf; j++)
		{
            c[((irow-1)*ncj+j+jcoff-1)*nck+k]=s[(i-1)*nsj+j];
        }
    }
}