Ejemplo n.º 1
0
/*!
* \return	Error code.
* \ingroup      AlgMatrix
* \brief	Solves the matrix equation A.x = b for x, where A is a
*		matrix with at least as many rows as columns.
*		On return the matrix A is overwritten by the matrix U
*		in the singular value decomposition:
*		  A = U.W.V'
* \param	aMat			Matrix A.
* \param	bVec			Column matrix b, overwritten
*					by matrix x on return.
* \param	tol			Tolerance for singular values,
*					1.0e-06 should be suitable as
*					a default value.
* \param	dstIC			Destination pointer for
*					ill-conditioned flag, which may
*					be NULL, but if not NULL is set
*					to the number of singular values
*					which are smaller than the maximum
*					singular value times the given
*					threshold.
*/
AlgError	AlgMatrixSVSolve(AlgMatrix aMat, double *bVec, double tol,
                                 int *dstIC)
{
  int		cnt0 = 0,
  		cntIC = 0;
  size_t	nM,
  		nN;
  double	thresh,
  		wMax = 0.0;
  double	*tDP0,
  		*wVec = NULL;
  AlgMatrix	vMat;
  AlgError	errCode = ALG_ERR_NONE;

  nM = aMat.core->nR;
  nN = aMat.core->nC;
  vMat.core = NULL;
  if((aMat.core == NULL) || (aMat.core->type != ALG_MATRIX_RECT) ||
     (aMat.core->nR <= 0) || (aMat.core->nR < aMat.core->nC) ||
     (bVec == NULL))
  {
    errCode = ALG_ERR_FUNC;
  }
  else if((wVec = (double *)AlcCalloc(sizeof(double), aMat.rect->nC)) == NULL)
  {
    errCode = ALG_ERR_MALLOC;
  }
  else
  {
    vMat.rect = AlgMatrixRectNew(nM, nN, &errCode);
  }
  if(errCode == ALG_ERR_NONE)
  {
    errCode = AlgMatrixSVDecomp(aMat, wVec, vMat);
  }
  if(errCode == ALG_ERR_NONE)
  {
    /* Find maximum singular value. */
    cnt0 = nN;
    cntIC = 0;
    tDP0 = wVec;
    while(cnt0-- > 0)
    {
      if(*tDP0 > wMax)
      {
	++cntIC;
        wMax = *tDP0;
      }
      ++tDP0;
    }
    /* Edit the singular values, replacing any less than tol * max singular
       value with 0.0. */
    cnt0 = nN;
    tDP0 = wVec;
    thresh = tol * wMax;
    while(cnt0-- > 0)
    {
      if(*tDP0 < thresh)
      {
	*tDP0 = 0.0;
      }
      ++tDP0;
    }
    errCode = AlgMatrixSVBackSub(aMat, wVec, vMat, bVec);
  }
  AlcFree(wVec);
  AlgMatrixRectFree(vMat.rect);
  if(dstIC)
  {
    *dstIC = cntIC;
  }
  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
	  ("AlgMatrixSVSolve FX %d\n",
	   (int )errCode));
  return(errCode);
}
Ejemplo n.º 2
0
/*!
* \return	Error code.
* \ingroup      AlgMatrix
* \brief	Solves the set of of linear equations A.x = b where
*		A is input as its singular value decomposition in
*		the three matricies U, W and V, as returned by
*		AlgMatrixSVDecomp().
*		The code for AlgMatrixSVBackSub was derived from:
*		Numerical Recipies function svbksb().
* \param	uMat			Given matrix U with nM rows and nN
* 					columns..
* \param	wVec			The diagonal matrix of singular
*					values, returned as a vector with
*					nN elements.
* \param	vMat			The matrix V (not it's
*					transpose) with nN columns.
* \param	bVec			Column matrix b with nM elements,
* 					overwritten by column matrix x on
* 					return.
*/
AlgError	AlgMatrixSVBackSub(AlgMatrix uMat, double *wVec, AlgMatrix vMat,
				   double *bVec)
{
  int		cnt0,
  		idJ;
  double	s;
  double	*tDP0,
		*tDP1,
  		*tDVec = NULL;
  double	**tDPP0;
  AlgError	errCode = ALG_ERR_NONE;

  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1), ("AlgMatrixSVBackSub FE\n"));
  if((uMat.core == NULL) || (uMat.core->type != ALG_MATRIX_RECT) ||
     (vMat.core == NULL) || (vMat.core->type != vMat.core->type) ||
     (uMat.core->nR <= 0) || (uMat.core->nR < uMat.core->nC) ||
     (uMat.core->nR != vMat.core->nR) || (uMat.core->nC != vMat.core->nC) ||
     (wVec == NULL) || (bVec == NULL))
  {
    errCode = ALG_ERR_FUNC;
  }
  else if((tDVec = (double *)AlcCalloc(sizeof(double), uMat.rect->nC)) == NULL)
  {
    errCode = ALG_ERR_MALLOC;
  }
  else
  {
#ifndef ALG_FAST_CODE
    int	   	idI;
#endif
    int 	nM,
    	   	nN;
    double 	**uAry,
    	   	**vAry;

    nM = uMat.rect->nR;
    nN = uMat.rect->nC;
    uAry = uMat.rect->array;
    vAry = vMat.rect->array;
    for(idJ = 0; idJ < nN; ++idJ) 
    {
      s = 0.0;
      if(fabs(wVec[idJ]) > DBL_EPSILON)
      {
#ifdef ALG_FAST_CODE
	cnt0 = nM;
	tDPP0 = uAry;
	tDP0 = bVec;
	while(cnt0-- > 0)
	{
	  s += *(*tDPP0++ + idJ) * *tDP0++;
	}
#else
	for(idI = 0; idI < nM; ++idI)
	{
	  s += uAry[idI][idJ] * bVec[idI];
        }
#endif
	s /= wVec[idJ];
      }
      tDVec[idJ] = s;
    }
    for(idJ = 0; idJ < nN; ++idJ) 
    {
      s = 0.0;
#ifdef ALG_FAST_CODE
      cnt0 = nN;
      tDP0 = *(vAry + idJ);
      tDP1 = tDVec;
      while(cnt0-- > 0)
      {
        s += *tDP0++ * *tDP1++;
      }
#else
      for(idI = 0; idI < nN; ++idI)
      {
	s += vAry[idJ][idI] * tDVec[idI];
      }
#endif
      bVec[idJ] = s;
    }
    AlcFree(tDVec);
  }
  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
	  ("AlgMatrixSVBackSub FX %d\n",
	   (int )errCode));
  return(errCode);
}
Ejemplo n.º 3
0
/*!
* \return	Error code.
* \ingroup      AlgMatrix
* \brief	Performs singular value decomposition of the given
*		matrix (A) and computes two additional matricies
*		(U and V) such that:
*		  A = U.W.V'
*		where V' is the transpose of V.
*		The code for AlgMatrixSVDecomp was derived from:
*		Numerical Recipies function svdcmp(), EISPACK
*		subroutine SVD(), CERN subroutine SVD() and ACM
*		algorithm 358.
*		See AlgMatrixSVSolve() for a usage example.
* \param	aMat			The given matrix A, and U on
*					return.
* \param	wVec			The diagonal matrix of singular
*					values, returned as a vector.
* \param	vMat			The matrix V (not it's
*					transpose).
*/
AlgError	AlgMatrixSVDecomp(AlgMatrix aMat, double *wVec, AlgMatrix vMat)
{
  int		cnt0,
  		flag,
  		idI,
		idJ,
		idK,
		idL = 0,
		its,
		nNL,
		nMI,
		nNM;
  double 	tD0,
  		c,
 		f,
		h,
		s,
		x,
		y,
		z,
		aNorm = 0.0,
		g = 0.0,
		scale = 0.0;
  double	*tDP0,
		*tDP1,
  		*tDVec = NULL;
  double	**tDPP0;
  const int	maxIts = 100;  /* Maximum iterations to find singular value. */
  const double	aScale = 0.01;  /* Used with aNorm to test for small values. */
  AlgError	errCode = ALG_ERR_NONE;

  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
	  ("AlgMatrixSVDecomp FE\n"));
  if((aMat.core == NULL) || (aMat.core->type != ALG_MATRIX_RECT) ||
     (vMat.core == NULL) || (aMat.core->type != vMat.core->type) ||
     (aMat.core->nR <= 0) || (aMat.core->nR < aMat.core->nC) ||
     (aMat.core->nR != vMat.core->nR) || (aMat.core->nC != vMat.core->nC) ||
     (wVec == NULL))
  {
    errCode = ALG_ERR_FUNC;
  }
  else if((tDVec = (double *)AlcCalloc(sizeof(double), aMat.rect->nC)) == NULL)
  {
    errCode = ALG_ERR_MALLOC;
  }
  else
  {
    int	   nM,
    	   nN;
    double **aAry,
    	   **vAry;

    nM = aMat.rect->nR;
    nN = aMat.rect->nC;
    aAry = aMat.rect->array;
    vAry = vMat.rect->array;
    /* Householder reduction to bidiagonal form */
    for(idI = 0; idI < nN; ++idI)
    {
      idL = idI + 1;
      nNL = nN - idL;
      nMI = nM - idI;
      tDVec[idI] = scale * g;
      g = s = scale = 0.0;
      if(idI < nM)
      {
        cnt0 = nMI;
	tDPP0 = aAry + idI;
	while(cnt0-- > 0)	/* for(idK = idI; idK < nM; ++idK) */
	{
	  tD0 = *(*tDPP0++ + idI);
	  scale += fabs(tD0);	/* scale += fabs(aMat[idK][idI]); */
	}
	if(scale > DBL_EPSILON)			   /* scale must always >= 0 */
	{
	  cnt0 = nMI;
	  tDPP0 = aAry + idI;
	  while(cnt0-- > 0)	/* for(idK = idI; idK < nM; ++idK) */
	  {
	    tDP0 = *tDPP0++ + idI;
	    *tDP0 /= scale;	/* aMat[idK][idI] /= scale; */
	    s += *tDP0 * *tDP0;	/* s += aMat[idK][idI] * aMat[idK][idI]; */
	  }
	  f = aAry[idI][idI];
	  g = (f > 0.0)? -(sqrt(s)): sqrt(s);
	  h = (f * g) - s;
	  aAry[idI][idI] = f - g;
	  if(idI != (nN - 1))
	  {
	    for(idJ = idL; idJ < nN; ++idJ)
	    {
	      s = 0.0;
	      cnt0 = nMI;
	      tDPP0 = aAry + idI;
	      while(cnt0-- > 0)	/* for(idK = idI; idK < nM; ++idK) */
	      {
	        s += *(*tDPP0 + idI) * *(*tDPP0 + idJ);
		++tDPP0;	/* s += aMat[idK][idI] * aMat[idK][idJ]; */
	      }
	      f = s / h;
	      cnt0 = nMI;
	      tDPP0 = aAry + idI;
	      while(cnt0-- > 0) /* for(idK = idI; idK < nM; ++idK) */
	      {
	        *(*tDPP0 + idJ) += f * *(*tDPP0 + idI);
		++tDPP0;	/* aMat[idK][idJ] += f * aMat[idK][idI]; */
	      }
	    }
	  }
	  cnt0 = nMI;
	  tDPP0 = aAry + idI;
	  while(cnt0-- > 0)	/* for(idK = idI; idK < nM; ++idK) */
	  {
	    *(*tDPP0++ + idI) *= scale; /* aMat[idK][idI] *= scale; */
	  }
	}
      }
      wVec[idI] = scale * g;
      g = s = scale = 0.0;
      if((idI < nM) && (idI != (nN - 1)))
      {
	cnt0 = nNL;
	tDP0 = *(aAry + idI) + idL;
	while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	{
	  scale += fabs(*tDP0++); /* scale += fabs(aMat[idI][idK]); */
	}
	if(scale > DBL_EPSILON)				/* scale always >= 0 */
	{
	  cnt0 = nNL;
	  tDP0 = *(aAry + idI) + idL;
	  while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	  {
	    *tDP0 /= scale;	/* aMat[idI][idK] /= scale; */
	    tD0 = *tDP0++;
	    s += tD0 * tD0;	/* s += aMat[idI][idK] * aMat[idI][idK]; */
	  }
	  f = aAry[idI][idL];
	  g = (f > 0.0)? -(sqrt(s)): sqrt(s);
	  h = (f * g) - s;
	  aAry[idI][idL] = f - g;
	  cnt0 = nNL;
	  tDP0 = *(aAry + idI) + idL;
	  tDP1 = tDVec + idL;
	  while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	  {
	    *tDP1++ = *tDP0++ / h; /* tDVec[idK] = aMat[idI][idK] / h; */
	  }
	  if(idI != (nM - 1))
	  {
	    for(idJ = idL; idJ < nM; ++idJ)
	    {
	      s = 0.0;
	      cnt0 = nNL;
	      tDP0 = *(aAry + idI) + idL;
	      tDP1 = *(aAry + idJ) + idL;
	      while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	      {
	        s += *tDP1++ * *tDP0++; /* s += aMat[idJ][idK] *
					        aMat[idI][idK]; */
	      }
	      cnt0 = nNL;
	      tDP0 = tDVec + idL; 
	      tDP1 = *(aAry + idJ) + idL;
	      while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	      {
	        *tDP1++ += s * *tDP0++; /* aMat[idJ][idK] += s * tDVec[idK]; */
	      }
	    }
	  }
	  cnt0 = nNL;
	  tDP0 = *(aAry + idI) + idL;
	  while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	  {
	    *tDP0++ *= scale;	/* aMat[idI][idK] *= scale; */
	  }
	}
      }
      if((tD0 = fabs(wVec[idI]) + fabs(tDVec[idI])) > aNorm)
      {
        aNorm = tD0;
      }
    }
    /* Accumulate right-hand transformations. */
    for(idI = nN - 1; idI >= 0; --idI)
    {
      if(idI < (nN - 1))
      {
	nNL = nN - idL;
	if(fabs(g) > DBL_EPSILON)
	{
	  cnt0 = nNL;
	  tDP0 = *(aAry + idI) + idL;
	  tD0 = *tDP0;
	  tDPP0 = vAry + idL;
	  while(cnt0-- > 0)	/* for(idJ = idL; idJ < nN; ++idJ) */
	  {
	    			/* vMat[idJ][idI] = (aMat[idI][idJ] /
				 		     aMat[idI][idL]) /g */
	    *(*tDPP0++ + idI) = (*tDP0++ / tD0) / g; /* Double division to try
	    						and avoid underflow */
	  }
	  for(idJ = idL; idJ < nN; ++idJ)
	  {
	    s = 0.0;
	    cnt0 = nNL;
	    tDP0 = *(aAry + idI) + idL;
	    tDPP0 = vAry + idL;
	    while(cnt0-- > 0)	/* for(idK = idL; idK < nN; ++idK) */
	    {
	      s += *tDP0++ * *(*tDPP0++ + idJ); /* s += aMat[idI][idK] *
	      						vMat[idK][idJ]; */
	    }
	    cnt0 = nNL;
	    tDPP0 = vAry + idL;
	    while(cnt0-- > 0)	
	    {
	      tDP0 = *tDPP0++;
	      *(tDP0 + idJ) += s * *(tDP0 + idI); /* vMat[idK][idJ] += s *
	      						     vMat[idK][idI]; */
	    }
	  }
	}
	cnt0 = nNL;
	tDP0 = *(vAry + idI) + idL;
	tDPP0 = vAry + idL;
	while(cnt0-- > 0)	/* for(idJ = idL; idJ < nN; ++idJ) */
	{
	  *tDP0++ = 0.0;	   /* vMat[idI][idJ] = 0.0 */
	  *(*tDPP0++ + idI) = 0.0; /* vMat[idJ][idI] = 0.0; */
	}
      }
      vAry[idI][idI] = 1.0;
      g = tDVec[idI];
      idL = idI;
    }
    /* Accumulate left-hand transformations. */
    for(idI = nN - 1; idI >= 0; --idI)
    {
      idL = idI + 1;
      nNL = nN - idL;
      nMI = nM - idI;
      g = wVec[idI];
      if(idI < (nN - 1))
      {
	cnt0 = nNL;
	tDP0 = *(aAry + idI) + idL;
	while(cnt0-- > 0)	/* for(idJ = idL; idJ < nN; ++idJ) */
	{
	  *tDP0++ = 0.0;	/* aMat[idI][idJ] = 0.0; */
	}
      }
      if(fabs(g) > DBL_EPSILON)
      {
	g = 1.0 / g;
	if(idI != (nN - 1))
	{
	  for(idJ = idL; idJ < nN; ++idJ)
	  {
	    s = 0.0;
	    cnt0 = nMI - 1;
	    tDPP0 = aAry + idL;
	    while(cnt0-- > 0)	/* for(idK = idL; idK < nM; ++idK) */
	    {
	      tDP0 = *tDPP0++;
	      s +=  *(tDP0 + idI) * *(tDP0 + idJ); /* s += aMat[idK][idI] * 
	      						   aMat[idK][idJ]; */
	    }
	    f = (s / aAry[idI][idI]) * g;
	    cnt0 = nMI;
	    tDPP0 = aAry + idI;
	    while(cnt0-- > 0)   /* for(idK = idI; idK < nM; ++idK) */
	    {
	      tDP0 = *tDPP0++;
	      *(tDP0 + idJ) += f * *(tDP0 + idI);
	    }
	  }
	}
	cnt0 = nMI;
	tDPP0 = aAry + idI;
	while(cnt0-- > 0)   /* for(idJ = idI; idJ < nM; ++idJ) */
	{
	  *(*tDPP0++ + idI) *= g; /* aMat[idJ][idI] *= g; */
	}
      } 
      else
      {
	cnt0 = nMI;
	tDPP0 = aAry + idI;
	while(cnt0-- > 0)	/* for(idJ = idI; idJ < nM; ++idJ) */
	{
	  *(*tDPP0++ + idI) = 0.0; /* aMat[idJ][idI] = 0.0; */
	}
      }
      aAry[idI][idI] += 1.0;
    }
    /* Diagonalize the bidiagonal form. */
    for(idK = nN - 1; (idK >= 0) && (errCode == ALG_ERR_NONE); --idK)
    {
      for(its = 1; its <= maxIts; ++its)
      {
	flag = 1;
	for(idL = idK; idL >= 0; --idL)
	{
	  nNM = idL - 1;
	  if(fabs((tDVec[idL] * aScale) + aNorm) == aNorm)
	  {
	    flag = 0;
	    break;
	  }
	  if((fabs(wVec[nNM] * aScale) + aNorm) == aNorm)
	  {
	    break;
	  }
	}
	if(flag)
	{
	  c = 0.0;
	  s = 1.0;
	  for(idI = idL; idI <= idK; ++idI)
	  {
	    f = s * tDVec[idI];

	    if(fabs(f * aScale) + aNorm != aNorm)
	    {
	      g = wVec[idI];
	      h = AlgMatrixSVPythag(f, g);
	      wVec[idI] = h;
	      h = 1.0 / h;
	      c = g * h;
	      s = (-f * h);
	      cnt0 = nM;
	      tDPP0 = aAry;
	      while(cnt0-- > 0)	/* for(idJ = 0; idJ < nM; ++idJ) */
	      {
	        tDP0 = *tDPP0 + nNM;
		tDP1 = *tDPP0++ + idI;
		y = *tDP0;	/* y = aMat[idJ][nNM]; */
		z = *tDP1;	/* z = aMat[idJ][idI]; */
		*tDP0 = (y * c) + (z * s); /* aMat[idJ][nNM] = (y * c) +
							       (z * s); */
		*tDP1 = (z * c) - (y * s); /* aMat[idJ][idI] = (z * c) -
							       (y * s); */
	      }
	    }
	  }
	}
	/* Test for convergence. */
	z = wVec[idK];
	if(idL == idK)
	{
	  if(z < 0.0)
	  {
	    wVec[idK] = -z;
	    cnt0 = nN;
	    tDPP0 = vAry;
	    while(cnt0-- > 0)	/* for(idJ = 0; idJ < nN; ++idJ) */
	    {
	      tDP0 = *tDPP0++ + idK;
	      *tDP0 = -*tDP0; /* vMat[idJ][idK] = -vMat[idJ][idK]; */
	    }
	  }
	  break;
	}
	if(its >= maxIts)
	{
	  errCode = ALG_ERR_MATRIX_SINGULAR;
	}
	else
	{
	  x = wVec[idL];
	  nNM = idK - 1;
	  y = wVec[nNM];
	  g = tDVec[nNM];
	  h = tDVec[idK];
	  f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
	  g = AlgMatrixSVPythag(f, 1.0);
	  f = (((x - z) * (x + z)) + 
	      (h * ((y / (f + ((f > 0)? g: -g))) - h))) / x;
	  c = s = 1.0;
	  for(idJ = idL; idJ <= nNM; ++idJ)
	  {
	    idI = idJ + 1;
	    g = tDVec[idI];
	    y = wVec[idI];
	    h = s * g;
	    g = c * g;
	    z = AlgMatrixSVPythag(f, h);
	    tDVec[idJ] = z;
	    c = f / z;
	    s = h / z;
	    f = (x * c) + (g * s);
	    g = (g * c) - (x * s);
	    h = y * s;
	    y = y * c;
	    cnt0 = nN;
	    tDPP0 = vAry;
	    while(cnt0-- > 0)	/* for(idM = 0; idM < nN; ++idM) */
	    {
	      tDP0 = *tDPP0 + idJ;
	      tDP1 = *tDPP0++ + idI;
	      x = *tDP0; 	/* x = vMat[idM][idJ]; */
	      z = *tDP1;	/* z = vMat[idM][idI]; */
	      *tDP0 = (x * c) + (z * s); /* vMat[idM][idJ] = (x * c) +
	      						     (z * s); */
	      *tDP1 = (z * c) - (x * s); /* vMat[idM][idI] = (z * c) -
	      						     (x * s); */
	    }
	    z = AlgMatrixSVPythag(f, h);
	    wVec[idJ] = z;
	    if(z > DBL_EPSILON)			       /* Can only be >= 0.0 */
	    {
	      z = 1.0 / z;
	      c = f * z;
	      s = h * z;
	    }
	    f = (c * g) + (s * y);
	    x = (c * y) - (s * g);
	    cnt0 = nM;
	    tDPP0 = aAry;
	    while(cnt0-- > 0)   /* for(idM = 0; idM < nM; ++idM) */
	    {
	      tDP0 = *tDPP0 + idJ;
	      tDP1 = *tDPP0++ + idI;
	      y = *tDP0;	/* y = aMat[idM][idJ]; */
	      z = *tDP1;	/* z = aMat[idM][idI]; */
	      *tDP0 = (y * c) + (z * s); /* aMat[idM][idJ] = (y * c) +
	      						     (z * s); */
	      *tDP1 = (z * c) - (y * s); /* aMat[idM][idI] = (z * c) -
	      						     (y * s); */
	    }
	  }
	  tDVec[idL] = 0.0;
	  tDVec[idK] = f;
	  wVec[idK] = x;
        }
      }
    }
    AlcFree(tDVec);
  }
  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
	  ("AlgMatrixSVDecomp FX %d\n",
	   (int )errCode));
  return(errCode);
}
Ejemplo n.º 4
0
/*!
* \return	Error code.
* \ingroup	AlgConvolve
* \brief	Convolves double 1D kernel and data arrays, cnv = krn * data.
*		The return convolution array must not be aliased to
*		either the kernel or data arrays.
* \param	sizeArrayCnv		Length of return array must be
*					>= max(len(dat),len(krn)).
* \param	arrayCnv		Return convolution array.
* \param	sizeArrayKrn		Length of kernel array, must be
*					odd.
* \param	arrayKrn		Kernel array.
* \param	sizeArrayDat		Length of data array.
* \param	arrayDat		Data array.
* \param	pad			Type of padding.
* \param	padVal			Padding value, only used when
* 					pad == ALG_PAD_VALUE.
*/
AlgError	AlgConvolveD(int sizeArrayCnv, double *arrayCnv,
                         int sizeArrayKrn, double *arrayKrn,
                         int sizeArrayDat, double *arrayDat,
                         AlgPadType pad, double padVal)
{
    int		pCnt,
            kCnt0,
            kCnt1,
            halfArrayKrn;
    double	dat0,
            dat1;
    AlgError	errCode = ALG_ERR_NONE;

    ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
            ("AlgConvolve FE %d 0x%lx %d 0x%lx %d 0x%lx %d\n",
             sizeArrayCnv, (unsigned long )arrayCnv,
             sizeArrayKrn, (unsigned long )arrayKrn,
             sizeArrayDat, (unsigned long )arrayDat,
             (int )pad));
    halfArrayKrn = sizeArrayKrn / 2;
    if((sizeArrayCnv <= 0) || (arrayCnv == NULL) ||
            (sizeArrayKrn <= 0) || ((sizeArrayKrn % 2) != 1) || (arrayKrn == NULL) ||
            (sizeArrayDat <= 0) || (arrayDat == NULL))
    {
        errCode = ALG_ERR_FUNC;
    }
    else
    {
        switch(pad)
        {
        case ALG_PAD_NONE:
            pad = ALG_PAD_ZERO;
            break;
        case ALG_PAD_ZERO:
            break;
        case ALG_PAD_END:
            dat0 = arrayDat[0];
            dat1 = arrayDat[sizeArrayDat - 1];
            break;
        case ALG_PAD_VALUE:
            dat0 = padVal;
            dat1 = padVal;
            break;
        default:
            errCode = ALG_ERR_FUNC;
            break;
        }
    }
    if(errCode == ALG_ERR_NONE)
    {
        /* Pad leading data with zeros or first data value and convolve with the
         * kernel until the whole of the kernel is within the data. */
        int		idp;

        for(idp = 0; idp < halfArrayKrn; ++idp)
        {
            int	idk;
            double	cnv = 0.0;

            pCnt = halfArrayKrn - idp;
            if((pad == ALG_PAD_END) || pad == (ALG_PAD_VALUE))
            {
                for(idk = 0; idk < pCnt; ++idk)
                {
                    cnv += arrayKrn[idk];
                }
                cnv *= dat0;
            }
            kCnt0 = sizeArrayKrn - pCnt;
            for(idk = 0; idk < kCnt0; ++idk)
            {
                cnv += arrayKrn[pCnt + idk] * arrayDat[idk];
            }
            arrayCnv[idp] = cnv;
        }
        /* Between leading and trailing padding regions just convolue the data
         * with the kernel. */
        pCnt = sizeArrayDat - sizeArrayKrn + 1;
#if defined ALG_FAST_CODE && defined __AVX2__
        {
            int	sizeArrayKrn4;

            sizeArrayKrn4 = sizeArrayKrn - (sizeArrayKrn % 4);
            for(idp = 0; idp < pCnt; ++idp)
            {
                int	idk;
                double	*dP;
                double	*cP;
                __m256d   c;

                c = _mm256_setzero_pd();
                dP = arrayDat + idp;
                for(idk = 0; idk < sizeArrayKrn4; idk += 4)
                {
                    __m256d d,
                            k;

                    d = _mm256_loadu_pd(dP + idk);
                    k = _mm256_loadu_pd(arrayKrn + idk);
                    c = _mm256_add_pd(c, _mm256_mul_pd(d, k));
                }
                cP = (double *)&c;
                cP[0] = cP[0] + cP[1] + cP[2] + cP[3];
                for(idk = sizeArrayKrn4; idk < sizeArrayKrn; ++idk)
                {
                    cP[0] += arrayKrn[idk] * dP[idk];
                }
                arrayCnv[halfArrayKrn + idp] = cP[0];
            }
        }
#else /* !ALG_FAST_CODE */
        for(idp = 0; idp < pCnt; ++idp)
        {
            int	idk;
            double	cnv = 0.0;

            for(idk = 0; idk < sizeArrayKrn; ++idk)
            {
                cnv += arrayKrn[idk] * arrayDat[idp + idk];
            }
            arrayCnv[halfArrayKrn + idp] = cnv;
        }
#endif /* ALG_FAST_CODE */
        /* Pad trailing data with zeros or last data value and convolve with the
         * kernel until the whole of the kernel is outside the data. */
        for(idp = 0; idp < halfArrayKrn; ++idp)
        {
            int	idk,
                idt;
            double	cnv = 0.0;

            kCnt0 = sizeArrayKrn - idp - 1;
            idt = idp + sizeArrayDat - sizeArrayKrn + 1;
            for(idk = 0; idk < kCnt0; ++idk)
            {
                cnv += arrayKrn[idk] * arrayDat[idt + idk];
            }
            if((pad == ALG_PAD_END) || pad == (ALG_PAD_VALUE))
            {
                double	cnv1 = 0.0;
                kCnt1 = sizeArrayKrn - kCnt0;
                for(idk = 0; idk < kCnt1; ++idk)
                {
                    cnv1 += arrayKrn[kCnt0 + idk];
                }
                cnv += cnv1 * dat1;
            }
            arrayCnv[sizeArrayDat - halfArrayKrn + idp] = cnv;
        }
    }
    ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
            ("AlgConvolve FX %d\n",
             (int )errCode));
    return(errCode);
}
Ejemplo n.º 5
0
/*!
* \return	Error code.
* \ingroup	AlgConvolve
* \brief	Convolves float 1D kernel and data arrays, cnv = krn * data.
*		The return convolution array must not be aliased to
*		either the kernel or data arrays.
* \param	sizeArrayCnv		Length of return array must be
*					>= max(len(dat),len(krn)).
* \param	arrayCnv		Return convolution array.
* \param	sizeArrayKrn		Length of kernel array, must be
*					odd.
* \param	arrayKrn		Kernel array.
* \param	sizeArrayDat		Length of data array.
* \param	arrayDat		Data array.
* \param	pad			Type of padding.
* \param	padVal			Padding value, only used when
* 					pad == ALG_PAD_VALUE.
*/
AlgError	AlgConvolveF(int sizeArrayCnv, float *arrayCnv,
                         int sizeArrayKrn, float *arrayKrn,
                         int sizeArrayDat, float *arrayDat,
                         AlgPadType pad, float padVal)
{
    int		pCnt,
            kCnt0,
            kCnt1,
            halfArrayKrn;
    float		dat0,
                dat1;
    AlgError	errCode = ALG_ERR_NONE;

    ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
            ("AlgConvolve FE %d 0x%lx %d 0x%lx %d 0x%lx %d\n",
             sizeArrayCnv, (unsigned long )arrayCnv,
             sizeArrayKrn, (unsigned long )arrayKrn,
             sizeArrayDat, (unsigned long )arrayDat,
             (int )pad));
    halfArrayKrn = sizeArrayKrn / 2;
    if((sizeArrayCnv <= 0) || (arrayCnv == NULL) ||
            (sizeArrayKrn <= 0) || ((sizeArrayKrn % 2) != 1) || (arrayKrn == NULL) ||
            (sizeArrayDat <= 0) || (arrayDat == NULL))
    {
        errCode = ALG_ERR_FUNC;
    }
    else
    {
        switch(pad)
        {
        case ALG_PAD_NONE:
            pad = ALG_PAD_ZERO;
            break;
        case ALG_PAD_ZERO:
            break;
        case ALG_PAD_END:
            dat0 = arrayDat[0];
            dat1 = arrayDat[sizeArrayDat - 1];
            break;
        case ALG_PAD_VALUE:
            dat0 = padVal;
            dat1 = padVal;
            break;
        default:
            errCode = ALG_ERR_FUNC;
            break;
        }
    }
    if(errCode == ALG_ERR_NONE)
    {
        /* Pad leading data with zeros or first data value and convolve with the
         * kernel until the whole of the kernel is within the data. */
        int		idp;

        for(idp = 0; idp < halfArrayKrn; ++idp)
        {
            int	idk;
            float	cnv = 0.0;

            pCnt = halfArrayKrn - idp;
            if((pad == ALG_PAD_END) || (pad == ALG_PAD_END))
            {
                for(idk = 0; idk < pCnt; ++idk)
                {
                    cnv += arrayKrn[idk];
                }
                cnv *= dat0;
            }
            kCnt0 = sizeArrayKrn - pCnt;
            for(idk = 0; idk < kCnt0; ++idk)
            {
                cnv += arrayKrn[pCnt + idk] * arrayDat[idk];
            }
            arrayCnv[idp] = cnv;
        }
        /* Between leading and trailing padding regions just convolue the data
         * with the kernel. */
        pCnt = sizeArrayDat - sizeArrayKrn + 1;
        for(idp = 0; idp < pCnt; ++idp)
        {
            int	idk;
            float	cnv = 0.0;

            for(idk = 0; idk < sizeArrayKrn; ++idk)
            {
                cnv += arrayKrn[idk] * arrayDat[idp + idk];
            }
            arrayCnv[halfArrayKrn + idp] = cnv;
        }
        /* Pad trailing data with zeros or last data value and convolve with the
         * kernel until the whole of the kernel is outside the data. */
        for(idp = 0; idp < halfArrayKrn; ++idp)
        {
            int	idk,
                idt;
            float	cnv = 0.0;

            kCnt0 = sizeArrayKrn - idp - 1;
            idt = idp + sizeArrayDat - sizeArrayKrn + 1;
            for(idk = 0; idk < kCnt0; ++idk)
            {
                cnv += arrayKrn[idk] * arrayDat[idt + idk];
            }
            if((pad == ALG_PAD_END) || (pad == ALG_PAD_END))
            {
                float	cnv1 = 0.0;
                kCnt1 = sizeArrayKrn - kCnt0;
                for(idk = 0; idk < kCnt1; ++idk)
                {
                    cnv1 += arrayKrn[kCnt0 + idk];
                }
                cnv += cnv1 * dat1;
            }
            arrayCnv[sizeArrayDat - halfArrayKrn + idp] = cnv;
        }
    }
    ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
            ("AlgConvolve FX %d\n",
             (int )errCode));
    return(errCode);
}
Ejemplo n.º 6
0
/*!
* \return	Error code.
* \ingroup      AlgMatrix
* \brief	Solves the matrix equation A.x = b for x by Gaussian
*		elimination with partial pivoting. Matrix A is the
*		matrix of coefficients.
* \param	abMat			The augmented matrix of size
*					aSz x (aSz + 1), whose columns
*					0 - (aSz - 1) are the
*					corresponding columns of A, and
*					aSz'th column is b. Overwritten
*					on exit.
* \param	aSz	 		Size of matrix A: The number of
*					unknowns.
* \param	xMat			On exit contains the solution
*					matrix x.
*/
AlgError	AlgMatrixGaussSolve(AlgMatrix aMat, double *xMat)
{
  int		idxI,
		idxK,
		count0,
		pivotRow,
		homogeneous = 0;
  double	tD0,
  		maxPivot;
  double	*tDP0,
  		*tDP1;
  double	**tDPP0;
  size_t	aSz;
  double	**abMat;
  AlgError	errCode = ALG_ERR_NONE;

  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
	  ("AlgMatrixGaussSolve FE\n"));
  if((aMat.core == NULL) || (aMat.core->type != ALG_MATRIX_RECT) ||
     (aMat.core->nR < 1) || (aMat.core->nC < 1) ||
     (aMat.core->nR != aMat.core->nC) || (xMat == NULL))
  {
    errCode = ALG_ERR_FUNC;
  }
  else
  {
    aSz = aMat.rect->nR;
    abMat = aMat.rect->array;
    /* Determine homogeneity */
    tDPP0 = abMat;
    count0 = aSz;
    while((count0-- > 0) && !homogeneous)
    {
      tD0 = *(*tDPP0++ + aSz);
      homogeneous = fabs(tD0) <= DBL_EPSILON;
    }
    /* Perform the elimination */
    for(idxK = 0; (idxK < aSz) && (errCode == ALG_ERR_NONE); ++idxK)
    {
      /* Search for maximum modulus pivot */
      pivotRow = idxK;
      tDPP0 = abMat + idxK;
      maxPivot = fabs(*(*tDPP0 + idxK));
      for(idxI = idxK + 1; idxI < aSz; ++idxI)
      {
	if((tD0 = fabs(*(*++tDPP0 + idxK))) > maxPivot)
	{
	  maxPivot = tD0;
	  pivotRow = idxI;
	}
      }
      if(maxPivot <= DBL_EPSILON)
      {
	errCode = ALG_ERR_MATRIX_SINGULAR;
      }
      else
      {
	/* Interchange rows idxK and pivotRow */
	if(pivotRow != idxK)
	{
	  tDP0 = *(abMat + idxK) + idxK;
	  tDP1 = *(abMat + pivotRow) + idxK;
	  count0 = aSz - idxK;
	  while(count0-- >= 0)	/* for(j = k; j < aSz + 1; ++j) */
	  {
	    tD0 = *tDP0;		/* tmp = ab[k][j]; */
	    *tDP0++ = *tDP1;		/* ab[k][j] = ab[pivotRow][j]; */
	    *tDP1++ = tD0;		/* ab[pivotRow][j] = tmp; */
	  }
	}
	/* Eliminate xMat[idxK] from equations k + 1, ..., aSz */
	tDP0 = *(abMat + idxK);
	tD0 = *(tDP0 + idxK);
	tDP0 += aSz;
	count0 = aSz - idxK;
	while(count0-- >= 0) 		/* for(j = aSz; j >= k; --j) */
	{
	  *tDP0-- /= tD0;		/* ab[k][j] /= ab[k][k]; */
	}
	for(idxI = idxK + 1; idxI < aSz; ++idxI)
	{
	  tDP0 = *(abMat + idxI);
	  tDP1 = *(abMat + idxK) + idxK + 1;
	  tD0 = *(tDP0 + idxK);
	  tDP0 += idxK + 1;
	  count0 = aSz - idxK;
	  while(count0-- > 0)		/* for(j = k + 1; j <= aSz; ++j) */
	  {
	    *tDP0++ -= tD0 * *tDP1++;	/* ab[i][j] -= ab[i][k] * ab[k][j]; */
	  }
	}
      }
    }
    if(errCode == ALG_ERR_NONE)
    {
      if(homogeneous)
      {
	count0 = aSz;
	tDP0 = xMat;
	while(count0-- > 0)
	{
	  *tDP0++ = 0.0;
	}
	errCode = ALG_ERR_MATRIX_HOMOGENEOUS;
      }
      else
      {
	/* Perform backward substitution */
	for(idxI = aSz - 1; idxI >= 0; idxI--)
	{
	  tDPP0 = abMat + idxI - 1;
	  tDP0 = *(abMat + idxI);
	  tD0 = *(tDP0 + aSz) / *(tDP0 + idxI);
	  xMat[idxI] = tD0;		/* x[i] = ab[i][aSz] / ab[i][i]; */
	  count0 = idxI - 1;
	  while(count0-- >= 0)	/* for(k = i - 1; k >= 0; --k) */
	  {
	    tDP1 = *tDPP0--;
	    *(tDP1 + aSz) -= *(tDP1 + idxI) *
			     tD0;	/* ab[k][aSz] -= ab[k][i] * x[i]; */
	  }
	}
      }
    }
  }
  ALG_DBG((ALG_DBG_LVL_FN|ALG_DBG_LVL_1),
	  ("AlgMatrixGaussSolve FX %d\n",
	   (int )errCode));
  return(errCode);
}