/*!
* \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);
}
/*!
* \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);
}
/*!
* \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);
}
/*!
* \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);
}
/*!
* \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);
}
/*!
* \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);
}