/*!
* \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);
}
WlzBasisFnTransform *WlzConfPolyFromCPts(
WlzFnType type,
int order,
int nDPts,
WlzDVertex2 *dPts,
int nSPts,
WlzDVertex2 *sPts,
WlzErrorNum *dstErr)
{
int idM,
idN,
idX,
nCoef=1;
double thresh,
wMax;
double *bMx = NULL,
*wMx = NULL;
double **aA, **vA;
AlgMatrix aMx, vMx;
WlzDVertex2 powVx,
sVx;
WlzBasisFnTransform *basis = NULL;
WlzBasisFn *basisFn=NULL;
WlzErrorNum errNum = WLZ_ERR_NONE;
int nPts = nDPts;
const double tol = 1.0e-06;
/* initialise */
aMx.core = NULL;
vMx.core = NULL;
/* allocate space */
if((order < 0) || (nPts <= 0))
{
errNum = WLZ_ERR_PARAM_DATA;
}
else if((basis = (WlzBasisFnTransform *)
AlcCalloc(sizeof(WlzBasisFnTransform), 1)) == NULL)
{
errNum = WLZ_ERR_MEM_ALLOC;
}
else
{
if((basisFn = (WlzBasisFn *)
AlcCalloc(sizeof(WlzBasisFn), 1)) == NULL){
AlcFree(basis);
basis = NULL;
errNum = WLZ_ERR_MEM_ALLOC;
}
else {
basis->type = WLZ_TRANSFORM_2D_BASISFN;
basis->linkcount = 0;
basis->freeptr = NULL;
basisFn->type = WLZ_FN_BASIS_2DCONF_POLY;
basisFn->nPoly = order;
basisFn->nBasis = 0;
basisFn->nVtx = 0;
basisFn->param = AlcMalloc(sizeof(double));
*((double *) basisFn->param) = 0.0;
}
}
if(errNum == WLZ_ERR_NONE)
{
nCoef = (order + 1);
if(((wMx = (double *)AlcCalloc(sizeof(double), nCoef)) == NULL) ||
((bMx = (double *)AlcMalloc(sizeof(double) * nPts)) == NULL) ||
((aMx.rect = AlgMatrixRectNew(nPts, nCoef, NULL)) == NULL) ||
((vMx.rect = AlgMatrixRectNew(nPts, nCoef, NULL)) == NULL) ||
((basisFn->poly.v = AlcMalloc(sizeof(WlzDVertex2) * nCoef)) == NULL))
{
errNum = WLZ_ERR_MEM_ALLOC;
}
}
if(errNum == WLZ_ERR_NONE)
{
aA = aMx.rect->array;
vA = vMx.rect->array;
/* Fill matrix A. */
for(idM = 0; idM < nPts; ++idM)
{
idN = 0;
powVx.vtX = 1.0;
sVx = *(sPts + idM);
for(idX = 0; idX <= basisFn->nPoly; ++idX)
{
*(*(aA + idM) + idX) = powVx.vtX;
powVx.vtX *= sVx.vtX;
}
}
/* Perform singular value decomposition of matrix A. */
errNum= WlzErrorFromAlg(AlgMatrixSVDecomp(aMx, wMx, vMx));
}
if(errNum == WLZ_ERR_NONE)
{
/* Edit the singular values. */
wMax = 0.0;
for(idN = 0; idN < nCoef; ++idN)
{
if(*(wMx + idN) > wMax)
{
wMax = *(wMx + idN);
}
}
thresh = tol * wMax;
for(idN = 0; idN < nCoef; ++idN)
{
if(*(wMx + idN) < thresh)
{
*(wMx + idN) = 0.0;
}
}
/* Fill matrix b for x coordinate */
for(idM = 0; idM < nPts; ++idM)
{
*(bMx + idM) = (dPts + idM)->vtX;
}
/* Solve for x polynomial coefficients. */
errNum = WlzErrorFromAlg(AlgMatrixSVBackSub(aMx, wMx, vMx,
bMx));
}
if(errNum == WLZ_ERR_NONE)
{
/* Copy out the x polynomial coefficients, fill matrix b for
y coordinate and re-solve. */
for(idN = 0; idN < nCoef; ++idN)
{
(basisFn->poly.d2 + idN)->vtX = *(bMx + idN);
}
for(idM = 0; idM < nPts; ++idM)
{
*(bMx + idM) = (dPts + idM)->vtY;
}
errNum = WlzErrorFromAlg(AlgMatrixSVBackSub(aMx, wMx, vMx,
bMx));
}
if(errNum == WLZ_ERR_NONE)
{
/* Copy out the ypolynomial coefficients. */
for(idN = 0; idN < nCoef; ++idN)
{
(basisFn->poly.d2 + idN)->vtY = *(bMx + idN);
}
}
if(bMx)
{
AlcFree(bMx);
}
if(wMx)
{
AlcFree(wMx);
}
AlgMatrixFree(aMx);
AlgMatrixFree(vMx);
if(errNum != WLZ_ERR_NONE)
{
if(basisFn)
{
if(basisFn->poly.v)
{
AlcFree(basisFn->poly.v);
}
if(basisFn->param){
AlcFree(basisFn->param);
}
AlcFree(basisFn);
}
if( basis ){
AlcFree(basis);
basis = NULL;
}
}
if( dstErr ){
*dstErr = errNum;
}
return basis;
}
